 	Serial.println(")");
 }
 
-
 void setup() {
 	initTimer();
-	Serial.begin(9600);
-	myStepper.begin();
-	myStepper.SilentStepStick2130(1000);
+	Serial.begin(500000);
+
+	pinMode(EN_PIN, OUTPUT);
+	pinMode(DIR_PIN, OUTPUT);
+	pinMode(STEP_PIN, OUTPUT);
+	pinMode(CS_PIN, OUTPUT);
+	digitalWrite(EN_PIN, HIGH); //deactivate driver (LOW active)
+	digitalWrite(DIR_PIN, LOW); //LOW or HIGH
+	digitalWrite(STEP_PIN, LOW);
+	digitalWrite(CS_PIN, HIGH);
+
+	myStepper.push();
+    myStepper.tbl(1); //blank_time(24);
+    myStepper.power_down_delay(255);
+	myStepper.toff(4);
+
+	// Effective hysteresis = 0
+	myStepper.hstrt(0);
+	myStepper.hend(2);
+
+	myStepper.en_pwm_mode(true);
+	myStepper.pwm_freq(1);
+	myStepper.pwm_autoscale(true);
+	myStepper.pwm_ampl(180);
+	myStepper.pwm_grad(1);
+
+    myStepper.rms_current(600); // mA
+    myStepper.microsteps(256);
+    myStepper.diag1_stall(1);
+    myStepper.diag1_active_high(1);
 	digitalWrite(EN_PIN, LOW);
-	Serial.println("Setup ready");
+
+	while (myStepper.cur_a() < 240) { // Use cur_b if measuring from B coil
+		digitalWrite(STEP_PIN, HIGH);
+		digitalWrite(STEP_PIN, LOW);
+		delay(3);
+	}
+	Serial.print("cur_a = ");
+	Serial.println(myStepper.cur_a(), DEC);
+	Serial.print("cur_b = ");
+	Serial.println(myStepper.cur_b(), DEC);
 }
 
 void loop() {

			Serial.print("GCONF: 0b");
			Serial.println(myStepper.GCONF(), BIN);
		}
		else if (cmd == "external_ref") {
			serialTuple("external_ref", arg);
			myStepper.external_ref(arg);
		}
		else if (cmd == "internal_sense_R") {
			serialTuple("internal_sense_R", arg);
			myStepper.internal_sense_R(arg);
		}
		else if (cmd == "stealthChop") {
			serialTuple("stealthChop", arg);
			myStepper.stealthChop(arg);
		}
		else if (cmd == "commutation") {
			serialTuple("commutation", arg);
			myStepper.commutation(arg);
		}
		else if (cmd == "shaft_dir") {
			serialTuple("shaft_dir", arg);
			myStepper.shaft_dir(arg);
		}
		else if (cmd == "diag0_errors") {
			serialTuple("diag0_errors", arg);

		if (cmd == "run") {
			serialTuple("run", arg);
			running = 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: ");
