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@22:568a6d19b98a, 2019-12-11 (annotated)

Committer:
Mikebob
Date:
Wed Dec 11 12:35:19 2019 +0000
Revision:
22:568a6d19b98a
Parent:
21:d553c43a5a26
Child:
24:b05cb3dd943e
Done!!!111!!!!!!11!!!!11!!

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 21:d553c43a5a26 53 void timeA()
Mikebob 21:d553c43a5a26 54 {
Mikebob 21:d553c43a5a26 55 static int n=0; //Number of pulse sets
Mikebob 21:d553c43a5a26 56 static int HallState = 0;
Mikebob 21:d553c43a5a26 57 //**********************
Mikebob 21:d553c43a5a26 58 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 59 //**********************
Mikebob 21:d553c43a5a26 60 if (n==0) {
Mikebob 21:d553c43a5a26 61 //Reset timer and Start
Mikebob 18:11937e78239c 62 timerA.reset();
Mikebob 21:d553c43a5a26 63 timerA.start();
Mikebob 22:568a6d19b98a 64 TA[0] = timerA.read_us();
Mikebob 21:d553c43a5a26 65 }
Mikebob 21:d553c43a5a26 66 switch(HallState)
Mikebob 18:11937e78239c 67 {
Mikebob 18:11937e78239c 68 case 0:
Mikebob 21:d553c43a5a26 69 if(HEA1 == PULSE){
Mikebob 21:d553c43a5a26 70 HallState = 1;
Mikebob 18:11937e78239c 71 }break;
Mikebob 18:11937e78239c 72 case 1:
Mikebob 21:d553c43a5a26 73 if(HEA1 == NOPULSE){
Mikebob 21:d553c43a5a26 74 HallState = 0;
Mikebob 21:d553c43a5a26 75 n++;
Mikebob 22:568a6d19b98a 76 pulse++;
Mikebob 22:568a6d19b98a 77 trav = ((176/20.8)/3)*pulse;
Mikebob 18:11937e78239c 78 }break;
Mikebob 18:11937e78239c 79 }
Mikebob 21:d553c43a5a26 80 if (n < 9) return;
Mikebob 22:568a6d19b98a 81 TA[1] = timerA.read_us();
Mikebob 22:568a6d19b98a 82 TAA = (TA[1]-TA[0]);
Mikebob 22:568a6d19b98a 83 // Calculate speeed
Mikebob 22:568a6d19b98a 84 fA = 2.0f/ (TAA *(float)1.0E-6);
Mikebob 22:568a6d19b98a 85 if(durA == 0){
Mikebob 22:568a6d19b98a 86 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 87 durA++;
Mikebob 22:568a6d19b98a 88 return;
Mikebob 22:568a6d19b98a 89 }
Mikebob 22:568a6d19b98a 90 else if(durA == 1){
Mikebob 22:568a6d19b98a 91 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 92 durA++;
Mikebob 22:568a6d19b98a 93 return;
Mikebob 22:568a6d19b98a 94 }
Mikebob 22:568a6d19b98a 95 else if(durA == 2){
Mikebob 22:568a6d19b98a 96 speedA[durA] = fA/20.8;
Mikebob 22:568a6d19b98a 97 durA = 0;
Mikebob 22:568a6d19b98a 98 }
Mikebob 22:568a6d19b98a 99 for(int xA=0;xA<3;xA++){
Mikebob 22:568a6d19b98a 100 sumA+=speedA[xA];
Mikebob 22:568a6d19b98a 101 }
Mikebob 22:568a6d19b98a 102 sumA = sumA/3;
Mikebob 21:d553c43a5a26 103 //Reset count
Mikebob 21:d553c43a5a26 104 n=0;
Mikebob 21:d553c43a5a26 105 }
Mikebob 18:11937e78239c 106
Mikebob 21:d553c43a5a26 107 void timeB()
Mikebob 21:d553c43a5a26 108 {
Mikebob 22:568a6d19b98a 109 static int nB=0; //Number of pulse sets
Mikebob 22:568a6d19b98a 110 static int HallStateB = 0;
Mikebob 21:d553c43a5a26 111 //**********************
Mikebob 21:d553c43a5a26 112 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 113 //**********************
Mikebob 22:568a6d19b98a 114 if (nB==0) {
Mikebob 21:d553c43a5a26 115 //Reset timer and Start
Mikebob 18:11937e78239c 116 timerB.reset();
Mikebob 21:d553c43a5a26 117 timerB.start();
Mikebob 22:568a6d19b98a 118 TB[0] = timerB.read_us();
Mikebob 21:d553c43a5a26 119 }
Mikebob 22:568a6d19b98a 120 switch(HallStateB)
Mikebob 18:11937e78239c 121 {
Mikebob 18:11937e78239c 122 case 0:
Mikebob 21:d553c43a5a26 123 if(HEB1 == PULSE){
Mikebob 22:568a6d19b98a 124 HallStateB = 1;
Mikebob 18:11937e78239c 125 }break;
Mikebob 18:11937e78239c 126 case 1:
Mikebob 21:d553c43a5a26 127 if(HEB1 == NOPULSE){
Mikebob 22:568a6d19b98a 128 HallStateB = 0;
Mikebob 22:568a6d19b98a 129 nB++;
Mikebob 18:11937e78239c 130 }break;
Mikebob 18:11937e78239c 131 }
Mikebob 22:568a6d19b98a 132 if (nB < 9) return;
Mikebob 22:568a6d19b98a 133 TB[1] = timerB.read_us();
Mikebob 22:568a6d19b98a 134 TBB = (TB[1]-TB[0]);
Mikebob 22:568a6d19b98a 135 // Calculate speeed
Mikebob 22:568a6d19b98a 136 fB = 2.0f/ (TBB *(float)1.0E-6);
Mikebob 22:568a6d19b98a 137 if(durB == 0){
Mikebob 22:568a6d19b98a 138 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 139 durB++;
Mikebob 22:568a6d19b98a 140 return;
Mikebob 22:568a6d19b98a 141 }
Mikebob 22:568a6d19b98a 142 else if(durB == 1){
Mikebob 22:568a6d19b98a 143 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 144 durB++;
Mikebob 22:568a6d19b98a 145 return;
Mikebob 22:568a6d19b98a 146 }
Mikebob 22:568a6d19b98a 147 else if(durB == 2){
Mikebob 22:568a6d19b98a 148 speedB[durB] = fB/20.8;
Mikebob 22:568a6d19b98a 149 durB = 0;
Mikebob 22:568a6d19b98a 150 }
Mikebob 22:568a6d19b98a 151 for(int xB=0;xB<3;xB++){
Mikebob 22:568a6d19b98a 152 sumB+=speedB[xB];
Mikebob 22:568a6d19b98a 153 }
Mikebob 22:568a6d19b98a 154 sumB = sumB/3;
Mikebob 21:d553c43a5a26 155 //Reset count
Mikebob 22:568a6d19b98a 156 nB=0;
Mikebob 21:d553c43a5a26 157 }
Mikebob 19:d3b82416df50 158
Mikebob 22:568a6d19b98a 159 void oneRPS(){
Mikebob 22:568a6d19b98a 160 float deltaA = 1.0f-sumA; //Error
Mikebob 22:568a6d19b98a 161 float deltaB = 1.0f-sumB;
Mikebob 22:568a6d19b98a 162 dutyA = dutyA + deltaA*0.001f; //Increase duty in proportion to the error
Mikebob 22:568a6d19b98a 163 dutyB = dutyB + deltaB*0.001f; //Increase duty in proportion to the error
Mikebob 22:568a6d19b98a 164 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 22:568a6d19b98a 165 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 22:568a6d19b98a 166 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 22:568a6d19b98a 167 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 22:568a6d19b98a 168 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 22:568a6d19b98a 169 //Update duty cycle to correct in the first direction
Mikebob 22:568a6d19b98a 170 PWMA.write(dutyA);
Mikebob 22:568a6d19b98a 171 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 172 }
Mikebob 22:568a6d19b98a 173 void reset(){
Mikebob 22:568a6d19b98a 174 trav = 0;
Mikebob 22:568a6d19b98a 175 pulse = 0;
Mikebob 22:568a6d19b98a 176 }
Mikebob 14:e66cf781f5b9 177 int main()
Mikebob 14:e66cf781f5b9 178 {
Mikebob 19:d3b82416df50 179
Mikebob 14:e66cf781f5b9 180 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 181 terminal.baud(115200);
Mikebob 19:d3b82416df50 182
Mikebob 14:e66cf781f5b9 183 //Set initial motor direction
Mikebob 14:e66cf781f5b9 184 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 185 DIRB = FORWARD;
Mikebob 19:d3b82416df50 186
Mikebob 14:e66cf781f5b9 187 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 188 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 189 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 190
Mikebob 14:e66cf781f5b9 191 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 192 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 193 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 194
Mikebob 14:e66cf781f5b9 195 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 196 terminal.puts("Press USER button to start");
Mikebob 21:d553c43a5a26 197
Mikebob 14:e66cf781f5b9 198 while (SW1 == RELEASED);
Mikebob 21:d553c43a5a26 199 wait(0.5);
Mikebob 21:d553c43a5a26 200 //Set align wheels
Mikebob 20:c89685cd0b02 201 PWMA.write(1.0f); //Set duty cycle (%)
Mikebob 20:c89685cd0b02 202 PWMB.write(1.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 203 //*********************************************************************
Mikebob 21:d553c43a5a26 204 //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)
Mikebob 21:d553c43a5a26 205 //*********************************************************************
Mikebob 19:d3b82416df50 206
Mikebob 21:d553c43a5a26 207 //Wait for rising edge of A1 and log time
Mikebob 21:d553c43a5a26 208 while (HEA1 == NOPULSE);
Mikebob 21:d553c43a5a26 209 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 21:d553c43a5a26 210 while (HEA2 == NOPULSE);
Mikebob 21:d553c43a5a26 211 //Wait for falling edge of A1
Mikebob 21:d553c43a5a26 212 while (HEA1 == PULSE);
Mikebob 21:d553c43a5a26 213 //Wait for falling edge of A2
Mikebob 21:d553c43a5a26 214 while (HEA2 == PULSE);
Mikebob 21:d553c43a5a26 215 //Wait for rising edge of B1 and log time
Mikebob 21:d553c43a5a26 216 while (HEB1 == NOPULSE);
Mikebob 21:d553c43a5a26 217 //Wait for rising edge of B2 and log time (30 degrees?)
Mikebob 21:d553c43a5a26 218 while (HEB2 == NOPULSE);
Mikebob 21:d553c43a5a26 219 //Wait for falling edge of B1
Mikebob 21:d553c43a5a26 220 while (HEB1 == PULSE);
Mikebob 21:d553c43a5a26 221 //Wait for falling edge of B2
Mikebob 21:d553c43a5a26 222 while (HEB2 == PULSE);
Mikebob 21:d553c43a5a26 223
Mikebob 21:d553c43a5a26 224 //Set initial motor speed to stop
Mikebob 21:d553c43a5a26 225 PWMA.write(0.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 226 PWMB.write(0.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 227
Mikebob 21:d553c43a5a26 228 //Wait - give time to start running
Mikebob 21:d553c43a5a26 229 wait(1.0);
Mikebob 18:11937e78239c 230 //Main polling loop
Mikebob 22:568a6d19b98a 231 PWMA.write(0.2f); //Set duty cycle (%)
Mikebob 22:568a6d19b98a 232 PWMB.write(0.2f);
Mikebob 14:e66cf781f5b9 233 while(1)
Mikebob 14:e66cf781f5b9 234 {
Mikebob 21:d553c43a5a26 235 timeA();
Mikebob 21:d553c43a5a26 236 timeB();
Mikebob 22:568a6d19b98a 237 oneRPS();
Mikebob 22:568a6d19b98a 238 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 239 while(trav <= 1250)
Mikebob 22:568a6d19b98a 240 {
Mikebob 22:568a6d19b98a 241 timeA();
Mikebob 22:568a6d19b98a 242 timeB();
Mikebob 22:568a6d19b98a 243 oneRPS();
Mikebob 22:568a6d19b98a 244 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 245 }
Mikebob 22:568a6d19b98a 246 reset();
Mikebob 22:568a6d19b98a 247 while(trav <= 330)
Mikebob 22:568a6d19b98a 248 {
Mikebob 22:568a6d19b98a 249 timeA();
Mikebob 22:568a6d19b98a 250 PWMB.write(0.0f);
Mikebob 22:568a6d19b98a 251 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 252 }
Mikebob 22:568a6d19b98a 253 reset();
Mikebob 22:568a6d19b98a 254 while(trav <= 1457)
Mikebob 22:568a6d19b98a 255 {
Mikebob 22:568a6d19b98a 256 timeB();
Mikebob 22:568a6d19b98a 257 timeA();
Mikebob 22:568a6d19b98a 258 oneRPS();
Mikebob 22:568a6d19b98a 259 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 260 }
Mikebob 22:568a6d19b98a 261 reset();
Mikebob 22:568a6d19b98a 262 while(trav <= 278.5)
Mikebob 22:568a6d19b98a 263 {
Mikebob 22:568a6d19b98a 264 timeA();
Mikebob 22:568a6d19b98a 265 PWMB.write(0.0f);
Mikebob 22:568a6d19b98a 266 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 267 }
Mikebob 22:568a6d19b98a 268 reset();
Mikebob 22:568a6d19b98a 269 while(trav <= 750)
Mikebob 22:568a6d19b98a 270 {
Mikebob 22:568a6d19b98a 271 timeB();
Mikebob 22:568a6d19b98a 272 timeA();
Mikebob 22:568a6d19b98a 273 oneRPS();
Mikebob 22:568a6d19b98a 274 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 275 }
Mikebob 22:568a6d19b98a 276 reset();
Mikebob 22:568a6d19b98a 277 while(trav <= 200)
Mikebob 22:568a6d19b98a 278 {
Mikebob 22:568a6d19b98a 279 timeA();
Mikebob 22:568a6d19b98a 280 PWMB.write(0.0f);
Mikebob 22:568a6d19b98a 281 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
Mikebob 22:568a6d19b98a 282 }
Mikebob 22:568a6d19b98a 283 break;
Mikebob 14:e66cf781f5b9 284 }
Mikebob 22:568a6d19b98a 285 PWMA.write(0.0f);
Mikebob 22:568a6d19b98a 286 PWMB.write(0.0f);
Mikebob 22:568a6d19b98a 287
Mikebob 22:568a6d19b98a 288 }