Justin Liu / Mbed 2 deprecated robotcode

robot

Dependencies:   MPU6050_Lab6_Part3 mbed

Fork of ESE519_Lab6_part3_skeleton by Carter Sharer

stepper_motors.h@9:d9776d8ce47c, 2016-11-16 (annotated)

Committer:
jliutang
Date:
Wed Nov 16 00:14:11 2016 +0000
Revision:
9:d9776d8ce47c
Parent:
6:ae3e6aefe908 
11/15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 6:ae3e6aefe908 1 //stepper_motors.h
csharer 6:ae3e6aefe908 2 //Author: Carter Sharer
csharer 6:ae3e6aefe908 3 //Date: 10/10/2016
csharer 6:ae3e6aefe908 4
csharer 3:2f76ffbc5cef 5 //STEPPER MOTORS
csharer 6:ae3e6aefe908 6 #define ZERO_SPEED 10000000 //65535
csharer 3:2f76ffbc5cef 7 #define MAX_ACCEL 7
csharer 3:2f76ffbc5cef 8 #define ENABLE 0
csharer 3:2f76ffbc5cef 9 #define DISABLE 1
csharer 3:2f76ffbc5cef 10
csharer 3:2f76ffbc5cef 11 Ticker timer_M1, timer_M2; //Timers for speed Control
csharer 3:2f76ffbc5cef 12
csharer 3:2f76ffbc5cef 13 // MOTOR 1
csharer 3:2f76ffbc5cef 14 DigitalOut step_M1(MOTOR1_STEP);
csharer 3:2f76ffbc5cef 15 DigitalOut dir_M1(MOTOR1_DIR);
csharer 3:2f76ffbc5cef 16 DigitalOut enable(MOTOR_ENABLE); //enable for both motors
csharer 3:2f76ffbc5cef 17 int16_t speed_M1; //Speed of motor 1
csharer 3:2f76ffbc5cef 18
csharer 3:2f76ffbc5cef 19 //MOTOR 2
csharer 3:2f76ffbc5cef 20 DigitalOut step_M2(MOTOR2_STEP);
csharer 3:2f76ffbc5cef 21 DigitalOut dir_M2(MOTOR2_DIR);
csharer 3:2f76ffbc5cef 22 int16_t speed_M2; //Speed of motor 2
csharer 3:2f76ffbc5cef 23 int16_t motor1, motor2;
csharer 3:2f76ffbc5cef 24
csharer 4:2512939c10f0 25 //Motor Position
csharer 4:2512939c10f0 26 int pos_M1 = 0, pos_M2 = 0;
csharer 4:2512939c10f0 27
csharer 3:2f76ffbc5cef 28 // =============================================================================
csharer 3:2f76ffbc5cef 29 // === Interrupt Service Soutine ===
csharer 3:2f76ffbc5cef 30 // =============================================================================
csharer 3:2f76ffbc5cef 31 //ISR to step motor 1
csharer 3:2f76ffbc5cef 32 void ISR1(void)
csharer 3:2f76ffbc5cef 33 {
csharer 4:2512939c10f0 34 //Step Motor
csharer 3:2f76ffbc5cef 35 step_M1 = 1;
csharer 3:2f76ffbc5cef 36 wait_us(1);
csharer 3:2f76ffbc5cef 37 step_M1 = 0;
csharer 4:2512939c10f0 38
csharer 6:ae3e6aefe908 39 //Update Motor Postion
csharer 4:2512939c10f0 40 if(dir_M1)
csharer 4:2512939c10f0 41 pos_M1++;
csharer 4:2512939c10f0 42 else
csharer 4:2512939c10f0 43 pos_M1--;
csharer 3:2f76ffbc5cef 44 }
csharer 3:2f76ffbc5cef 45 //ISR to step motor 2
csharer 3:2f76ffbc5cef 46 void ISR2(void)
csharer 3:2f76ffbc5cef 47 {
csharer 4:2512939c10f0 48 //Step Motor
csharer 3:2f76ffbc5cef 49 step_M2 = 1;
csharer 3:2f76ffbc5cef 50 wait_us(1);
csharer 3:2f76ffbc5cef 51 step_M2 = 0;
csharer 4:2512939c10f0 52
csharer 6:ae3e6aefe908 53 //Update Motor Position
csharer 4:2512939c10f0 54 if(dir_M2)
csharer 4:2512939c10f0 55 pos_M2++;
csharer 4:2512939c10f0 56 else
csharer 4:2512939c10f0 57 pos_M2--;
csharer 3:2f76ffbc5cef 58 }
csharer 3:2f76ffbc5cef 59
csharer 6:ae3e6aefe908 60 //Set motor 1 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
csharer 3:2f76ffbc5cef 61 void setMotor1Speed(int16_t speed)
csharer 3:2f76ffbc5cef 62 {
csharer 3:2f76ffbc5cef 63 long timer_period;
csharer 3:2f76ffbc5cef 64 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 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 6:ae3e6aefe908 90 //Set motor 2 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
csharer 3:2f76ffbc5cef 91 void setMotor2Speed(int16_t speed)
csharer 3:2f76ffbc5cef 92 {
csharer 3:2f76ffbc5cef 93 long timer_period;
csharer 3:2f76ffbc5cef 94 speed = CAP(speed, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 95
csharer 3:2f76ffbc5cef 96 //Calculate acceleration from the desired speed
csharer 3:2f76ffbc5cef 97 int16_t desired_accel = speed_M2 - speed;
csharer 3:2f76ffbc5cef 98 if(desired_accel > MAX_ACCEL)
csharer 3:2f76ffbc5cef 99 speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 100 else if(desired_accel < -MAX_ACCEL)
csharer 3:2f76ffbc5cef 101 speed_M2 += MAX_ACCEL; //Change speed of motor by max acceleration
csharer 3:2f76ffbc5cef 102 else
csharer 3:2f76ffbc5cef 103 speed_M2 = speed;
csharer 3:2f76ffbc5cef 104
csharer 3:2f76ffbc5cef 105 if(speed_M2 == 0) {
csharer 3:2f76ffbc5cef 106 timer_period = ZERO_SPEED;
csharer 3:2f76ffbc5cef 107 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 108 } else if (speed_M2 > 0) {
csharer 3:2f76ffbc5cef 109 timer_period = 10000 / speed_M2;
csharer 3:2f76ffbc5cef 110 dir_M2 = 1; //sets motor direction
csharer 3:2f76ffbc5cef 111 } else {
csharer 3:2f76ffbc5cef 112 timer_period = 10000 / -speed_M2;
csharer 3:2f76ffbc5cef 113 dir_M2 = 0; ////sets motor direction
csharer 3:2f76ffbc5cef 114 }
csharer 3:2f76ffbc5cef 115
csharer 3:2f76ffbc5cef 116 // Update Timer period
csharer 3:2f76ffbc5cef 117 timer_M2.attach_us(&ISR2, timer_period); //This is what sets motor speed
csharer 3:2f76ffbc5cef 118 }