Carter Sharer / Mbed 2 deprecated BroBot_ESE350_Skeleton

BroBot Code for ESE350 Lab6 part 3 (Skeleton)

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_ESE350 by Carter Sharer

stepper_motors.h@11:2553f5798f84, 2017-01-19 (annotated)

Committer:
arvindnr89
Date:
Thu Jan 19 19:18:54 2017 +0000
Revision:
11:2553f5798f84
Parent:
6:62cdb7482b50
Child:
12:16d1a5390022
Working code with bug fixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 3:2f76ffbc5cef 1 //STEPPER MOTORS
csharer 6:62cdb7482b50 2 #define ZERO_SPEED 1000000 //65535
arvindnr89 11:2553f5798f84 3 #define MAX_ACCEL 2
csharer 3:2f76ffbc5cef 4 #define ENABLE 0
csharer 3:2f76ffbc5cef 5 #define DISABLE 1
csharer 3:2f76ffbc5cef 6
csharer 3:2f76ffbc5cef 7 Ticker timer_M1, timer_M2; //Timers for speed Control
csharer 3:2f76ffbc5cef 8
csharer 3:2f76ffbc5cef 9 // MOTOR 1
csharer 3:2f76ffbc5cef 10 DigitalOut step_M1(MOTOR1_STEP);
csharer 3:2f76ffbc5cef 11 DigitalOut dir_M1(MOTOR1_DIR);
csharer 3:2f76ffbc5cef 12 DigitalOut enable(MOTOR_ENABLE); //enable for both motors
csharer 3:2f76ffbc5cef 13 int16_t speed_M1; //Speed of motor 1
csharer 3:2f76ffbc5cef 14
csharer 3:2f76ffbc5cef 15 //MOTOR 2
csharer 3:2f76ffbc5cef 16 DigitalOut step_M2(MOTOR2_STEP);
csharer 3:2f76ffbc5cef 17 DigitalOut dir_M2(MOTOR2_DIR);
csharer 3:2f76ffbc5cef 18 int16_t speed_M2; //Speed of motor 2
csharer 3:2f76ffbc5cef 19 int16_t motor1, motor2;
csharer 3:2f76ffbc5cef 20
csharer 4:2512939c10f0 21 //Motor Position
csharer 4:2512939c10f0 22 int pos_M1 = 0, pos_M2 = 0;
csharer 4:2512939c10f0 23
csharer 3:2f76ffbc5cef 24 // =============================================================================
csharer 3:2f76ffbc5cef 25 // === Interrupt Service Soutine ===
csharer 3:2f76ffbc5cef 26 // =============================================================================
csharer 3:2f76ffbc5cef 27 //ISR to step motor 1
csharer 3:2f76ffbc5cef 28 void ISR1(void)
csharer 3:2f76ffbc5cef 29 {
csharer 4:2512939c10f0 30 //Step Motor
csharer 3:2f76ffbc5cef 31 step_M1 = 1;
csharer 3:2f76ffbc5cef 32 wait_us(1);
csharer 3:2f76ffbc5cef 33 step_M1 = 0;
csharer 4:2512939c10f0 34
csharer 4:2512939c10f0 35 //Update Postion
csharer 4:2512939c10f0 36 if(dir_M1)
csharer 4:2512939c10f0 37 pos_M1++;
csharer 4:2512939c10f0 38 else
csharer 4:2512939c10f0 39 pos_M1--;
csharer 3:2f76ffbc5cef 40 }
csharer 3:2f76ffbc5cef 41 //ISR to step motor 2
csharer 3:2f76ffbc5cef 42 void ISR2(void)
csharer 3:2f76ffbc5cef 43 {
csharer 4:2512939c10f0 44 //Step Motor
csharer 3:2f76ffbc5cef 45 step_M2 = 1;
csharer 3:2f76ffbc5cef 46 wait_us(1);
csharer 3:2f76ffbc5cef 47 step_M2 = 0;
csharer 4:2512939c10f0 48
csharer 4:2512939c10f0 49 //Update Position
csharer 4:2512939c10f0 50 if(dir_M2)
csharer 4:2512939c10f0 51 pos_M2++;
csharer 4:2512939c10f0 52 else
csharer 4:2512939c10f0 53 pos_M2--;
csharer 3:2f76ffbc5cef 54 }
csharer 3:2f76ffbc5cef 55
csharer 3:2f76ffbc5cef 56 //Set motor 1 speed. Speed [-100, +100]
csharer 3:2f76ffbc5cef 57 void setMotor1Speed(int16_t speed)
csharer 3:2f76ffbc5cef 58 {
csharer 3:2f76ffbc5cef 59 long timer_period;
arvindnr89 11:2553f5798f84 60 //If the speed has not changed, do not update the timer!
arvindnr89 11:2553f5798f84 61 if(speed_M1 == speed){
arvindnr89 11:2553f5798f84 62 return;
arvindnr89 11:2553f5798f84 63 }
csharer 3:2f76ffbc5cef 64 speed = CAP(speed, MAX_CONTROL_OUTPUT);
arvindnr89 11:2553f5798f84 65
csharer 3:2f76ffbc5cef 66 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 67 int16_t desired_accel = speed_M1 - speed;
csharer 3:2f76ffbc5cef 68 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 69 speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 70 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 71 speed_M1 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 72 else
csharer 3:2f76ffbc5cef 73 speed_M1 = speed;
csharer 3:2f76ffbc5cef 74
csharer 3:2f76ffbc5cef 75 if(speed_M1 == 0) {
csharer 3:2f76ffbc5cef 76 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 77 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 78 } else if (speed_M1 > 0) {
csharer 3:2f76ffbc5cef 79 timer_period = 10000 / speed_M1;
csharer 3:2f76ffbc5cef 80 dir_M1 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 81 } else {
csharer 3:2f76ffbc5cef 82 timer_period = 10000 / -speed_M1;
csharer 3:2f76ffbc5cef 83 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 84 }
csharer 3:2f76ffbc5cef 85
csharer 3:2f76ffbc5cef 86 // Update Timer period
csharer 3:2f76ffbc5cef 87 timer_M1.attach_us(&ISR1, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 88 }
csharer 3:2f76ffbc5cef 89
csharer 3:2f76ffbc5cef 90 //Set motor 2 speed. Speed [-100, +100]
csharer 3:2f76ffbc5cef 91 void setMotor2Speed(int16_t speed)
csharer 3:2f76ffbc5cef 92 {
csharer 3:2f76ffbc5cef 93 long timer_period;
arvindnr89 11:2553f5798f84 94 //If the speed has not changed, do not update the timer!
arvindnr89 11:2553f5798f84 95 if(speed_M2 == speed){
arvindnr89 11:2553f5798f84 96 return;
arvindnr89 11:2553f5798f84 97 }
csharer 3:2f76ffbc5cef 98 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 99
csharer 3:2f76ffbc5cef 100 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 101 int16_t desired_accel = speed_M2 - speed;
csharer 3:2f76ffbc5cef 102 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 103 speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 104 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 105 speed_M2 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 106 else
csharer 3:2f76ffbc5cef 107 speed_M2 = speed;
csharer 3:2f76ffbc5cef 108
csharer 3:2f76ffbc5cef 109 if(speed_M2 == 0) {
csharer 3:2f76ffbc5cef 110 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 111 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 112 } else if (speed_M2 > 0) {
csharer 3:2f76ffbc5cef 113 timer_period = 10000 / speed_M2;
csharer 3:2f76ffbc5cef 114 dir_M2 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 115 } else {
csharer 3:2f76ffbc5cef 116 timer_period = 10000 / -speed_M2;
csharer 3:2f76ffbc5cef 117 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 118 }
csharer 3:2f76ffbc5cef 119
csharer 3:2f76ffbc5cef 120 // Update Timer period
csharer 3:2f76ffbc5cef 121 timer_M2.attach_us(&ISR2, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 122 }