Carter Sharer / Mbed 2 deprecated ESE519_Lab6_part3_skeleton

ESE519 Lab6 Part3

Dependencies:   MPU6050_Lab6_Part3 mbed

Fork of BroBot_v2 by Carter Sharer

stepper_motors.h@3:2f76ffbc5cef, 2016-10-12 (annotated)

Committer:
csharer
Date:
Wed Oct 12 05:04:10 2016 +0000
Revision:
3:2f76ffbc5cef
Child:
4:2512939c10f0
first commit to BroBot;

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 3:2f76ffbc5cef 21 // =============================================================================
csharer 3:2f76ffbc5cef 22 // === Interrupt Service Soutine ===
csharer 3:2f76ffbc5cef 23 // =============================================================================
csharer 3:2f76ffbc5cef 24 //ISR to step motor 1
csharer 3:2f76ffbc5cef 25 void ISR1(void)
csharer 3:2f76ffbc5cef 26 {
csharer 3:2f76ffbc5cef 27 step_M1 = 1;
csharer 3:2f76ffbc5cef 28 wait_us(1);
csharer 3:2f76ffbc5cef 29 step_M1 = 0;
csharer 3:2f76ffbc5cef 30 }
csharer 3:2f76ffbc5cef 31 //ISR to step motor 2
csharer 3:2f76ffbc5cef 32 void ISR2(void)
csharer 3:2f76ffbc5cef 33 {
csharer 3:2f76ffbc5cef 34 step_M2 = 1;
csharer 3:2f76ffbc5cef 35 wait_us(1);
csharer 3:2f76ffbc5cef 36 step_M2 = 0;
csharer 3:2f76ffbc5cef 37 }
csharer 3:2f76ffbc5cef 38
csharer 3:2f76ffbc5cef 39 //Set motor 1 speed. Speed [-100, +100]
csharer 3:2f76ffbc5cef 40 void setMotor1Speed(int16_t speed)
csharer 3:2f76ffbc5cef 41 {
csharer 3:2f76ffbc5cef 42 long timer_period;
csharer 3:2f76ffbc5cef 43 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 44
csharer 3:2f76ffbc5cef 45 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 46 int16_t desired_accel = speed_M1 - speed;
csharer 3:2f76ffbc5cef 47 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 48 speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 49 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 50 speed_M1 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 51 else
csharer 3:2f76ffbc5cef 52 speed_M1 = speed;
csharer 3:2f76ffbc5cef 53
csharer 3:2f76ffbc5cef 54 if(speed_M1 == 0) {
csharer 3:2f76ffbc5cef 55 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 56 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 57 } else if (speed_M1 > 0) {
csharer 3:2f76ffbc5cef 58 timer_period = 10000 / speed_M1;
csharer 3:2f76ffbc5cef 59 dir_M1 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 60 } else {
csharer 3:2f76ffbc5cef 61 timer_period = 10000 / -speed_M1;
csharer 3:2f76ffbc5cef 62 dir_M1 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 63 }
csharer 3:2f76ffbc5cef 64
csharer 3:2f76ffbc5cef 65 // Update Timer period
csharer 3:2f76ffbc5cef 66 timer_M1.attach_us(&ISR1, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 67 }
csharer 3:2f76ffbc5cef 68
csharer 3:2f76ffbc5cef 69 //Set motor 2 speed. Speed [-100, +100]
csharer 3:2f76ffbc5cef 70 void setMotor2Speed(int16_t speed)
csharer 3:2f76ffbc5cef 71 {
csharer 3:2f76ffbc5cef 72 long timer_period;
csharer 3:2f76ffbc5cef 73 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 74
csharer 3:2f76ffbc5cef 75 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 76 int16_t desired_accel = speed_M2 - speed;
csharer 3:2f76ffbc5cef 77 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 78 speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 79 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 80 speed_M2 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 81 else
csharer 3:2f76ffbc5cef 82 speed_M2 = speed;
csharer 3:2f76ffbc5cef 83
csharer 3:2f76ffbc5cef 84 if(speed_M2 == 0) {
csharer 3:2f76ffbc5cef 85 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 86 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 87 } else if (speed_M2 > 0) {
csharer 3:2f76ffbc5cef 88 timer_period = 10000 / speed_M2;
csharer 3:2f76ffbc5cef 89 dir_M2 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 90 } else {
csharer 3:2f76ffbc5cef 91 timer_period = 10000 / -speed_M2;
csharer 3:2f76ffbc5cef 92 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 93 }
csharer 3:2f76ffbc5cef 94
csharer 3:2f76ffbc5cef 95 // Update Timer period
csharer 3:2f76ffbc5cef 96 timer_M2.attach_us(&ISR2, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 97 }