 #define DIR_PIN   55  //			19			55	//direction
 #define STEP_PIN  54  //			18			54	//step
 #define CS_PIN    40  //			17			40	//chip select
+#define STEP_PORT   PORTF
+#define STEP_BIT     0
 
 int speed = 10;
 bool running = false;
 float Rsense = 0.11;
 float hold_x = 0.5;
 boolean toggle1 = 0;
+uint8_t stall_value = 9;
+volatile uint32_t step_counter = 0;
+const uint32_t steps_per_mm = 80 * 16; // @ 256 microsteps
+const uint16_t fsteps_per_rotation = 200;
+const uint32_t MHz = 16000000>>8; // Scaled by 256
+const uint16_t acceleration = 2000;
 
 #include <TMC2130Stepper.h>
-TMC2130Stepper myStepper = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN);
-
-ISR(TIMER1_COMPA_vect){//timer1 interrupt 1Hz toggles pin 13 (LED)
-//generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low)
-  if (toggle1){
-    digitalWrite(STEP_PIN, HIGH);
-    toggle1 = 0;
-  }
-  else{
-    digitalWrite(STEP_PIN, LOW);
-    toggle1 = 1;
-  }
+TMC2130Stepper myStepper = TMC2130Stepper(CS_PIN);
+
+ISR(TIMER1_COMPA_vect){
+	STEP_PORT |= 1 << STEP_BIT;
+	STEP_PORT &= ~(1 << STEP_BIT);
+	step_counter++;
 }
 
 void initTimer() {
 	cli();//stop interrupts
-	//set timer1 interrupt at 1Hz
-	TCCR1A = 0;// set entire TCCR1A register to 0
-	TCCR1B = 0;// same for TCCR1B
-	TCNT1  = 0;//initialize counter value to 0
-	// set compare match register for 1hz increments
-	OCR1A = 256;// = (16*10^6) / (1*1024) - 1 (must be <65536)
+	//set timer1 interrupt at 10kHz => 10000 steps/s => ~ 20 fullsteps/s
+	TCCR1A = 0; // set entire TCCR1A register to 0
+	TCCR1B = 0; // same for TCCR1B
+	TCNT1  = 0; //initialize counter value to 0
+	// set compare match register for 2kHz increments
+	OCR1A = 61;// = (16*10^6) / (8*2000) - 1 (must be <65536)
 	// turn on CTC mode
 	TCCR1B |= (1 << WGM12);
-	// Set CS12 and CS10 bits for 1024 prescaler
-	TCCR1B |= (1 << CS11);// | (1 << CS10);  
-	// enable timer compare interrupt
-	TIMSK1 |= (1 << OCIE1A);
+	// Set CS01 bit for no prescaler
+	TCCR1B |= (1 << CS10);
+	// enable/disable timer compare interrupt on start
+	//TIMSK1 |= (1 << OCIE1A);
+	TIMSK1 &= ~(1 << OCIE1A);
 	sei();//allow interrupts
 }
 
-void setTimer(int t) {
+uint16_t calculateMMSTimer(uint8_t mms) {
+	uint32_t steps_per_second = mms * steps_per_mm;
+	steps_per_second >>= 8;
+	uint32_t tmr = MHz / steps_per_second - 1;
+	return tmr;
+}
+uint16_t calculateFSPSTimer(uint16_t fsps) {
+	return MHz / fsps - 1;
+}
+uint16_t calculateRPSTimer(uint8_t rps) {
+	return MHz / (rps * fsteps_per_rotation) - 1;
+}
+
+void accelerationRamp(uint16_t maxMMS = 100) {
+	TIMSK1 &= ~(1 << OCIE1A);
+	while (myStepper.cur_a() != 0) { // Use cur_b if measuring from B coil
+		digitalWrite(STEP_PIN, HIGH);
+		digitalWrite(STEP_PIN, LOW);
+		delay(1);
+	}
+	delay(100);
+	digitalWrite(EN_PIN, HIGH);
+	uint16_t mms = 2;
+	uint16_t maxOCR1A = calculateMMSTimer(maxMMS);
+	uint16_t _OCR1A = calculateMMSTimer(mms);
+	digitalWrite(EN_PIN, LOW);
+	OCR1A = _OCR1A;
+	TIMSK1 |= 1 << OCIE1A;
+	for (; mms <= maxMMS; mms += acceleration) {
+		delay(1);
+		OCR1A = calculateMMSTimer(mms);
+	}
 	cli();
-	TCNT1 = 0;
-	OCR1A = t;
+	step_counter = 0;
 	sei();
+	while (step_counter <= 100*steps_per_mm);
+	for (; mms >= 2; mms -= acceleration) {
+		delay(1);
+		OCR1A = calculateMMSTimer(mms);
+	}
 }
 
 void serialTuple(String cmd, int arg) {

	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);
