 		if (cmd == "run") {
 			serialTuple("run", arg);
 			running = arg;
-			arg ? digitalWrite(EN_PIN, LOW) : digitalWrite(EN_PIN, HIGH);
-		}
-		else if (cmd == "speed") {
-			serialTuple("speed", arg);
-			//speed = arg;
-			setTimer(arg);
+			arg ? TIMSK1 |= 1 << OCIE1A : TIMSK1 &= ~(1 << OCIE1A);
+			//arg ? digitalWrite(EN_PIN, LOW) : digitalWrite(EN_PIN, HIGH);
+		}
+
+		else if (cmd == "mms") {
+			serialTuple("mms", arg);
+			cli();
+			TCNT1 = 0;
+			OCR1A = calculateMMSTimer(arg);
+			Serial.print("Set OCR1A to ");
+			Serial.println(OCR1A);
+			sei();
+		}
+		else if (cmd == "fsps") {
+			serialTuple("fsps", arg);
+			cli();
+			OCR1A = calculateFSPSTimer(arg);
+			Serial.print("Set OCR1A to ");
+			Serial.println(OCR1A);
+			sei();
+		}
+		else if (cmd == "rps") {
+			serialTuple("rps", arg);
+			cli();
+			OCR1A = calculateRPSTimer(arg);
+			Serial.print("Set OCR1A to ");
+			Serial.println(OCR1A);
+			sei();
 		}
 		else if (cmd == "setCurrent") {
 			serialTuple("setCurrent", arg);
 			myStepper.setCurrent(arg, Rsense, hold_x);
+		}
+		else if (cmd == "find_pos") {
+			serialTuple("find_pos", arg);
+			TIMSK1 &= ~(1 << OCIE1A);
+			while (myStepper.cur_a() != arg) { // Use cur_b if measuring from B coil
+				digitalWrite(STEP_PIN, HIGH);
+				digitalWrite(STEP_PIN, LOW);
+				delay(1);
+			}
+		}
+		else if (cmd == "rampTo") {
+			serialTuple("rampTo", arg);
+			accelerationRamp(arg);
 		}
 		else if (cmd == "Rsense") {
 			Serial.print("Setting R sense value to: ");

	Serial.println(")");
}


void setup() {
	initTimer();
	Serial.begin(9600);
	myStepper.begin();
	myStepper.SilentStepStick2130(1000);
	digitalWrite(EN_PIN, LOW);
	Serial.println("Setup ready");
}

void loop() {

			Serial.print("GCONF: 0b");
			Serial.println(myStepper.GCONF(), BIN);
		}
		else if (cmd == "I_scale_analog") {
			serialTuple("I_scale_analog", arg);
			myStepper.I_scale_analog(arg);
		}
		else if (cmd == "internal_Rsense") {
			serialTuple("internal_Rsense", arg);
			myStepper.internal_Rsense(arg);
		}
		else if (cmd == "en_pwm_mode") {
			serialTuple("en_pwm_mode", arg);
			myStepper.en_pwm_mode(arg);
		}
		else if (cmd == "enc_commutation") {
			serialTuple("enc_commutation", arg);
			myStepper.enc_commutation(arg);
		}
		else if (cmd == "shaft") {
			serialTuple("shaft", arg);
			myStepper.shaft(arg);
		}
		else if (cmd == "diag0_errors") {
			serialTuple("diag0_errors", arg);
