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@4:2512939c10f0, 2016-10-18 (annotated)

Committer:
csharer
Date:
Tue Oct 18 20:44:21 2016 +0000
Revision:
4:2512939c10f0
Parent:
3:2f76ffbc5cef
Child:
6:62cdb7482b50
brobot version 3, this is using the old MRF24J40 Lib that used uint8_t

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 3:2f76ffbc5cef 1 //STEPPER MOTORS
csharer 3:2f76ffbc5cef 2 #define ZERO_SPEED 65535
csharer 3:2f76ffbc5cef 3 #define MAX_ACCEL 7
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;
csharer 3:2f76ffbc5cef 60 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 61
csharer 3:2f76ffbc5cef 62 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 63 int16_t desired_accel = speed_M1 - speed;
csharer 3:2f76ffbc5cef 64 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 65 speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 66 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 67 speed_M1 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 68 else
csharer 3:2f76ffbc5cef 69 speed_M1 = speed;
csharer 3:2f76ffbc5cef 70
csharer 3:2f76ffbc5cef 71 if(speed_M1 == 0) {
csharer 3:2f76ffbc5cef 72 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 73 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 74 } else if (speed_M1 > 0) {
csharer 3:2f76ffbc5cef 75 timer_period = 10000 / speed_M1;
csharer 3:2f76ffbc5cef 76 dir_M1 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 77 } else {
csharer 3:2f76ffbc5cef 78 timer_period = 10000 / -speed_M1;
csharer 3:2f76ffbc5cef 79 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 80 }
csharer 3:2f76ffbc5cef 81
csharer 3:2f76ffbc5cef 82 // Update Timer period
csharer 3:2f76ffbc5cef 83 timer_M1.attach_us(&ISR1, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 84 }
csharer 3:2f76ffbc5cef 85
csharer 3:2f76ffbc5cef 86 //Set motor 2 speed. Speed [-100, +100]
csharer 3:2f76ffbc5cef 87 void setMotor2Speed(int16_t speed)
csharer 3:2f76ffbc5cef 88 {
csharer 3:2f76ffbc5cef 89 long timer_period;
csharer 3:2f76ffbc5cef 90 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 91
csharer 3:2f76ffbc5cef 92 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 93 int16_t desired_accel = speed_M2 - speed;
csharer 3:2f76ffbc5cef 94 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 95 speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 96 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 97 speed_M2 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 98 else
csharer 3:2f76ffbc5cef 99 speed_M2 = speed;
csharer 3:2f76ffbc5cef 100
csharer 3:2f76ffbc5cef 101 if(speed_M2 == 0) {
csharer 3:2f76ffbc5cef 102 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 103 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 104 } else if (speed_M2 > 0) {
csharer 3:2f76ffbc5cef 105 timer_period = 10000 / speed_M2;
csharer 3:2f76ffbc5cef 106 dir_M2 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 107 } else {
csharer 3:2f76ffbc5cef 108 timer_period = 10000 / -speed_M2;
csharer 3:2f76ffbc5cef 109 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 110 }
csharer 3:2f76ffbc5cef 111
csharer 3:2f76ffbc5cef 112 // Update Timer period
csharer 3:2f76ffbc5cef 113 timer_M2.attach_us(&ISR2, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 114 }