Diff: stepper_motors.h

Revision:

3:2f76ffbc5cef

Child:

4:2512939c10f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper_motors.h	Wed Oct 12 05:04:10 2016 +0000
@@ -0,0 +1,97 @@
+//STEPPER MOTORS
+#define ZERO_SPEED 65535
+#define MAX_ACCEL 7
+#define ENABLE 0
+#define DISABLE 1
+
+Ticker timer_M1, timer_M2; //Timers for speed Control
+
+// MOTOR 1
+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
+
+//MOTOR 2
+DigitalOut step_M2(MOTOR2_STEP);
+DigitalOut dir_M2(MOTOR2_DIR);
+int16_t speed_M2;  //Speed of motor 2
+int16_t motor1, motor2;
+
+// =============================================================================
+// ===                   Interrupt Service Soutine                           ===
+// =============================================================================
+//ISR to step motor 1
+void ISR1(void)
+{
+    step_M1 = 1;
+    wait_us(1);
+    step_M1 = 0;
+}
+//ISR to step motor 2
+void ISR2(void)
+{
+    step_M2 = 1;
+    wait_us(1);
+    step_M2 = 0;
+}
+
+//Set motor 1 speed. Speed [-100, +100]
+void setMotor1Speed(int16_t speed)
+{
+    long timer_period;
+    speed = CAP(speed, MAX_CONTROL_OUTPUT);
+
+    //Calculate acceleration from the desired speed
+    int16_t 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)
+        speed_M1 += MAX_ACCEL; //Change speed of motor by max acceleration
+    else
+        speed_M1 = speed;
+
+    if(speed_M1 == 0) {
+        timer_period = ZERO_SPEED;
+        dir_M1 = 0; ////sets motor direction
+    } else if (speed_M1 > 0) {
+        timer_period = 10000 / speed_M1;
+        dir_M1 = 1; //sets motor direction
+    } else {
+        timer_period = 10000 / -speed_M1;
+        dir_M1 = 0; ////sets motor direction
+    }
+
+    // Update Timer period
+    timer_M1.attach_us(&ISR1, timer_period); //This is what sets motor speed
+}
+
+//Set motor 2 speed. Speed [-100, +100]
+void setMotor2Speed(int16_t speed)
+{
+    long timer_period;
+    speed = CAP(speed, MAX_CONTROL_OUTPUT);
+
+    //Calculate acceleration from the desired speed
+    int16_t 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)
+        speed_M2 += MAX_ACCEL; //Change speed of motor by max acceleration
+    else
+        speed_M2 = speed;
+
+    if(speed_M2 == 0) {
+        timer_period = ZERO_SPEED;
+        dir_M2 = 0; ////sets motor direction
+    } else if (speed_M2 > 0) {
+        timer_period = 10000 / speed_M2;
+        dir_M2 = 1; //sets motor direction
+    } else {
+        timer_period = 10000 / -speed_M2;
+        dir_M2 = 0; ////sets motor direction
+    }
+
+    // Update Timer period
+    timer_M2.attach_us(&ISR2, timer_period); //This is what sets motor speed
+}
\ No newline at end of file