Coursework Buggy / Mbed 2 deprecated 15_10_15_001

with speed calc

Dependencies:   mbed

main.cpp@0:7118ab174b9c, 2015-10-15 (annotated)

Committer:
benparkes
Date:
Thu Oct 15 12:13:00 2015 +0000
Revision:
0:7118ab174b9c
speeds 15/10/15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benparkes 0:7118ab174b9c 1 #include "mbed.h"
benparkes 0:7118ab174b9c 2
benparkes 0:7118ab174b9c 3 #define wheelc 0.1759292
benparkes 0:7118ab174b9c 4 #define pi 3.141592654
benparkes 0:7118ab174b9c 5 #define arcl 1.979203372
benparkes 0:7118ab174b9c 6 //Status LED
benparkes 0:7118ab174b9c 7 DigitalOut led(LED1);
benparkes 0:7118ab174b9c 8
benparkes 0:7118ab174b9c 9 //Motor PWM (speed)
benparkes 0:7118ab174b9c 10 PwmOut PWMA(PA_8);
benparkes 0:7118ab174b9c 11 PwmOut PWMB(PB_4);
benparkes 0:7118ab174b9c 12
benparkes 0:7118ab174b9c 13 //Motor Direction
benparkes 0:7118ab174b9c 14 DigitalOut DIRA(PA_9);
benparkes 0:7118ab174b9c 15 DigitalOut DIRB(PB_10);
benparkes 0:7118ab174b9c 16
benparkes 0:7118ab174b9c 17 //Hall-Effect Sensor Inputs
benparkes 0:7118ab174b9c 18 InterruptIn HEA1(PB_2);
benparkes 0:7118ab174b9c 19 DigitalIn HEA2(PB_1);
benparkes 0:7118ab174b9c 20 InterruptIn HEB1(PB_15);
benparkes 0:7118ab174b9c 21 DigitalIn HEB2(PB_14);
benparkes 0:7118ab174b9c 22
benparkes 0:7118ab174b9c 23 //On board switch
benparkes 0:7118ab174b9c 24 DigitalIn SW1(USER_BUTTON);
benparkes 0:7118ab174b9c 25
benparkes 0:7118ab174b9c 26 //Use the serial object so we can use higher speeds
benparkes 0:7118ab174b9c 27 Serial terminal(USBTX, USBRX);
benparkes 0:7118ab174b9c 28
benparkes 0:7118ab174b9c 29 //Timer used for measuring speeds
benparkes 0:7118ab174b9c 30 Timer timer;
benparkes 0:7118ab174b9c 31
benparkes 0:7118ab174b9c 32 //Enumerated types
benparkes 0:7118ab174b9c 33 enum DIRECTION {FORWARD=0, REVERSE};
benparkes 0:7118ab174b9c 34 enum PULSE {NOPULSE=0, PULSE};
benparkes 0:7118ab174b9c 35 enum SWITCHSTATE {PRESSED=0, RELEASED};
benparkes 0:7118ab174b9c 36
benparkes 0:7118ab174b9c 37 //Debug GPIO
benparkes 0:7118ab174b9c 38 DigitalOut probe(D10);
benparkes 0:7118ab174b9c 39
benparkes 0:7118ab174b9c 40 //Duty cycles
benparkes 0:7118ab174b9c 41 float dutyA = 1.0f; //100%
benparkes 0:7118ab174b9c 42 float dutyB = 1.0f; //100%
benparkes 0:7118ab174b9c 43
benparkes 0:7118ab174b9c 44 //distance measurement
benparkes 0:7118ab174b9c 45 float distanceA ;
benparkes 0:7118ab174b9c 46 float distanceB ;
benparkes 0:7118ab174b9c 47 float speedA ;
benparkes 0:7118ab174b9c 48 float speedB ;
benparkes 0:7118ab174b9c 49 float pretimerA;
benparkes 0:7118ab174b9c 50 float afttimerA;
benparkes 0:7118ab174b9c 51 float pretimerB;
benparkes 0:7118ab174b9c 52 float afttimerB;
benparkes 0:7118ab174b9c 53 float wheel_spacing = 0.128;
benparkes 0:7118ab174b9c 54
benparkes 0:7118ab174b9c 55 //Completed Loop
benparkes 0:7118ab174b9c 56 int loop=0;
benparkes 0:7118ab174b9c 57
benparkes 0:7118ab174b9c 58
benparkes 0:7118ab174b9c 59 void ResetDistanceTimeSpeed()
benparkes 0:7118ab174b9c 60 {
benparkes 0:7118ab174b9c 61 distanceA=0;
benparkes 0:7118ab174b9c 62 distanceB=0;
benparkes 0:7118ab174b9c 63 speedA=0;
benparkes 0:7118ab174b9c 64 speedB=0;
benparkes 0:7118ab174b9c 65 pretimerA=0;
benparkes 0:7118ab174b9c 66 pretimerB=0;
benparkes 0:7118ab174b9c 67 afttimerA=0;
benparkes 0:7118ab174b9c 68 afttimerB=0;
benparkes 0:7118ab174b9c 69 }
benparkes 0:7118ab174b9c 70
benparkes 0:7118ab174b9c 71 void stopmotors()
benparkes 0:7118ab174b9c 72 {
benparkes 0:7118ab174b9c 73 PWMA.write(0.0f); //0% duty cycle
benparkes 0:7118ab174b9c 74 PWMB.write(0.0f); //0% duty cycle
benparkes 0:7118ab174b9c 75 }
benparkes 0:7118ab174b9c 76
benparkes 0:7118ab174b9c 77 int forward(float distance, float speed, float duty)
benparkes 0:7118ab174b9c 78 {
benparkes 0:7118ab174b9c 79 //add forward to input with switch for scalability
benparkes 0:7118ab174b9c 80 int i;
benparkes 0:7118ab174b9c 81 // Set motor direction forward
benparkes 0:7118ab174b9c 82 DIRA = FORWARD;
benparkes 0:7118ab174b9c 83 DIRB = FORWARD;
benparkes 0:7118ab174b9c 84
benparkes 0:7118ab174b9c 85 // Set motor speed to input speed
benparkes 0:7118ab174b9c 86 PWMA.write(duty); //Set duty cycle (%)
benparkes 0:7118ab174b9c 87 PWMB.write(duty); //Set duty cycle (%)
benparkes 0:7118ab174b9c 88
benparkes 0:7118ab174b9c 89 //reset distance
benparkes 0:7118ab174b9c 90 ResetDistanceTimeSpeed();
benparkes 0:7118ab174b9c 91
benparkes 0:7118ab174b9c 92 //wait for run to complete
benparkes 0:7118ab174b9c 93 while (((distanceA+distanceB)/2) < distance) {
benparkes 0:7118ab174b9c 94 switch {
benparkes 0:7118ab174b9c 95 case(speedA<speed) {
benparkes 0:7118ab174b9c 96 duty += (0.1*duty);
benparkes 0:7118ab174b9c 97 PWMA.write(duty);
benparkes 0:7118ab174b9c 98 }
benparkes 0:7118ab174b9c 99
benparkes 0:7118ab174b9c 100 case(speedA>speed) {
benparkes 0:7118ab174b9c 101 duty+=(0.1*duty);
benparkes 0:7118ab174b9c 102 PWMA.write(duty);
benparkes 0:7118ab174b9c 103 }
benparkes 0:7118ab174b9c 104 return 1;
benparkes 0:7118ab174b9c 105 }
benparkes 0:7118ab174b9c 106
benparkes 0:7118ab174b9c 107 int turn(float distance, float duty, int loop) {
benparkes 0:7118ab174b9c 108 //Set initial motor direction
benparkes 0:7118ab174b9c 109 DIRA = FORWARD;
benparkes 0:7118ab174b9c 110 DIRB = FORWARD;
benparkes 0:7118ab174b9c 111
benparkes 0:7118ab174b9c 112 //test for loop completion
benparkes 0:7118ab174b9c 113 if(loop==1) {
benparkes 0:7118ab174b9c 114 DIRA = REVERSE;
benparkes 0:7118ab174b9c 115 DIRB = REVERSE;
benparkes 0:7118ab174b9c 116 }
benparkes 0:7118ab174b9c 117 /*
benparkes 0:7118ab174b9c 118 float C1 = (pi * 2 * distance_outside_wheel_turn_point)*(degrees/360);
benparkes 0:7118ab174b9c 119 float C2 = (pi * 2 * (distance_outside_wheel_turn_point-wheel_spacing))*(degrees/360);
benparkes 0:7118ab174b9c 120 */
benparkes 0:7118ab174b9c 121 //Set motor speed
benparkes 0:7118ab174b9c 122 PWMA.write(duty); //Set duty cycle (%)
benparkes 0:7118ab174b9c 123 PWMB.write(0.0f); //Set duty cycle (%)
benparkes 0:7118ab174b9c 124 //reset distance time and speed
benparkes 0:7118ab174b9c 125 resetDistanceTimeSpeed();
benparkes 0:7118ab174b9c 126 //wait for turn to complete
benparkes 0:7118ab174b9c 127 while (distanceA < distance) {
benparkes 0:7118ab174b9c 128 }
benparkes 0:7118ab174b9c 129 return 1;
benparkes 0:7118ab174b9c 130 }
benparkes 0:7118ab174b9c 131
benparkes 0:7118ab174b9c 132 void set_distanceA() {
benparkes 0:7118ab174b9c 133 float time = 0;
benparkes 0:7118ab174b9c 134
benparkes 0:7118ab174b9c 135 afttimerA = timer; //set latest timer to equal timer
benparkes 0:7118ab174b9c 136 distanceA += (wheelc/6); //set distance travelled for this instruction i.e forward/turn etc
benparkes 0:7118ab174b9c 137 terminal.printf("A =%f\n\r",distanceA); // print for fault finding (delete once working)
benparkes 0:7118ab174b9c 138 time = afttimer - pretimer; //
benparkes 0:7118ab174b9c 139 speedA = (wheelc/6)/time);
benparkes 0:7118ab174b9c 140 pretimerA = afttimerA;
benparkes 0:7118ab174b9c 141 }
benparkes 0:7118ab174b9c 142
benparkes 0:7118ab174b9c 143 void set_distanceB() {
benparkes 0:7118ab174b9c 144 float time = 0;
benparkes 0:7118ab174b9c 145
benparkes 0:7118ab174b9c 146 afttimerB = timer;
benparkes 0:7118ab174b9c 147 distanceB += (wheelc/6);
benparkes 0:7118ab174b9c 148 terminal.printf("A =%f\n\r",distanceB);
benparkes 0:7118ab174b9c 149 time = afttimer - pretimer;
benparkes 0:7118ab174b9c 150 speedB = (wheelc/6)/time);
benparkes 0:7118ab174b9c 151 pretimerB = afttimerB;
benparkes 0:7118ab174b9c 152 }
benparkes 0:7118ab174b9c 153
benparkes 0:7118ab174b9c 154
benparkes 0:7118ab174b9c 155 int main() {
benparkes 0:7118ab174b9c 156 //Configure the terminal to high speed
benparkes 0:7118ab174b9c 157 terminal.baud(115200);
benparkes 0:7118ab174b9c 158
benparkes 0:7118ab174b9c 159 HEA1.rise(set_distanceA);
benparkes 0:7118ab174b9c 160 HEB1.rise(set_distanceB);
benparkes 0:7118ab174b9c 161 //Set initial motor speed to stop
benparkes 0:7118ab174b9c 162 ` stopmotors();
benparkes 0:7118ab174b9c 163 PWMA.period_ms(10);
benparkes 0:7118ab174b9c 164 PWMB.period_ms(10);
benparkes 0:7118ab174b9c 165 while(1) {
benparkes 0:7118ab174b9c 166 //wait for switch press
benparkes 0:7118ab174b9c 167 while (SW1 == PRESSED) {
benparkes 0:7118ab174b9c 168 wait(0.01);
benparkes 0:7118ab174b9c 169 while(SW1 == RELEASED) {
benparkes 0:7118ab174b9c 170
benparkes 0:7118ab174b9c 171 //navigate course
benparkes 0:7118ab174b9c 172 for (int i = 0; i<2; i++) {
benparkes 0:7118ab174b9c 173 forward(12,50);
benparkes 0:7118ab174b9c 174 //terminal.printf("turn\n\r");
benparkes 0:7118ab174b9c 175 turn(arcl,50,0);
benparkes 0:7118ab174b9c 176 //terminal.printf("forward\n\r");
benparkes 0:7118ab174b9c 177 forward(6,50);
benparkes 0:7118ab174b9c 178 //terminal.printf("turn\n\r");
benparkes 0:7118ab174b9c 179 turn(arcl,50,0);
benparkes 0:7118ab174b9c 180 }
benparkes 0:7118ab174b9c 181
benparkes 0:7118ab174b9c 182 stopmotors();
benparkes 0:7118ab174b9c 183
benparkes 0:7118ab174b9c 184 wait(0.5);
benparkes 0:7118ab174b9c 185
benparkes 0:7118ab174b9c 186 //victory spin
benparkes 0:7118ab174b9c 187 turn(arcl*4.5,50,1);
benparkes 0:7118ab174b9c 188
benparkes 0:7118ab174b9c 189 stopmotors();
benparkes 0:7118ab174b9c 190 break;
benparkes 0:7118ab174b9c 191 }
benparkes 0:7118ab174b9c 192 }
benparkes 0:7118ab174b9c 193 }
benparkes 0:7118ab174b9c 194 }