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@12:16d1a5390022, 2017-01-24 (annotated)

Committer:
arvindnr89
Date:
Tue Jan 24 01:30:16 2017 +0000
Revision:
12:16d1a5390022
Parent:
11:2553f5798f84 
Added Average Velocity Feature

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