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@29:3284cda80b4a, 2019-12-19 (annotated)

Committer:
Mikebob
Date:
Thu Dec 19 13:37:34 2019 +0000
Revision:
29:3284cda80b4a
Parent:
28:b650e7f6c269
Child:
30:9aeca82187f5
Just 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;
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 28:b650e7f6c269 59 if(HEA1 == PULSE) {
dparmenter 27:44ab9ebf07eb 60 HallState = 1; //change state
Mikebob 18:11937e78239c 61 }
Mikebob 26:072ab2309eec 62 break;
Mikebob 26:072ab2309eec 63 case 1:
Mikebob 28:b650e7f6c269 64 if(HEA1 == NOPULSE) {
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 }
Mikebob 28:b650e7f6c269 72 if (n < 4) return;
Mikebob 28:b650e7f6c269 73 TA[1] = timerA.read_us(); //reads timer at the end of one shaft rotation
Mikebob 28:b650e7f6c269 74 timerA.stop();
Mikebob 28:b650e7f6c269 75 TAA = (TA[1]-TA[0]); // time taken to do one shaft rotation
dparmenter 27:44ab9ebf07eb 76
Mikebob 28:b650e7f6c269 77 fA = 1.0f/ (TAA *(float)1.0E-6); //frequency of shaft rotation
Mikebob 28:b650e7f6c269 78 sumA = fA/20.8;
Mikebob 21:d553c43a5a26 79 //Reset count
Mikebob 28:b650e7f6c269 80 n=0;
Mikebob 26:072ab2309eec 81 }
Mikebob 28:b650e7f6c269 82 void timeB() // this funtion calulates the rotation speed and distance of wheel A
Mikebob 21:d553c43a5a26 83 {
Mikebob 22:568a6d19b98a 84 static int nB=0; //Number of pulse sets
Mikebob 28:b650e7f6c269 85 static int HallStateB = 0; //the hall effect current state
Mikebob 22:568a6d19b98a 86 if (nB==0) {
Mikebob 21:d553c43a5a26 87 //Reset timer and Start
Mikebob 28:b650e7f6c269 88 timerB.reset(); //resets timerA
Mikebob 28:b650e7f6c269 89 timerB.start(); // starts timerA
Mikebob 28:b650e7f6c269 90 TB[0] = timerB.read_us(); //reads timer from the beginning of the beginning of the first pulse
Mikebob 21:d553c43a5a26 91 }
Mikebob 26:072ab2309eec 92 switch(HallStateB) {
Mikebob 26:072ab2309eec 93 case 0:
Mikebob 26:072ab2309eec 94 if(HEB1 == PULSE) {
Mikebob 28:b650e7f6c269 95 HallStateB = 1; //change state
Mikebob 18:11937e78239c 96 }
Mikebob 26:072ab2309eec 97 break;
Mikebob 26:072ab2309eec 98 case 1:
Mikebob 26:072ab2309eec 99 if(HEB1 == NOPULSE) {
Mikebob 28:b650e7f6c269 100 HallStateB = 0; //change state
Mikebob 28:b650e7f6c269 101 nB++; // add 1 to the number of pulses counted (resets after 9 pulsees)
Mikebob 28:b650e7f6c269 102 pulseB++; // add 1 to the number of pulses counted for the duration
Mikebob 28:b650e7f6c269 103 travB = ((176/20.8)/3)*pulseB; // gives the distance travelled by wheel B in mm
Mikebob 26:072ab2309eec 104 }
Mikebob 26:072ab2309eec 105 break;
Mikebob 26:072ab2309eec 106 }
Mikebob 28:b650e7f6c269 107 if (nB < 4) return;
Mikebob 28:b650e7f6c269 108 TB[1] = timerB.read_us(); //reads timer at the end of one shaft rotation
Mikebob 28:b650e7f6c269 109 timerB.stop();
Mikebob 28:b650e7f6c269 110 TBB = (TB[1]-TB[0]); // time taken to do one shaft rotation
Mikebob 28:b650e7f6c269 111
Mikebob 28:b650e7f6c269 112 fB = 1.0f/ (TBB *(float)1.0E-6); //frequency of shaft rotation
Mikebob 28:b650e7f6c269 113 sumB = fB/20.8;
Mikebob 21:d553c43a5a26 114 //Reset count
Mikebob 28:b650e7f6c269 115 nB=0;
Mikebob 26:072ab2309eec 116 }
Mikebob 26:072ab2309eec 117 void oneRPS()
Mikebob 26:072ab2309eec 118 {
Mikebob 22:568a6d19b98a 119 float deltaA = 1.0f-sumA; //Error
Mikebob 22:568a6d19b98a 120 float deltaB = 1.0f-sumB;
Mikebob 28:b650e7f6c269 121 dutyA = dutyA + deltaA*0.01f; //Increase duty in proportion to the error
Mikebob 28:b650e7f6c269 122 dutyB = dutyB + deltaB*0.01f; //Increase duty in proportion to the error
Mikebob 22:568a6d19b98a 123 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 22:568a6d19b98a 124 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 22:568a6d19b98a 125 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 22:568a6d19b98a 126 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 22:568a6d19b98a 127 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 22:568a6d19b98a 128 //Update duty cycle to correct in the first direction
Mikebob 22:568a6d19b98a 129 PWMA.write(dutyA);
Mikebob 22:568a6d19b98a 130 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 131 }
Mikebob 26:072ab2309eec 132 void cornerRPS()
Mikebob 26:072ab2309eec 133 {
Mikebob 26:072ab2309eec 134 float deltaA = 1.2f-sumA; //Error
Mikebob 28:b650e7f6c269 135 float deltaB = 0.55f-sumB;
Mikebob 28:b650e7f6c269 136 dutyA = dutyA + deltaA*0.001f; //Increase duty in proportion to the error
Mikebob 28:b650e7f6c269 137 dutyB = dutyB + deltaB*0.001f; //Increase duty in proportion to the error
Mikebob 24:b05cb3dd943e 138 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 25:50d3f80cb763 139 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 24:b05cb3dd943e 140 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 25:50d3f80cb763 141 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 24:b05cb3dd943e 142 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 24:b05cb3dd943e 143 //Update duty cycle to correct in the first direction
Mikebob 26:072ab2309eec 144 PWMA.write(dutyA);
Mikebob 26:072ab2309eec 145 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 146 }
dparmenter 27:44ab9ebf07eb 147 void reset() // this fuction restes distnace travelled and pulses to 0 allowing fo a new distance measurement
Mikebob 26:072ab2309eec 148 {
Mikebob 26:072ab2309eec 149 travA = 0;
Mikebob 26:072ab2309eec 150 travB = 0;
Mikebob 26:072ab2309eec 151 pulseA = 0;
Mikebob 26:072ab2309eec 152 pulseB = 0;
Mikebob 24:b05cb3dd943e 153 }
dparmenter 27:44ab9ebf07eb 154 void straight() // this sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
Mikebob 26:072ab2309eec 155 {
Mikebob 26:072ab2309eec 156 dutyA = 0.463f;
Mikebob 26:072ab2309eec 157 dutyB = 0.457f;
Mikebob 26:072ab2309eec 158 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 159 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 160 }
dparmenter 27:44ab9ebf07eb 161 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 162 {
Mikebob 28:b650e7f6c269 163 dutyA = 0.556f;
Mikebob 29:3284cda80b4a 164 dutyB = 0.3f;
Mikebob 26:072ab2309eec 165 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 166 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 167 }
Mikebob 28:b650e7f6c269 168
Mikebob 14:e66cf781f5b9 169 int main()
Mikebob 14:e66cf781f5b9 170 {
Mikebob 19:d3b82416df50 171
Mikebob 14:e66cf781f5b9 172 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 173 terminal.baud(115200);
Mikebob 19:d3b82416df50 174
Mikebob 14:e66cf781f5b9 175 //Set initial motor direction
Mikebob 14:e66cf781f5b9 176 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 177 DIRB = FORWARD;
Mikebob 19:d3b82416df50 178
Mikebob 14:e66cf781f5b9 179 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 180 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 181 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 182
Mikebob 14:e66cf781f5b9 183 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 184 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 185 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 186
Mikebob 14:e66cf781f5b9 187 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 188 terminal.puts("Press USER button to start");
Mikebob 21:d553c43a5a26 189
Mikebob 14:e66cf781f5b9 190 while (SW1 == RELEASED);
Mikebob 21:d553c43a5a26 191 wait(0.5);
Mikebob 21:d553c43a5a26 192 //Wait - give time to start running
Mikebob 21:d553c43a5a26 193 wait(1.0);
Mikebob 18:11937e78239c 194 //Main polling loop
Mikebob 28:b650e7f6c269 195
Mikebob 26:072ab2309eec 196 while(1) {
Mikebob 28:b650e7f6c269 197 dutyA = 0.463f;
Mikebob 28:b650e7f6c269 198 dutyB = 0.457f;
Mikebob 28:b650e7f6c269 199 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 28:b650e7f6c269 200 PWMB.write(dutyB);
Mikebob 28:b650e7f6c269 201 oneRPS();
Mikebob 21:d553c43a5a26 202 timeA();
Mikebob 21:d553c43a5a26 203 timeB();
Mikebob 26:072ab2309eec 204 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 205 while(travA <= 1250) {
Mikebob 26:072ab2309eec 206 timeA();
Mikebob 26:072ab2309eec 207 timeB();
Mikebob 26:072ab2309eec 208 oneRPS();
Mikebob 26:072ab2309eec 209 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 210 }
Mikebob 22:568a6d19b98a 211 reset();
Mikebob 26:072ab2309eec 212 turn();
Mikebob 26:072ab2309eec 213 while(travA <= 597) {
Mikebob 26:072ab2309eec 214 timeA();
Mikebob 26:072ab2309eec 215 timeB();
Mikebob 26:072ab2309eec 216 cornerRPS();
Mikebob 26:072ab2309eec 217 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 218 }
Mikebob 22:568a6d19b98a 219 reset();
Mikebob 26:072ab2309eec 220 straight();
Mikebob 28:b650e7f6c269 221 while(travA <= 1457) {
Mikebob 26:072ab2309eec 222 timeB();
Mikebob 26:072ab2309eec 223 timeA();
Mikebob 26:072ab2309eec 224 oneRPS();
Mikebob 26:072ab2309eec 225 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 226 }
Mikebob 26:072ab2309eec 227 reset();
Mikebob 26:072ab2309eec 228 turn();
Mikebob 28:b650e7f6c269 229 while(travA <= 453.8) {
Mikebob 26:072ab2309eec 230 timeA();
Mikebob 26:072ab2309eec 231 timeB();
Mikebob 26:072ab2309eec 232 cornerRPS();
Mikebob 26:072ab2309eec 233 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 234 }
Mikebob 22:568a6d19b98a 235 reset();
Mikebob 26:072ab2309eec 236 straight();
Mikebob 28:b650e7f6c269 237 while(travA <= 750) {
Mikebob 26:072ab2309eec 238 timeB();
Mikebob 26:072ab2309eec 239 timeA();
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 28:b650e7f6c269 245 while(travA <= 349) {
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 break;
Mikebob 14:e66cf781f5b9 252 }
Mikebob 26:072ab2309eec 253 PWMA.write(0.0f);
Mikebob 22:568a6d19b98a 254 PWMB.write(0.0f);
Mikebob 26:072ab2309eec 255
Mikebob 26:072ab2309eec 256 }