Rachel Ireland-Jones / Mbed OS FinalYear0

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@26:072ab2309eec, 2019-12-18 (annotated)

Committer:
Mikebob
Date:
Wed Dec 18 16:11:53 2019 +0000
Revision:
26:072ab2309eec
Parent:
25:50d3f80cb763
Child:
27:44ab9ebf07eb
just needs speed altering and comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mikebob 26:072ab2309eec 1 //Enhancement 3//
Mikebob 21:d553c43a5a26 2
Mikebob 14:e66cf781f5b9 3 #include "mbed.h"
Mikebob 19:d3b82416df50 4
Mikebob 14:e66cf781f5b9 5 //Motor PWM (speed)
Mikebob 14:e66cf781f5b9 6 PwmOut PWMA(PA_8);
Mikebob 14:e66cf781f5b9 7 PwmOut PWMB(PB_4);
Mikebob 19:d3b82416df50 8
Mikebob 14:e66cf781f5b9 9 //Motor Direction
Mikebob 14:e66cf781f5b9 10 DigitalOut DIRA(PA_9);
Mikebob 14:e66cf781f5b9 11 DigitalOut DIRB(PB_10);
Mikebob 19:d3b82416df50 12
Mikebob 14:e66cf781f5b9 13 //Hall-Effect Sensor Inputs
Mikebob 14:e66cf781f5b9 14 DigitalIn HEA1(PB_2);
Mikebob 14:e66cf781f5b9 15 DigitalIn HEA2(PB_1);
Mikebob 14:e66cf781f5b9 16 DigitalIn HEB1(PB_15);
Mikebob 14:e66cf781f5b9 17 DigitalIn HEB2(PB_14);
Mikebob 18:11937e78239c 18
Mikebob 14:e66cf781f5b9 19 //On board switch
Mikebob 14:e66cf781f5b9 20 DigitalIn SW1(USER_BUTTON);
Mikebob 19:d3b82416df50 21
Mikebob 14:e66cf781f5b9 22 //Use the serial object so we can use higher speeds
Mikebob 14:e66cf781f5b9 23 Serial terminal(USBTX, USBRX);
Mikebob 19:d3b82416df50 24
Mikebob 14:e66cf781f5b9 25 //Timer used for measuring speeds
Mikebob 18:11937e78239c 26 Timer timerA;
Mikebob 18:11937e78239c 27 Timer timerB;
Mikebob 19:d3b82416df50 28
Mikebob 14:e66cf781f5b9 29 //Enumerated types
Mikebob 14:e66cf781f5b9 30 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 14:e66cf781f5b9 31 enum PULSE {NOPULSE=0, PULSE};
Mikebob 14:e66cf781f5b9 32 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 19:d3b82416df50 33
Mikebob 14:e66cf781f5b9 34 //Duty cycles
Mikebob 22:568a6d19b98a 35 float dutyA = 0.2f; //100%
Mikebob 22:568a6d19b98a 36 float dutyB = 0.2f; //100%
Mikebob 22:568a6d19b98a 37 float speedA[3];
Mikebob 22:568a6d19b98a 38 float speedB[3];
Mikebob 22:568a6d19b98a 39 float fA, fB = 0.0f;
Mikebob 22:568a6d19b98a 40 float sumA, sumB = 0.0f;
Mikebob 22:568a6d19b98a 41 int durA, durB = 0;
Mikebob 22:568a6d19b98a 42 float TA[2];
Mikebob 22:568a6d19b98a 43 float TB[2];
Mikebob 22:568a6d19b98a 44 float TAA, TBB = 0.0f;
Mikebob 26:072ab2309eec 45 float pulseA, pulseB = 0.0f;
Mikebob 26:072ab2309eec 46 float travA, travB = 0.0f;
Mikebob 21:d553c43a5a26 47 void timeA()
Mikebob 21:d553c43a5a26 48 {
Mikebob 21:d553c43a5a26 49 static int n=0; //Number of pulse sets
Mikebob 21:d553c43a5a26 50 static int HallState = 0;
Mikebob 21:d553c43a5a26 51 if (n==0) {
Mikebob 21:d553c43a5a26 52 //Reset timer and Start
Mikebob 18:11937e78239c 53 timerA.reset();
Mikebob 26:072ab2309eec 54 timerA.start();
Mikebob 26:072ab2309eec 55 TA[0] = timerA.read_us();
Mikebob 21:d553c43a5a26 56 }
Mikebob 26:072ab2309eec 57 switch(HallState) {
Mikebob 26:072ab2309eec 58 case 0:
Mikebob 26:072ab2309eec 59 if(HEA1 == NOPULSE) {
Mikebob 26:072ab2309eec 60 HallState = 1;
Mikebob 18:11937e78239c 61 }
Mikebob 26:072ab2309eec 62 break;
Mikebob 26:072ab2309eec 63 case 1:
Mikebob 26:072ab2309eec 64 if(HEA1 == PULSE) {
Mikebob 26:072ab2309eec 65 HallState = 0;
Mikebob 26:072ab2309eec 66 n++;
Mikebob 26:072ab2309eec 67 pulseA++;
Mikebob 26:072ab2309eec 68 travA = ((176/20.8)/3)*pulseA;
Mikebob 26:072ab2309eec 69 }
Mikebob 26:072ab2309eec 70 break;
Mikebob 26:072ab2309eec 71 }
Mikebob 21:d553c43a5a26 72 if (n < 9) return;
Mikebob 26:072ab2309eec 73 TA[1] = timerA.read_us();
Mikebob 22:568a6d19b98a 74 TAA = (TA[1]-TA[0]);
Mikebob 22:568a6d19b98a 75 // Calculate speeed
Mikebob 22:568a6d19b98a 76 fA = 2.0f/ (TAA *(float)1.0E-6);
Mikebob 26:072ab2309eec 77 if(durA == 0) {
Mikebob 22:568a6d19b98a 78 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 79 durA++;
Mikebob 22:568a6d19b98a 80 return;
Mikebob 26:072ab2309eec 81 } else if(durA == 1) {
Mikebob 22:568a6d19b98a 82 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 83 durA++;
Mikebob 22:568a6d19b98a 84 return;
Mikebob 26:072ab2309eec 85 } else if(durA == 2) {
Mikebob 22:568a6d19b98a 86 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 87 durA = 0;
Mikebob 22:568a6d19b98a 88 }
Mikebob 26:072ab2309eec 89 for(int xA=0; xA<3; xA++) {
Mikebob 26:072ab2309eec 90 sumA+=speedA[xA];
Mikebob 22:568a6d19b98a 91 }
Mikebob 22:568a6d19b98a 92 sumA = sumA/3;
Mikebob 21:d553c43a5a26 93 //Reset count
Mikebob 21:d553c43a5a26 94 n=0;
Mikebob 26:072ab2309eec 95 }
Mikebob 18:11937e78239c 96
Mikebob 21:d553c43a5a26 97 void timeB()
Mikebob 21:d553c43a5a26 98 {
Mikebob 22:568a6d19b98a 99 static int nB=0; //Number of pulse sets
Mikebob 22:568a6d19b98a 100 static int HallStateB = 0;
Mikebob 21:d553c43a5a26 101 //**********************
Mikebob 21:d553c43a5a26 102 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 103 //**********************
Mikebob 22:568a6d19b98a 104 if (nB==0) {
Mikebob 21:d553c43a5a26 105 //Reset timer and Start
Mikebob 18:11937e78239c 106 timerB.reset();
Mikebob 26:072ab2309eec 107 timerB.start();
Mikebob 26:072ab2309eec 108 TB[0] = timerB.read_us();
Mikebob 21:d553c43a5a26 109 }
Mikebob 26:072ab2309eec 110 switch(HallStateB) {
Mikebob 26:072ab2309eec 111 case 0:
Mikebob 26:072ab2309eec 112 if(HEB1 == PULSE) {
Mikebob 26:072ab2309eec 113 HallStateB = 1;
Mikebob 18:11937e78239c 114 }
Mikebob 26:072ab2309eec 115 break;
Mikebob 26:072ab2309eec 116 case 1:
Mikebob 26:072ab2309eec 117 if(HEB1 == NOPULSE) {
Mikebob 26:072ab2309eec 118 HallStateB = 0;
Mikebob 26:072ab2309eec 119 nB++;
Mikebob 26:072ab2309eec 120 pulseB++;
Mikebob 26:072ab2309eec 121 travB = ((176/20.8)/3)*pulseB;
Mikebob 26:072ab2309eec 122 }
Mikebob 26:072ab2309eec 123 break;
Mikebob 26:072ab2309eec 124 }
Mikebob 22:568a6d19b98a 125 if (nB < 9) return;
Mikebob 26:072ab2309eec 126 TB[1] = timerB.read_us();
Mikebob 22:568a6d19b98a 127 TBB = (TB[1]-TB[0]);
Mikebob 22:568a6d19b98a 128 // Calculate speeed
Mikebob 22:568a6d19b98a 129 fB = 2.0f/ (TBB *(float)1.0E-6);
Mikebob 26:072ab2309eec 130 if(durB == 0) {
Mikebob 22:568a6d19b98a 131 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 132 durB++;
Mikebob 22:568a6d19b98a 133 return;
Mikebob 26:072ab2309eec 134 } else if(durB == 1) {
Mikebob 22:568a6d19b98a 135 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 136 durB++;
Mikebob 22:568a6d19b98a 137 return;
Mikebob 26:072ab2309eec 138 } else if(durB == 2) {
Mikebob 22:568a6d19b98a 139 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 140 durB = 0;
Mikebob 22:568a6d19b98a 141 }
Mikebob 26:072ab2309eec 142 for(int xB=0; xB<3; xB++) {
Mikebob 26:072ab2309eec 143 sumB+=speedB[xB];
Mikebob 22:568a6d19b98a 144 }
Mikebob 22:568a6d19b98a 145 sumB = sumB/3;
Mikebob 21:d553c43a5a26 146 //Reset count
Mikebob 22:568a6d19b98a 147 nB=0;
Mikebob 26:072ab2309eec 148 }
Mikebob 19:d3b82416df50 149
Mikebob 26:072ab2309eec 150 void oneRPS()
Mikebob 26:072ab2309eec 151 {
Mikebob 22:568a6d19b98a 152 float deltaA = 1.0f-sumA; //Error
Mikebob 22:568a6d19b98a 153 float deltaB = 1.0f-sumB;
Mikebob 26:072ab2309eec 154 dutyA = dutyA + deltaA*0.0001f; //Increase duty in proportion to the error
Mikebob 26:072ab2309eec 155 dutyB = dutyB + deltaB*0.0001f; //Increase duty in proportion to the error
Mikebob 22:568a6d19b98a 156 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 22:568a6d19b98a 157 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 22:568a6d19b98a 158 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 22:568a6d19b98a 159 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 22:568a6d19b98a 160 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 22:568a6d19b98a 161 //Update duty cycle to correct in the first direction
Mikebob 22:568a6d19b98a 162 PWMA.write(dutyA);
Mikebob 22:568a6d19b98a 163 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 164 }
Mikebob 26:072ab2309eec 165 void cornerRPS()
Mikebob 26:072ab2309eec 166 {
Mikebob 26:072ab2309eec 167 float deltaA = 1.2f-sumA; //Error
Mikebob 26:072ab2309eec 168 float deltaB = 0.8f-sumB;
Mikebob 26:072ab2309eec 169 dutyA = dutyA + deltaA*0.0001f; //Increase duty in proportion to the error
Mikebob 26:072ab2309eec 170 dutyB = dutyB + deltaB*0.0001f; //Increase duty in proportion to the error
Mikebob 24:b05cb3dd943e 171 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 25:50d3f80cb763 172 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 24:b05cb3dd943e 173 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 25:50d3f80cb763 174 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 24:b05cb3dd943e 175 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 24:b05cb3dd943e 176 //Update duty cycle to correct in the first direction
Mikebob 26:072ab2309eec 177 PWMA.write(dutyA);
Mikebob 26:072ab2309eec 178 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 179 }
Mikebob 26:072ab2309eec 180 void reset()
Mikebob 26:072ab2309eec 181 {
Mikebob 26:072ab2309eec 182 travA = 0;
Mikebob 26:072ab2309eec 183 travB = 0;
Mikebob 26:072ab2309eec 184 pulseA = 0;
Mikebob 26:072ab2309eec 185 pulseB = 0;
Mikebob 24:b05cb3dd943e 186 }
Mikebob 26:072ab2309eec 187 void straight()
Mikebob 26:072ab2309eec 188 {
Mikebob 26:072ab2309eec 189 dutyA = 0.463f;
Mikebob 26:072ab2309eec 190 dutyB = 0.457f;
Mikebob 26:072ab2309eec 191 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 192 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 193 }
Mikebob 26:072ab2309eec 194 void turn()
Mikebob 26:072ab2309eec 195 {
Mikebob 26:072ab2309eec 196 dutyA = 0.5556f;
Mikebob 26:072ab2309eec 197 dutyB = 0.28f;
Mikebob 26:072ab2309eec 198 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 199 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 200 }
Mikebob 14:e66cf781f5b9 201 int main()
Mikebob 14:e66cf781f5b9 202 {
Mikebob 19:d3b82416df50 203
Mikebob 14:e66cf781f5b9 204 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 205 terminal.baud(115200);
Mikebob 19:d3b82416df50 206
Mikebob 14:e66cf781f5b9 207 //Set initial motor direction
Mikebob 14:e66cf781f5b9 208 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 209 DIRB = FORWARD;
Mikebob 19:d3b82416df50 210
Mikebob 14:e66cf781f5b9 211 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 212 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 213 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 214
Mikebob 14:e66cf781f5b9 215 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 216 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 217 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 218
Mikebob 14:e66cf781f5b9 219 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 220 terminal.puts("Press USER button to start");
Mikebob 21:d553c43a5a26 221
Mikebob 14:e66cf781f5b9 222 while (SW1 == RELEASED);
Mikebob 21:d553c43a5a26 223 wait(0.5);
Mikebob 21:d553c43a5a26 224 //Wait - give time to start running
Mikebob 21:d553c43a5a26 225 wait(1.0);
Mikebob 18:11937e78239c 226 //Main polling loop
Mikebob 26:072ab2309eec 227 dutyA = 0.463f;
Mikebob 26:072ab2309eec 228 dutyB = 0.457f;
Mikebob 26:072ab2309eec 229 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 230 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 231 while(1) {
Mikebob 21:d553c43a5a26 232 timeA();
Mikebob 21:d553c43a5a26 233 timeB();
Mikebob 26:072ab2309eec 234 cornerRPS();
Mikebob 26:072ab2309eec 235 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 236 while(travA <= 1250) {
Mikebob 26:072ab2309eec 237 timeA();
Mikebob 26:072ab2309eec 238 timeB();
Mikebob 26:072ab2309eec 239 oneRPS();
Mikebob 26:072ab2309eec 240 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 241 }
Mikebob 22:568a6d19b98a 242 reset();
Mikebob 26:072ab2309eec 243 turn();
Mikebob 26:072ab2309eec 244 while(travA <= 597) {
Mikebob 26:072ab2309eec 245 timeA();
Mikebob 26:072ab2309eec 246 timeB();
Mikebob 26:072ab2309eec 247 cornerRPS();
Mikebob 26:072ab2309eec 248 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 249 }
Mikebob 22:568a6d19b98a 250 reset();
Mikebob 26:072ab2309eec 251 straight();
Mikebob 26:072ab2309eec 252 while(travA <= 1118) {
Mikebob 26:072ab2309eec 253 timeB();
Mikebob 26:072ab2309eec 254 timeA();
Mikebob 26:072ab2309eec 255 oneRPS();
Mikebob 26:072ab2309eec 256 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 257 }
Mikebob 26:072ab2309eec 258 reset();
Mikebob 26:072ab2309eec 259 turn();
Mikebob 26:072ab2309eec 260 while(travA <= 453.7) {
Mikebob 26:072ab2309eec 261 timeA();
Mikebob 26:072ab2309eec 262 timeB();
Mikebob 26:072ab2309eec 263 cornerRPS();
Mikebob 26:072ab2309eec 264 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 265 }
Mikebob 22:568a6d19b98a 266 reset();
Mikebob 26:072ab2309eec 267 straight();
Mikebob 26:072ab2309eec 268 while(travA <= 500) {
Mikebob 26:072ab2309eec 269 timeB();
Mikebob 26:072ab2309eec 270 timeA();
Mikebob 26:072ab2309eec 271 oneRPS();
Mikebob 26:072ab2309eec 272 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 273 }
Mikebob 22:568a6d19b98a 274 reset();
Mikebob 26:072ab2309eec 275 turn();
Mikebob 26:072ab2309eec 276 while(travA <= 350.29) {
Mikebob 26:072ab2309eec 277 timeA();
Mikebob 26:072ab2309eec 278 timeB();
Mikebob 26:072ab2309eec 279 cornerRPS();
Mikebob 26:072ab2309eec 280 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
Mikebob 26:072ab2309eec 281 }
Mikebob 22:568a6d19b98a 282 break;
Mikebob 14:e66cf781f5b9 283 }
Mikebob 26:072ab2309eec 284 PWMA.write(0.0f);
Mikebob 22:568a6d19b98a 285 PWMB.write(0.0f);
Mikebob 26:072ab2309eec 286
Mikebob 26:072ab2309eec 287 }