 #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) {

			serialTuple("direct_mode", arg);
			myStepper.direct_mode(arg);
		}
		else if (cmd == "hold_current") {
			serialTuple("hold_current", arg);
			myStepper.hold_current(arg);
		}
		else if (cmd == "run_current") {
			serialTuple("run_current", arg);
			myStepper.run_current(arg);
		}
		else if (cmd == "hold_delay") {
			serialTuple("hold_delay", arg);
			myStepper.hold_delay(arg);
		}
		else if (cmd == "power_down_delay") {
			serialTuple("power_down_delay", arg);
			myStepper.power_down_delay(arg);
		}
		else if (cmd == "microstep_time") {
			Serial.print("microstep_time: ");
			Serial.println(myStepper.microstep_time());
		}
		else if (cmd == "stealth_max_speed") {
			serialTuple("stealth_max_speed", arg);
			myStepper.stealth_max_speed(arg);
		}
		else if (cmd == "coolstep_min_speed") {
			serialTuple("coolstep_min_speed", arg);
			myStepper.coolstep_min_speed(arg);
		}
		else if (cmd == "mode_sw_speed") {
			serialTuple("mode_sw_speed", arg);
			myStepper.mode_sw_speed(arg);
		}
		else if (cmd == "coil_A_current") {
			serialTuple("coil_A_current", arg);
			myStepper.coil_A_current(arg);
		}
		else if (cmd == "coil_B_current") {
			serialTuple("coil_B_current", arg);
			myStepper.coil_B_current(arg);
		}
		else if (cmd == "DCstep_min_speed") {
			serialTuple("DCstep_min_speed", arg);
			myStepper.DCstep_min_speed(arg);
		}
		else if (cmd == "CHOPCONF") {
			Serial.print("CHOPCONF: 0b");
			Serial.println(myStepper.CHOPCONF(), BIN);
		}
		else if (cmd == "off_time") {
			serialTuple("off_time", arg);
			myStepper.off_time(arg);
		}
		else if (cmd == "hysteresis_start") {
			serialTuple("hysteresis_start", arg);
			myStepper.hysteresis_start(arg);
		}
		else if (cmd == "fast_decay_time") {
			serialTuple("fast_decay_time", arg);
			myStepper.fast_decay_time(arg);
		}
		else if (cmd == "hysteresis_low") {
			serialTuple("hysteresis_low", arg);
			myStepper.hysteresis_low(arg);
		}
/*		else if (cmd == "sine_offset") {
			serialTuple("sine_offset", arg);
			myStepper.sine_offset(arg);
		}*/
		else if (cmd == "disable_I_comparator") {
			serialTuple("disable_I_comparator", arg);
			myStepper.disable_I_comparator(arg);
		}
		else if (cmd == "random_off_time") {
			serialTuple("random_off_time", arg);
			myStepper.random_off_time(arg);
		}
		else if (cmd == "chopper_mode") {
			serialTuple("chopper_mode", arg);
			myStepper.chopper_mode(arg);
		}
		else if (cmd == "blank_time") {
			serialTuple("blank_time", arg);
			myStepper.blank_time(arg);
		}
		else if (cmd == "high_sense_R") {
			serialTuple("high_sense_R", arg);
			myStepper.high_sense_R(arg);
		}
		else if (cmd == "fullstep_threshold") {
			serialTuple("fullstep_threshold", arg);
			myStepper.fullstep_threshold(arg);
		}
		else if (cmd == "high_speed_mode") {
			serialTuple("high_speed_mode", arg);
			myStepper.high_speed_mode(arg);
		}
		else if (cmd == "sync_phases") {
			serialTuple("sync_phases", arg);
			myStepper.sync_phases(arg);
		}
		else if (cmd == "microsteps") {
			serialTuple("microsteps", arg);
			myStepper.microsteps(arg);
		}
		else if (cmd == "interpolate") {
			serialTuple("interpolate", arg);
			myStepper.interpolate(arg);
		}
		else if (cmd == "double_edge_step") {
			serialTuple("double_edge_step", arg);
			myStepper.double_edge_step(arg);
		}
		else if (cmd == "disable_short_protection") {
			serialTuple("disable_short_protection", arg);
			myStepper.disable_short_protection(arg);
		}
		else if (cmd == "sg_min") {
			serialTuple("sg_min", arg);
			myStepper.sg_min(arg);
		}
		else if (cmd == "sg_max") {
			serialTuple("sg_max", arg);
			myStepper.sg_max(arg);
		}
		else if (cmd == "sg_step_width") {
			serialTuple("sg_step_width", arg);
			myStepper.sg_step_width(arg);
		}
		else if (cmd == "sg_current_decrease") {
			serialTuple("sg_current_decrease", arg);
			myStepper.sg_current_decrease(arg);
		}
		else if (cmd == "smart_min_current") {
			serialTuple("smart_min_current", arg);
			myStepper.smart_min_current(arg);
		}
		else if (cmd == "sg_stall_value") {
			serialTuple("sg_stall_value", arg);
			myStepper.sg_stall_value(arg);
		}
		else if (cmd == "sg_filter") {
			serialTuple("sg_filter", arg);
			myStepper.sg_filter(arg);
		}
		else if (cmd == "stealth_amplitude") {
			serialTuple("stealth_amplitude", arg);
			myStepper.stealth_amplitude(arg);
		}
		else if (cmd == "stealth_gradient") {
			serialTuple("stealth_gradient", arg);
			myStepper.stealth_gradient(arg);
		}
		else if (cmd == "stealth_freq") {
			serialTuple("stealth_freq", arg);
			myStepper.stealth_freq(arg);
		}
		else if (cmd == "stealth_freq") {
			serialTuple("stealth_freq", arg);
			myStepper.stealth_freq(arg);
		}
		else if (cmd == "stealth_autoscale") {
			serialTuple("stealth_autoscale", arg);
			myStepper.stealth_autoscale(arg);
		}
		else if (cmd == "stealth_symmetric") {
			serialTuple("stealth_symmetric", arg);
			myStepper.stealth_symmetric(arg);
		}
		else if (cmd == "standstill_mode") {
			serialTuple("standstill_mode", arg);
			myStepper.standstill_mode(arg);
		}
		else if (cmd == "DRVSTATUS") {
			Serial.print("DRVSTATUS: 0b");
			Serial.println(myStepper.DRV_STATUS(), BIN);
		}
		else if (cmd == "PWM_SCALE") {
			Serial.print("PWM_SCALE: 0b");
			Serial.println(myStepper.PWM_SCALE(), BIN);
		}
		else if (cmd == "invert_encoder") {
			serialTuple("invert_encoder", arg);
			myStepper.invert_encoder(arg);
		}
		else if (cmd == "maxspeed") {
			serialTuple("maxspeed", arg);
			myStepper.maxspeed(arg);
		}
		else if (cmd == "interpolate") {
			serialTuple("interpolate", arg);
			myStepper.interpolate(arg);
		}
		else if (cmd == "LOST_STEPS") {
			Serial.print("LOST_STEPS: 0b");
			Serial.println(myStepper.LOST_STEPS(), BIN);
		}
		else {
			Serial.println("Invalid command!");
		}
	}
/*	
	if (running) {
		digitalWrite(STEP_PIN, HIGH);
		delayMicroseconds(speed);
		digitalWrite(STEP_PIN, LOW);
		delayMicroseconds(speed);
	}
*/
}

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