Coursework Buggy / Mbed 2 deprecated 161015

speed code

Dependencies:   mbed

main.cpp@1:f4e3365155e1, 2015-10-18 (annotated)

Committer:
benparkes
Date:
Sun Oct 18 18:23:58 2015 +0000
Revision:
1:f4e3365155e1
Parent:
0:e79700919e2e 
new code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benparkes 0:e79700919e2e 1 #include "mbed.h"
benparkes 0:e79700919e2e 2
benparkes 1:f4e3365155e1 3
benparkes 0:e79700919e2e 4 #define wheelc 0.1759292
benparkes 1:f4e3365155e1 5 #define wheel_spacing 0.128 //add this in
benparkes 0:e79700919e2e 6 #define pi 3.141592654
benparkes 0:e79700919e2e 7 #define degreel 0.021988888
benparkes 0:e79700919e2e 8 //Status LED
benparkes 0:e79700919e2e 9 DigitalOut led(LED1);
benparkes 0:e79700919e2e 10
benparkes 0:e79700919e2e 11 //Motor PWM (speed)
benparkes 0:e79700919e2e 12 PwmOut PWMA(PA_8);
benparkes 0:e79700919e2e 13 PwmOut PWMB(PB_4);
benparkes 0:e79700919e2e 14
benparkes 0:e79700919e2e 15 //Motor Direction
benparkes 0:e79700919e2e 16 DigitalOut DIRA(PA_9);
benparkes 0:e79700919e2e 17 DigitalOut DIRB(PB_10);
benparkes 0:e79700919e2e 18
benparkes 0:e79700919e2e 19 //Hall-Effect Sensor Inputs
benparkes 0:e79700919e2e 20 InterruptIn HEA1(PB_2);
benparkes 1:f4e3365155e1 21 InterruptIn HEA2(PB_1);
benparkes 0:e79700919e2e 22 InterruptIn HEB1(PB_15);
benparkes 1:f4e3365155e1 23 InterruptIn HEB2(PB_14);
benparkes 0:e79700919e2e 24
benparkes 0:e79700919e2e 25 //On board switch
benparkes 0:e79700919e2e 26 DigitalIn SW1(USER_BUTTON);
benparkes 0:e79700919e2e 27
benparkes 0:e79700919e2e 28 //Use the serial object so we can use higher speeds
benparkes 0:e79700919e2e 29 Serial terminal(USBTX, USBRX);
benparkes 0:e79700919e2e 30
benparkes 0:e79700919e2e 31 //Timer used for measuring speeds
benparkes 0:e79700919e2e 32 Timer timer;
benparkes 0:e79700919e2e 33
benparkes 0:e79700919e2e 34 //Enumerated types
benparkes 0:e79700919e2e 35 enum DIRECTION {FORWARD=0, REVERSE};
benparkes 1:f4e3365155e1 36 enum PULSE {NOPULSE=0, PULSE};
benparkes 0:e79700919e2e 37 enum SWITCHSTATE {PRESSED=0, RELEASED};
benparkes 0:e79700919e2e 38
benparkes 0:e79700919e2e 39 //Debug GPIO
benparkes 0:e79700919e2e 40 DigitalOut probe(D10);
benparkes 0:e79700919e2e 41
benparkes 0:e79700919e2e 42 //Duty cycles
benparkes 1:f4e3365155e1 43 float dutyA = 0.000f; //100%
benparkes 1:f4e3365155e1 44 float dutyB = 0.000f; //100%
benparkes 0:e79700919e2e 45
benparkes 0:e79700919e2e 46 //distance measurement
benparkes 0:e79700919e2e 47 float distanceA ;
benparkes 0:e79700919e2e 48 float distanceB ;
benparkes 0:e79700919e2e 49 float speedA ;
benparkes 0:e79700919e2e 50 float speedB ;
benparkes 1:f4e3365155e1 51
benparkes 1:f4e3365155e1 52 //for interrupt speed and distance calcs
benparkes 1:f4e3365155e1 53 int tA1[2];
benparkes 1:f4e3365155e1 54 int tA2[2];
benparkes 1:f4e3365155e1 55 int tB1[2];
benparkes 1:f4e3365155e1 56 int tB2[2];
benparkes 1:f4e3365155e1 57 int pulse_orderA = 1;
benparkes 1:f4e3365155e1 58 int pulse_orderB = 1;
benparkes 1:f4e3365155e1 59
benparkes 1:f4e3365155e1 60 //interupt handler
benparkes 1:f4e3365155e1 61 int HEPulse(int let,int num, int order) //tell us which hall effect HEA1= hall effect, (let, A=1, B=2), (num, 1/2), order (index possition to be filled alternates)
benparkes 1:f4e3365155e1 62 {
benparkes 1:f4e3365155e1 63 float fA1;
benparkes 1:f4e3365155e1 64 float fA2;
benparkes 1:f4e3365155e1 65 float fB1;
benparkes 1:f4e3365155e1 66 float fB2;
benparkes 1:f4e3365155e1 67 int time;
benparkes 1:f4e3365155e1 68 time = timer.read_us(); //use to check for interrupt debounce
benparkes 1:f4e3365155e1 69
benparkes 1:f4e3365155e1 70 if((tA1[0]-time<5)||(tA2[0]-time<5)||(tA1[1]-time<5)||(tA2[1]-time<5)) {
benparkes 1:f4e3365155e1 71 return 1;
benparkes 1:f4e3365155e1 72 } // if less than 5us from last interupt assume debouce and ignore
benparkes 1:f4e3365155e1 73 //find right thing to do for current hall effect
benparkes 1:f4e3365155e1 74 switch(order) { //check whether to fill index possition 0/1
benparkes 1:f4e3365155e1 75 case(1) :
benparkes 1:f4e3365155e1 76 switch(let) { //check hall effect A/B
benparkes 1:f4e3365155e1 77 case(1):
benparkes 1:f4e3365155e1 78
benparkes 1:f4e3365155e1 79 switch(num) { //check hall effect 1/2
benparkes 1:f4e3365155e1 80 case (1):
benparkes 1:f4e3365155e1 81 tA1[0] = timer.read_us();
benparkes 1:f4e3365155e1 82 distanceA += (wheelc/6); //increment distance
benparkes 1:f4e3365155e1 83 pulse_orderA = 2; //toggle index to be filled
benparkes 1:f4e3365155e1 84 break;
benparkes 1:f4e3365155e1 85
benparkes 1:f4e3365155e1 86 case(2) :
benparkes 1:f4e3365155e1 87 tA2[0] = timer.read_us();
benparkes 1:f4e3365155e1 88 pulse_orderA = 2;
benparkes 1:f4e3365155e1 89 break;
benparkes 1:f4e3365155e1 90 }
benparkes 1:f4e3365155e1 91 break;
benparkes 1:f4e3365155e1 92
benparkes 1:f4e3365155e1 93
benparkes 1:f4e3365155e1 94 case(2): //check hall effect A/B
benparkes 1:f4e3365155e1 95
benparkes 1:f4e3365155e1 96 switch(num) {
benparkes 1:f4e3365155e1 97 case (1):
benparkes 1:f4e3365155e1 98 tB1[0] = timer.read_us();
benparkes 1:f4e3365155e1 99 distanceB += (wheelc/6);
benparkes 1:f4e3365155e1 100 pulse_orderB = 2;
benparkes 1:f4e3365155e1 101 break;
benparkes 0:e79700919e2e 102
benparkes 1:f4e3365155e1 103 case(2) :
benparkes 1:f4e3365155e1 104 tB2[0] = timer.read_us();
benparkes 1:f4e3365155e1 105 pulse_orderB = 2;
benparkes 1:f4e3365155e1 106 break;
benparkes 1:f4e3365155e1 107 }
benparkes 1:f4e3365155e1 108 break;
benparkes 1:f4e3365155e1 109
benparkes 1:f4e3365155e1 110 }
benparkes 1:f4e3365155e1 111 case(2) :
benparkes 1:f4e3365155e1 112 switch(let) {
benparkes 1:f4e3365155e1 113 case(1):
benparkes 1:f4e3365155e1 114
benparkes 1:f4e3365155e1 115 switch(num) {
benparkes 1:f4e3365155e1 116 case (1):
benparkes 1:f4e3365155e1 117 tA1[1] = timer.read_us();
benparkes 1:f4e3365155e1 118 distanceA += (wheelc/6);
benparkes 1:f4e3365155e1 119 fA1 = 1.0f/(( fabsf(tA1[1]-tA1[0]) ) * (float)3.0E-6);
benparkes 1:f4e3365155e1 120 pulse_orderA = 1;
benparkes 1:f4e3365155e1 121 break;
benparkes 1:f4e3365155e1 122
benparkes 1:f4e3365155e1 123 case(2) :
benparkes 1:f4e3365155e1 124 tA2[1] = timer.read_us();
benparkes 1:f4e3365155e1 125 fA2 = 1.0f/(( fabsf(tA2[1]-tA2[0]) ) * (float)3.0E-6);
benparkes 1:f4e3365155e1 126 pulse_orderA = 1;
benparkes 1:f4e3365155e1 127 break;
benparkes 1:f4e3365155e1 128 }
benparkes 1:f4e3365155e1 129 break;
benparkes 1:f4e3365155e1 130
benparkes 0:e79700919e2e 131
benparkes 1:f4e3365155e1 132 case(2):
benparkes 1:f4e3365155e1 133
benparkes 1:f4e3365155e1 134 switch(num) {
benparkes 1:f4e3365155e1 135 case (1):
benparkes 1:f4e3365155e1 136 tB1[1] = timer.read_us();
benparkes 1:f4e3365155e1 137 distanceB += (wheelc/6);
benparkes 1:f4e3365155e1 138 fB1 = 1.0f/(( fabsf(tB1[1]-tB1[0]) ) * (float)3.0E-6);
benparkes 1:f4e3365155e1 139 pulse_orderB = 1;
benparkes 1:f4e3365155e1 140 break;
benparkes 1:f4e3365155e1 141
benparkes 1:f4e3365155e1 142 case(2) :
benparkes 1:f4e3365155e1 143 tB2[1] = timer.read_us();
benparkes 1:f4e3365155e1 144 fB2 = 1.0f/((fabsf( tB2[1]-tB2[0]) ) * (float)3.0E-6);
benparkes 1:f4e3365155e1 145 pulse_orderB = 1;
benparkes 1:f4e3365155e1 146 break;
benparkes 1:f4e3365155e1 147 }
benparkes 1:f4e3365155e1 148 break;
benparkes 1:f4e3365155e1 149
benparkes 1:f4e3365155e1 150 }
benparkes 1:f4e3365155e1 151
benparkes 1:f4e3365155e1 152 float fA = (fA1 + fA2)*0.5f; //calculate average frequency of hall effect A
benparkes 1:f4e3365155e1 153 float fB = (fB1 + fB2)*0.5f; //calculate average frequency of hall effect B
benparkes 1:f4e3365155e1 154 speedA = fA/20.8f; //output speeds of motors by frequency/gear box ratio
benparkes 1:f4e3365155e1 155 speedB = fB/20.8f;
benparkes 1:f4e3365155e1 156 }
benparkes 1:f4e3365155e1 157 return 1;
benparkes 1:f4e3365155e1 158 }
benparkes 1:f4e3365155e1 159
benparkes 1:f4e3365155e1 160 void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
benparkes 0:e79700919e2e 161 {
benparkes 0:e79700919e2e 162 distanceA=0;
benparkes 0:e79700919e2e 163 distanceB=0;
benparkes 0:e79700919e2e 164 speedA=0;
benparkes 0:e79700919e2e 165 speedB=0;
benparkes 0:e79700919e2e 166 timer.reset();
benparkes 0:e79700919e2e 167 timer.start();
benparkes 0:e79700919e2e 168 }
benparkes 0:e79700919e2e 169
benparkes 1:f4e3365155e1 170 void stopmotors() //stop motors dead
benparkes 0:e79700919e2e 171 {
benparkes 0:e79700919e2e 172 PWMA.write(0.0f); //0% duty cycle
benparkes 0:e79700919e2e 173 PWMB.write(0.0f); //0% duty cycle
benparkes 0:e79700919e2e 174 }
benparkes 0:e79700919e2e 175
benparkes 1:f4e3365155e1 176 void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
benparkes 0:e79700919e2e 177 {
benparkes 1:f4e3365155e1 178
benparkes 1:f4e3365155e1 179 float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed
benparkes 1:f4e3365155e1 180 dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error
benparkes 1:f4e3365155e1 181 PWMA.write(dutyA); //write new duty to motor
benparkes 1:f4e3365155e1 182
benparkes 1:f4e3365155e1 183 float deltaB = target_speedB -speedB; //repeat for B motor
benparkes 1:f4e3365155e1 184 dutyB = dutyB + deltaB*0.1f;
benparkes 1:f4e3365155e1 185 PWMB.write(dutyB);
benparkes 1:f4e3365155e1 186 }
benparkes 1:f4e3365155e1 187
benparkes 1:f4e3365155e1 188
benparkes 1:f4e3365155e1 189 int forward(float distance, float speed) //forward motion, input distance and speed (rev/s)
benparkes 1:f4e3365155e1 190 {
benparkes 0:e79700919e2e 191 // Set motor direction forward
benparkes 0:e79700919e2e 192 DIRA = FORWARD;
benparkes 0:e79700919e2e 193 DIRB = FORWARD;
benparkes 0:e79700919e2e 194
benparkes 1:f4e3365155e1 195 //reset distance
benparkes 0:e79700919e2e 196 ResetDistanceTimeSpeed();
benparkes 1:f4e3365155e1 197
benparkes 1:f4e3365155e1 198 // Set initial motor to be altered by speed code
benparkes 1:f4e3365155e1 199 PWMA.write(0.1f*speed); //Set duty cycle (%)
benparkes 1:f4e3365155e1 200 PWMB.write(0.1f*speed); //Set duty cycle (%)
benparkes 0:e79700919e2e 201
benparkes 0:e79700919e2e 202
benparkes 1:f4e3365155e1 203 //wait for run to complete byy checking if distance has been covered yet
benparkes 0:e79700919e2e 204 while (((distanceA+distanceB)/2) < distance) {
benparkes 1:f4e3365155e1 205 //maintain desired speed
benparkes 1:f4e3365155e1 206 HuntSpeeds(speed, speed);
benparkes 0:e79700919e2e 207 }
benparkes 0:e79700919e2e 208 return 1;
benparkes 0:e79700919e2e 209 }
benparkes 0:e79700919e2e 210
benparkes 1:f4e3365155e1 211 int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m))
benparkes 1:f4e3365155e1 212
benparkes 1:f4e3365155e1 213 {
benparkes 1:f4e3365155e1 214 // Calculate Turn Distance
benparkes 1:f4e3365155e1 215 float C1 = (pi * 2 * (float)turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
benparkes 1:f4e3365155e1 216 float C2 = (pi * 2 * ((float)turn_radius-(float)wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
benparkes 1:f4e3365155e1 217 //Set Initial Motor Direction
benparkes 1:f4e3365155e1 218 DIRA = FORWARD;
benparkes 1:f4e3365155e1 219 DIRB = FORWARD;
benparkes 0:e79700919e2e 220
benparkes 1:f4e3365155e1 221 // Test for direction to Enter Victory Spin
benparkes 1:f4e3365155e1 222 if(direction==REVERSE) {
benparkes 1:f4e3365155e1 223 DIRA = REVERSE;
benparkes 1:f4e3365155e1 224 DIRB = FORWARD;//victory spin opposite wheels
benparkes 1:f4e3365155e1 225 }
benparkes 1:f4e3365155e1 226 // Reset Distance Travelled
benparkes 1:f4e3365155e1 227 ResetDistanceTimeSpeed();
benparkes 1:f4e3365155e1 228
benparkes 1:f4e3365155e1 229 // Wait for Turn to Complete by checking whether outer wheel completed spin yet
benparkes 1:f4e3365155e1 230 while (distanceA < C1) {
benparkes 1:f4e3365155e1 231 if(direction==FORWARD) {
benparkes 1:f4e3365155e1 232 HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel
benparkes 1:f4e3365155e1 233 }
benparkes 1:f4e3365155e1 234
benparkes 1:f4e3365155e1 235 if(direction==REVERSE) {
benparkes 1:f4e3365155e1 236 HuntSpeeds(speed,speed); //set speeds same for reverse spin
benparkes 1:f4e3365155e1 237 }
benparkes 0:e79700919e2e 238 }
benparkes 1:f4e3365155e1 239 return 1;
benparkes 1:f4e3365155e1 240 }
benparkes 1:f4e3365155e1 241 //interupt handlers
benparkes 1:f4e3365155e1 242 void HEA1I(void)
benparkes 1:f4e3365155e1 243 {
benparkes 1:f4e3365155e1 244 HEPulse(1,1,pulse_orderA);
benparkes 1:f4e3365155e1 245 }
benparkes 1:f4e3365155e1 246 void HEA2I(void)
benparkes 1:f4e3365155e1 247 {
benparkes 1:f4e3365155e1 248 HEPulse(1,2,pulse_orderA);
benparkes 1:f4e3365155e1 249 }
benparkes 1:f4e3365155e1 250 void HEB1I(void)
benparkes 1:f4e3365155e1 251 {
benparkes 1:f4e3365155e1 252 HEPulse(2,1,pulse_orderB);
benparkes 1:f4e3365155e1 253 }
benparkes 1:f4e3365155e1 254 void HEB2I(void)
benparkes 1:f4e3365155e1 255 {
benparkes 1:f4e3365155e1 256 HEPulse(2,2,pulse_orderB);
benparkes 1:f4e3365155e1 257 }
benparkes 0:e79700919e2e 258
benparkes 1:f4e3365155e1 259 int main()
benparkes 1:f4e3365155e1 260 {
benparkes 1:f4e3365155e1 261 //Configure the terminal to high speed
benparkes 1:f4e3365155e1 262 terminal.baud(115200);
benparkes 1:f4e3365155e1 263 // initiate interupts to call interupt code
benparkes 1:f4e3365155e1 264 HEA1.rise(&HEA1I);
benparkes 1:f4e3365155e1 265 HEA2.rise(&HEA2I);
benparkes 1:f4e3365155e1 266 HEB1.rise(&HEB1I);
benparkes 1:f4e3365155e1 267 HEB2.rise(&HEB2I);
benparkes 1:f4e3365155e1 268 //Set initial motor speed to stop
benparkes 1:f4e3365155e1 269 stopmotors();
benparkes 1:f4e3365155e1 270 //set motor frequency
benparkes 1:f4e3365155e1 271 PWMA.period_ms(10);
benparkes 1:f4e3365155e1 272 PWMB.period_ms(10);
benparkes 1:f4e3365155e1 273 ResetDistanceTimeSpeed();
benparkes 1:f4e3365155e1 274 while(1) {
benparkes 1:f4e3365155e1 275 //wait for switch press
benparkes 1:f4e3365155e1 276 while (SW1 == PRESSED) {
benparkes 1:f4e3365155e1 277 wait(0.01);
benparkes 1:f4e3365155e1 278 while(SW1 == RELEASED) {
benparkes 1:f4e3365155e1 279
benparkes 1:f4e3365155e1 280 //navigate course
benparkes 1:f4e3365155e1 281 for (int i = 0; i<2; i++) {
benparkes 1:f4e3365155e1 282 //forward 1m
benparkes 1:f4e3365155e1 283 forward(12,1); //distance, speed in rotations/s
benparkes 1:f4e3365155e1 284 //turn 90
benparkes 1:f4e3365155e1 285 turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
benparkes 1:f4e3365155e1 286 //forward 1/2m
benparkes 1:f4e3365155e1 287 forward(7,1);//distance, speed in rotations/s
benparkes 1:f4e3365155e1 288 //turn 90
benparkes 1:f4e3365155e1 289 turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
benparkes 1:f4e3365155e1 290 }
benparkes 1:f4e3365155e1 291 //repeat twice to end up back in starting possition
benparkes 1:f4e3365155e1 292
benparkes 1:f4e3365155e1 293 //victory spin
benparkes 1:f4e3365155e1 294 turn(365,0.5,1,0.001);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
benparkes 1:f4e3365155e1 295
benparkes 1:f4e3365155e1 296 stopmotors();
benparkes 1:f4e3365155e1 297 break;
benparkes 1:f4e3365155e1 298 }
benparkes 1:f4e3365155e1 299 }
benparkes 0:e79700919e2e 300 }
benparkes 0:e79700919e2e 301
benparkes 1:f4e3365155e1 302 }
benparkes 0:e79700919e2e 303