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@27:44ab9ebf07eb, 2019-12-18 (annotated)

Committer:
dparmenter
Date:
Wed Dec 18 16:58:47 2019 +0000
Revision:
27:44ab9ebf07eb
Parent:
26:072ab2309eec
Child:
28:b650e7f6c269
90% commented mike needs to do shit

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;
dparmenter 27:44ab9ebf07eb 47 void timeA() // this funtion calulates the rotation speed and distance of wheel A
Mikebob 21:d553c43a5a26 48 {
Mikebob 21:d553c43a5a26 49 static int n=0; //Number of pulse sets
dparmenter 27:44ab9ebf07eb 50 static int HallState = 0; //the hall effect current state
Mikebob 21:d553c43a5a26 51 if (n==0) {
Mikebob 21:d553c43a5a26 52 //Reset timer and Start
dparmenter 27:44ab9ebf07eb 53 timerA.reset(); //resets timerA
dparmenter 27:44ab9ebf07eb 54 timerA.start(); // starts timerA
dparmenter 27:44ab9ebf07eb 55 TA[0] = timerA.read_us(); //reads timer from the beginning of the beginning of the first pulse
Mikebob 21:d553c43a5a26 56 }
Mikebob 26:072ab2309eec 57 switch(HallState) {
Mikebob 26:072ab2309eec 58 case 0:
Mikebob 26:072ab2309eec 59 if(HEA1 == NOPULSE) {
dparmenter 27:44ab9ebf07eb 60 HallState = 1; //change state
Mikebob 18:11937e78239c 61 }
Mikebob 26:072ab2309eec 62 break;
Mikebob 26:072ab2309eec 63 case 1:
Mikebob 26:072ab2309eec 64 if(HEA1 == PULSE) {
dparmenter 27:44ab9ebf07eb 65 HallState = 0; //change state
dparmenter 27:44ab9ebf07eb 66 n++; // add 1 to the number of pulses counted (resets after 9 pulsees)
dparmenter 27:44ab9ebf07eb 67 pulseA++; // add 1 to the number of pulses counted for the duration
dparmenter 27:44ab9ebf07eb 68 travA = ((176/20.8)/3)*pulseA; // gives the distance travelled by wheel A in mm
Mikebob 26:072ab2309eec 69 }
Mikebob 26:072ab2309eec 70 break;
Mikebob 26:072ab2309eec 71 }
dparmenter 27:44ab9ebf07eb 72 if (n < 9) return; //returns to main until n=9
dparmenter 27:44ab9ebf07eb 73 TA[1] = timerA.read_us(); // time after 9 pulses
dparmenter 27:44ab9ebf07eb 74 TAA = (TA[1]-TA[0]); // time taken to do 9 pulses (3 shaft rotations)
dparmenter 27:44ab9ebf07eb 75
dparmenter 27:44ab9ebf07eb 76 fA = 2.0f/ (TAA *(float)1.0E-6); //frequency of shaft rotation
dparmenter 27:44ab9ebf07eb 77 //calcultes speed
Mikebob 26:072ab2309eec 78 if(durA == 0) {
Mikebob 22:568a6d19b98a 79 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 80 durA++;
Mikebob 22:568a6d19b98a 81 return;
Mikebob 26:072ab2309eec 82 } else if(durA == 1) {
Mikebob 22:568a6d19b98a 83 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 84 durA++;
Mikebob 22:568a6d19b98a 85 return;
Mikebob 26:072ab2309eec 86 } else if(durA == 2) {
Mikebob 22:568a6d19b98a 87 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 88 durA = 0;
Mikebob 22:568a6d19b98a 89 }
Mikebob 26:072ab2309eec 90 for(int xA=0; xA<3; xA++) {
Mikebob 26:072ab2309eec 91 sumA+=speedA[xA];
Mikebob 22:568a6d19b98a 92 }
Mikebob 22:568a6d19b98a 93 sumA = sumA/3;
Mikebob 21:d553c43a5a26 94 //Reset count
dparmenter 27:44ab9ebf07eb 95 n=0; //note to mkike put this after each if statements
Mikebob 26:072ab2309eec 96 }
Mikebob 18:11937e78239c 97
Mikebob 21:d553c43a5a26 98 void timeB()
Mikebob 21:d553c43a5a26 99 {
Mikebob 22:568a6d19b98a 100 static int nB=0; //Number of pulse sets
Mikebob 22:568a6d19b98a 101 static int HallStateB = 0;
Mikebob 21:d553c43a5a26 102 //**********************
Mikebob 21:d553c43a5a26 103 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 104 //**********************
Mikebob 22:568a6d19b98a 105 if (nB==0) {
Mikebob 21:d553c43a5a26 106 //Reset timer and Start
Mikebob 18:11937e78239c 107 timerB.reset();
Mikebob 26:072ab2309eec 108 timerB.start();
Mikebob 26:072ab2309eec 109 TB[0] = timerB.read_us();
Mikebob 21:d553c43a5a26 110 }
Mikebob 26:072ab2309eec 111 switch(HallStateB) {
Mikebob 26:072ab2309eec 112 case 0:
Mikebob 26:072ab2309eec 113 if(HEB1 == PULSE) {
Mikebob 26:072ab2309eec 114 HallStateB = 1;
Mikebob 18:11937e78239c 115 }
Mikebob 26:072ab2309eec 116 break;
Mikebob 26:072ab2309eec 117 case 1:
Mikebob 26:072ab2309eec 118 if(HEB1 == NOPULSE) {
Mikebob 26:072ab2309eec 119 HallStateB = 0;
Mikebob 26:072ab2309eec 120 nB++;
Mikebob 26:072ab2309eec 121 pulseB++;
Mikebob 26:072ab2309eec 122 travB = ((176/20.8)/3)*pulseB;
Mikebob 26:072ab2309eec 123 }
Mikebob 26:072ab2309eec 124 break;
Mikebob 26:072ab2309eec 125 }
Mikebob 22:568a6d19b98a 126 if (nB < 9) return;
Mikebob 26:072ab2309eec 127 TB[1] = timerB.read_us();
Mikebob 22:568a6d19b98a 128 TBB = (TB[1]-TB[0]);
Mikebob 22:568a6d19b98a 129 // Calculate speeed
Mikebob 22:568a6d19b98a 130 fB = 2.0f/ (TBB *(float)1.0E-6);
Mikebob 26:072ab2309eec 131 if(durB == 0) {
Mikebob 22:568a6d19b98a 132 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 133 durB++;
Mikebob 22:568a6d19b98a 134 return;
Mikebob 26:072ab2309eec 135 } else if(durB == 1) {
Mikebob 22:568a6d19b98a 136 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 137 durB++;
Mikebob 22:568a6d19b98a 138 return;
Mikebob 26:072ab2309eec 139 } else if(durB == 2) {
Mikebob 22:568a6d19b98a 140 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 141 durB = 0;
Mikebob 22:568a6d19b98a 142 }
Mikebob 26:072ab2309eec 143 for(int xB=0; xB<3; xB++) {
Mikebob 26:072ab2309eec 144 sumB+=speedB[xB];
Mikebob 22:568a6d19b98a 145 }
Mikebob 22:568a6d19b98a 146 sumB = sumB/3;
Mikebob 21:d553c43a5a26 147 //Reset count
dparmenter 27:44ab9ebf07eb 148 nB=0; // note to mike put this in if statements
Mikebob 26:072ab2309eec 149 }
Mikebob 19:d3b82416df50 150
Mikebob 26:072ab2309eec 151 void oneRPS()
Mikebob 26:072ab2309eec 152 {
Mikebob 22:568a6d19b98a 153 float deltaA = 1.0f-sumA; //Error
Mikebob 22:568a6d19b98a 154 float deltaB = 1.0f-sumB;
Mikebob 26:072ab2309eec 155 dutyA = dutyA + deltaA*0.0001f; //Increase duty in proportion to the error
Mikebob 26:072ab2309eec 156 dutyB = dutyB + deltaB*0.0001f; //Increase duty in proportion to the error
Mikebob 22:568a6d19b98a 157 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 22:568a6d19b98a 158 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 22:568a6d19b98a 159 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 22:568a6d19b98a 160 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 22:568a6d19b98a 161 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 22:568a6d19b98a 162 //Update duty cycle to correct in the first direction
Mikebob 22:568a6d19b98a 163 PWMA.write(dutyA);
Mikebob 22:568a6d19b98a 164 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 165 }
Mikebob 26:072ab2309eec 166 void cornerRPS()
Mikebob 26:072ab2309eec 167 {
Mikebob 26:072ab2309eec 168 float deltaA = 1.2f-sumA; //Error
Mikebob 26:072ab2309eec 169 float deltaB = 0.8f-sumB;
Mikebob 26:072ab2309eec 170 dutyA = dutyA + deltaA*0.0001f; //Increase duty in proportion to the error
Mikebob 26:072ab2309eec 171 dutyB = dutyB + deltaB*0.0001f; //Increase duty in proportion to the error
Mikebob 24:b05cb3dd943e 172 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 25:50d3f80cb763 173 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 24:b05cb3dd943e 174 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 25:50d3f80cb763 175 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 24:b05cb3dd943e 176 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 24:b05cb3dd943e 177 //Update duty cycle to correct in the first direction
Mikebob 26:072ab2309eec 178 PWMA.write(dutyA);
Mikebob 26:072ab2309eec 179 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 180 }
dparmenter 27:44ab9ebf07eb 181 void reset() // this fuction restes distnace travelled and pulses to 0 allowing fo a new distance measurement
Mikebob 26:072ab2309eec 182 {
Mikebob 26:072ab2309eec 183 travA = 0;
Mikebob 26:072ab2309eec 184 travB = 0;
Mikebob 26:072ab2309eec 185 pulseA = 0;
Mikebob 26:072ab2309eec 186 pulseB = 0;
Mikebob 24:b05cb3dd943e 187 }
dparmenter 27:44ab9ebf07eb 188 void straight() // this sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
Mikebob 26:072ab2309eec 189 {
Mikebob 26:072ab2309eec 190 dutyA = 0.463f;
Mikebob 26:072ab2309eec 191 dutyB = 0.457f;
Mikebob 26:072ab2309eec 192 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 193 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 194 }
dparmenter 27:44ab9ebf07eb 195 void turn() // this sets the weheel speed to roughly what is needed to make the turn so that the program doesnt have to make any major adjustments
Mikebob 26:072ab2309eec 196 {
Mikebob 26:072ab2309eec 197 dutyA = 0.5556f;
Mikebob 26:072ab2309eec 198 dutyB = 0.28f;
Mikebob 26:072ab2309eec 199 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 200 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 201 }
Mikebob 14:e66cf781f5b9 202 int main()
Mikebob 14:e66cf781f5b9 203 {
Mikebob 19:d3b82416df50 204
Mikebob 14:e66cf781f5b9 205 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 206 terminal.baud(115200);
Mikebob 19:d3b82416df50 207
Mikebob 14:e66cf781f5b9 208 //Set initial motor direction
Mikebob 14:e66cf781f5b9 209 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 210 DIRB = FORWARD;
Mikebob 19:d3b82416df50 211
Mikebob 14:e66cf781f5b9 212 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 213 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 214 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 215
Mikebob 14:e66cf781f5b9 216 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 217 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 218 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 219
Mikebob 14:e66cf781f5b9 220 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 221 terminal.puts("Press USER button to start");
Mikebob 21:d553c43a5a26 222
Mikebob 14:e66cf781f5b9 223 while (SW1 == RELEASED);
Mikebob 21:d553c43a5a26 224 wait(0.5);
Mikebob 21:d553c43a5a26 225 //Wait - give time to start running
Mikebob 21:d553c43a5a26 226 wait(1.0);
Mikebob 18:11937e78239c 227 //Main polling loop
Mikebob 26:072ab2309eec 228 dutyA = 0.463f;
Mikebob 26:072ab2309eec 229 dutyB = 0.457f;
Mikebob 26:072ab2309eec 230 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 231 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 232 while(1) {
Mikebob 21:d553c43a5a26 233 timeA();
Mikebob 21:d553c43a5a26 234 timeB();
Mikebob 26:072ab2309eec 235 cornerRPS();
Mikebob 26:072ab2309eec 236 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 237 while(travA <= 1250) {
Mikebob 26:072ab2309eec 238 timeA();
Mikebob 26:072ab2309eec 239 timeB();
Mikebob 26:072ab2309eec 240 oneRPS();
Mikebob 26:072ab2309eec 241 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 242 }
Mikebob 22:568a6d19b98a 243 reset();
Mikebob 26:072ab2309eec 244 turn();
Mikebob 26:072ab2309eec 245 while(travA <= 597) {
Mikebob 26:072ab2309eec 246 timeA();
Mikebob 26:072ab2309eec 247 timeB();
Mikebob 26:072ab2309eec 248 cornerRPS();
Mikebob 26:072ab2309eec 249 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 250 }
Mikebob 22:568a6d19b98a 251 reset();
Mikebob 26:072ab2309eec 252 straight();
Mikebob 26:072ab2309eec 253 while(travA <= 1118) {
Mikebob 26:072ab2309eec 254 timeB();
Mikebob 26:072ab2309eec 255 timeA();
Mikebob 26:072ab2309eec 256 oneRPS();
Mikebob 26:072ab2309eec 257 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 258 }
Mikebob 26:072ab2309eec 259 reset();
Mikebob 26:072ab2309eec 260 turn();
Mikebob 26:072ab2309eec 261 while(travA <= 453.7) {
Mikebob 26:072ab2309eec 262 timeA();
Mikebob 26:072ab2309eec 263 timeB();
Mikebob 26:072ab2309eec 264 cornerRPS();
Mikebob 26:072ab2309eec 265 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 266 }
Mikebob 22:568a6d19b98a 267 reset();
Mikebob 26:072ab2309eec 268 straight();
Mikebob 26:072ab2309eec 269 while(travA <= 500) {
Mikebob 26:072ab2309eec 270 timeB();
Mikebob 26:072ab2309eec 271 timeA();
Mikebob 26:072ab2309eec 272 oneRPS();
Mikebob 26:072ab2309eec 273 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 274 }
Mikebob 22:568a6d19b98a 275 reset();
Mikebob 26:072ab2309eec 276 turn();
Mikebob 26:072ab2309eec 277 while(travA <= 350.29) {
Mikebob 26:072ab2309eec 278 timeA();
Mikebob 26:072ab2309eec 279 timeB();
Mikebob 26:072ab2309eec 280 cornerRPS();
Mikebob 26:072ab2309eec 281 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 282 }
Mikebob 22:568a6d19b98a 283 break;
Mikebob 14:e66cf781f5b9 284 }
Mikebob 26:072ab2309eec 285 PWMA.write(0.0f);
Mikebob 22:568a6d19b98a 286 PWMB.write(0.0f);
Mikebob 26:072ab2309eec 287
Mikebob 26:072ab2309eec 288 }