Diff: stepper_motors.h

Revision:

11:dc410a980771

Parent:

6:ae3e6aefe908

--- a/stepper_motors.h	Sun Apr 16 19:52:52 2017 +0000
+++ b/stepper_motors.h	Wed Apr 26 20:58:31 2017 +0000
@@ -14,13 +14,13 @@
 DigitalOut step_M1(MOTOR1_STEP);
 DigitalOut dir_M1(MOTOR1_DIR);
 DigitalOut enable(MOTOR_ENABLE); //enable for both motors
-int16_t speed_M1;  //Speed of motor 1
+float speed_M1;  //Speed of motor 1
 
 //MOTOR 2
 DigitalOut step_M2(MOTOR2_STEP);
 DigitalOut dir_M2(MOTOR2_DIR);
-int16_t speed_M2;  //Speed of motor 2
-int16_t motor1, motor2;
+float speed_M2;  //Speed of motor 2
+float motor1, motor2;
 
 //Motor Position
 int pos_M1 = 0, pos_M2 = 0;
@@ -31,13 +31,14 @@
 //ISR to step motor 1
 void ISR1(void)
 {
+    if (enable == DISABLE) return;
     //Step Motor
-    step_M1 = 1;
+    step_M1 = 1.0;
     wait_us(1);
-    step_M1 = 0;
+    step_M1 = 0.0;
     
     //Update Motor Postion
-    if(dir_M1)
+    if(!dir_M1)
         pos_M1++;
     else
         pos_M1--;    
@@ -45,26 +46,27 @@
 //ISR to step motor 2
 void ISR2(void)
 {
+    if (enable == DISABLE) return;
     //Step Motor
-    step_M2 = 1;
+    step_M2 = 1.0;
     wait_us(1);
-    step_M2 = 0;
+    step_M2 = 0.0;
     
     //Update Motor Position
-    if(dir_M2)
+    if(!dir_M2)
         pos_M2++;
     else
         pos_M2--;
 }
 
 //Set motor 1 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor1Speed(int16_t speed)
+void setMotor1Speed(float speed)
 {
     long timer_period;
     speed = CAP(speed, MAX_CONTROL_OUTPUT);
 
     //Calculate acceleration from the desired speed
-    int16_t desired_accel = speed_M1 - speed;
+    float desired_accel = speed_M1 - speed;
     if(desired_accel > MAX_ACCEL)
         speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
     else if(desired_accel < -MAX_ACCEL)
@@ -72,10 +74,10 @@
     else
         speed_M1 = speed;
 
-    if(speed_M1 == 0) {
+    if(speed_M1 == 0.0) {
         timer_period = ZERO_SPEED;
         dir_M1 = 0; ////sets motor direction
-    } else if (speed_M1 > 0) {
+    } else if (speed_M1 > 0.0) {
         timer_period = 10000 / speed_M1;
         dir_M1 = 1; //sets motor direction
     } else {
@@ -88,13 +90,13 @@
 }
 
 //Set motor 2 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor2Speed(int16_t speed)
+void setMotor2Speed(float speed)
 {
     long timer_period;
     speed = CAP(speed, MAX_CONTROL_OUTPUT);
 
     //Calculate acceleration from the desired speed
-    int16_t desired_accel = speed_M2 - speed;
+    float desired_accel = speed_M2 - speed;
     if(desired_accel > MAX_ACCEL)
         speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
     else if(desired_accel < -MAX_ACCEL)
@@ -102,10 +104,10 @@
     else
         speed_M2 = speed;
 
-    if(speed_M2 == 0) {
+    if(speed_M2 == 0.0) {
         timer_period = ZERO_SPEED;
         dir_M2 = 0; ////sets motor direction
-    } else if (speed_M2 > 0) {
+    } else if (speed_M2 > 0.0) {
         timer_period = 10000 / speed_M2;
         dir_M2 = 1; //sets motor direction
     } else {