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@25:50d3f80cb763, 2019-12-11 (annotated)

Committer:
Mikebob
Date:
Wed Dec 11 16:32:28 2019 +0000
Revision:
25:50d3f80cb763
Parent:
24:b05cb3dd943e
Child:
26:072ab2309eec
Done  but needs comments and maths

Who changed what in which revision?

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