Update locomotion.h

ScaffoldingBotIntegration/ScaffoldingBotIntegration/SoftConsole/SB_MSS_MSS_CM3_0/SB_MSS_MSS_CM3_0_app/locomotion.h

@@ -37,13 +37,22 @@ volatile int16_t* stepper_state = (int16_t*) 0x40050014;
                  // if distance is 14, it equates to roughly 3 cm //
 
 // ---- clockwise and counter clockwise have the perspective from the middle of the bot ---- //
+void wait() {
+	int i = 0;
+	for (; i < 100000 ; ++i) {}
+}
+
 void move_forward(int distance)
 {
     *front_left_steps = distance; //  clockwise
+    wait();
     *front_right_steps = distance; // counter-clockwise
+    wait();
 
-    *back_left_steps = distance; // counter-clockwise
+    *back_left_steps = -distance; // counter-clockwise
+    wait();
     *back_right_steps = distance; // clockwise
+    wait();
 }
 
 void move_back(int distance)
@@ -187,15 +196,19 @@ void voxel(int box_size) // (2 layers)
 
 void init_loco()
 {    //this gives us a speed of roughly 35mm/second (vary per hotend requirements) -- (3.5cm per sec)
-    *wheel_speed = 10000000/32;
-    *z_state = 0x936c;
-    *stepper_state = 0x936c;
+	*wheel_speed = 10000000/32;
+    *z_state = 0x9a65;
+    *stepper_state = 0x9a65;
 }
 
 
 void __attribute__ ((interrupt)) GPIO0_IRQHandler( void )
 {
     Steps_taken += root->box_size;
+    double percent = (double)Steps_taken/Required_steps;
+    percent *= 100;
+    uint8_t tx_buf[5];
+    sprintf(tx_buf, "%i", (uint8_t)percent);
     // int five_percent = .05*Required_steps;
     // if(fmod(Steps_taken, Required_steps) == .five_percent)
     // {
@@ -215,6 +228,4 @@ void __attribute__ ((interrupt)) GPIO0_IRQHandler( void )
     MSS_GPIO_clear_irq( MSS_GPIO_0 ); //clear interrupt and resume
 }
 
-#endif /* LOCOMOTION_H_ */
-
-
+#endif /* LOCOMOTION_H_ */
\ No newline at end of file