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@18:11937e78239c, 2019-12-03 (annotated)

Committer:
Mikebob
Date:
Tue Dec 03 16:39:25 2019 +0000
Revision:
18:11937e78239c
Parent:
17:a92d96b65cbc
Child:
19:d3b82416df50
Getting better, less shit than it was this morning;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mikebob 18:11937e78239c 1 /*
Mikebob 18:11937e78239c 2 Our version
Mikebob 18:11937e78239c 3 */
Mikebob 14:e66cf781f5b9 4 #include "mbed.h"
Mikebob 18:11937e78239c 5
Mikebob 18:11937e78239c 6 //Status LED
Mikebob 18:11937e78239c 7 DigitalOut led(LED1);
Mikebob 18:11937e78239c 8
Mikebob 14:e66cf781f5b9 9 //Motor PWM (speed)
Mikebob 14:e66cf781f5b9 10 PwmOut PWMA(PA_8);
Mikebob 14:e66cf781f5b9 11 PwmOut PWMB(PB_4);
Mikebob 18:11937e78239c 12
Mikebob 14:e66cf781f5b9 13 //Motor Direction
Mikebob 14:e66cf781f5b9 14 DigitalOut DIRA(PA_9);
Mikebob 14:e66cf781f5b9 15 DigitalOut DIRB(PB_10);
Mikebob 18:11937e78239c 16
Mikebob 14:e66cf781f5b9 17 //Hall-Effect Sensor Inputs
Mikebob 14:e66cf781f5b9 18 DigitalIn HEA1(PB_2);
Mikebob 14:e66cf781f5b9 19 DigitalIn HEA2(PB_1);
Mikebob 14:e66cf781f5b9 20 DigitalIn HEB1(PB_15);
Mikebob 14:e66cf781f5b9 21 DigitalIn HEB2(PB_14);
Mikebob 18:11937e78239c 22
Mikebob 18:11937e78239c 23
Mikebob 14:e66cf781f5b9 24 //On board switch
Mikebob 14:e66cf781f5b9 25 DigitalIn SW1(USER_BUTTON);
Mikebob 18:11937e78239c 26
Mikebob 14:e66cf781f5b9 27 //Use the serial object so we can use higher speeds
Mikebob 14:e66cf781f5b9 28 Serial terminal(USBTX, USBRX);
Mikebob 18:11937e78239c 29
Mikebob 14:e66cf781f5b9 30 //Timer used for measuring speeds
Mikebob 18:11937e78239c 31 Timer timerA;
Mikebob 18:11937e78239c 32 Timer timerB;
Mikebob 14:e66cf781f5b9 33 Timer timer1;
Mikebob 18:11937e78239c 34
Mikebob 14:e66cf781f5b9 35 //Enumerated types
Mikebob 14:e66cf781f5b9 36 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 14:e66cf781f5b9 37 enum PULSE {NOPULSE=0, PULSE};
Mikebob 14:e66cf781f5b9 38 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 18:11937e78239c 39
Mikebob 18:11937e78239c 40 //Debug GPIO
Mikebob 18:11937e78239c 41 DigitalOut probe(D10);
Mikebob 18:11937e78239c 42
Mikebob 14:e66cf781f5b9 43 //Duty cycles
Mikebob 18:11937e78239c 44 float dutyA = 1.0f; //100%
Mikebob 18:11937e78239c 45 float dutyB = 1.0f; //100%
Mikebob 18:11937e78239c 46 //Array of sensor data
Mikebob 18:11937e78239c 47 int tA1[2];
Mikebob 18:11937e78239c 48 int tA2[2];
Mikebob 18:11937e78239c 49 int tB1[2];
Mikebob 18:11937e78239c 50 int tB2[2];
Mikebob 14:e66cf781f5b9 51 float dis;
Mikebob 14:e66cf781f5b9 52 float trav =0;
Mikebob 18:11937e78239c 53
Mikebob 18:11937e78239c 54 void HallA()
Mikebob 18:11937e78239c 55 {
Mikebob 18:11937e78239c 56 //Reset timer and Start
Mikebob 18:11937e78239c 57 timerA.reset();
Mikebob 18:11937e78239c 58 timerA.start();
Mikebob 18:11937e78239c 59 bool all = true;
Mikebob 18:11937e78239c 60 //**********************
Mikebob 18:11937e78239c 61 //TIME THE FULL SEQUENCE
Mikebob 18:11937e78239c 62 //**********************
Mikebob 18:11937e78239c 63 int HallStateA = 0;
Mikebob 18:11937e78239c 64 while(all)
Mikebob 18:11937e78239c 65 {
Mikebob 18:11937e78239c 66 switch(HallStateA)
Mikebob 18:11937e78239c 67 {
Mikebob 18:11937e78239c 68 case 0:
Mikebob 18:11937e78239c 69 if(HEA1 == NOPULSE){
Mikebob 18:11937e78239c 70 HallStateA = 1;
Mikebob 18:11937e78239c 71 tA1[0] = timerA.read_us();
Mikebob 18:11937e78239c 72 }break;
Mikebob 18:11937e78239c 73 case 1:
Mikebob 18:11937e78239c 74 if(HEA2 == NOPULSE){
Mikebob 18:11937e78239c 75 HallStateA = 2;
Mikebob 18:11937e78239c 76 tA2[0] = timerA.read_us();
Mikebob 18:11937e78239c 77 }break;
Mikebob 18:11937e78239c 78 case 2:
Mikebob 18:11937e78239c 79 if(HEA1 == PULSE){
Mikebob 18:11937e78239c 80 HallStateA = 3;
Mikebob 18:11937e78239c 81 tA1[1] = timerA.read_us();
Mikebob 18:11937e78239c 82 }break;
Mikebob 18:11937e78239c 83 case 3:
Mikebob 18:11937e78239c 84 if(HEA2 == PULSE){
Mikebob 18:11937e78239c 85 HallStateA = 0;
Mikebob 18:11937e78239c 86 all = false;
Mikebob 18:11937e78239c 87 tA2[1] = timerA.read_us();
Mikebob 18:11937e78239c 88 }break;
Mikebob 18:11937e78239c 89 }
Mikebob 18:11937e78239c 90 }
Mikebob 18:11937e78239c 91
Mikebob 18:11937e78239c 92
Mikebob 18:11937e78239c 93 terminal.printf("tA1(0) = %d\n", tA1[0]);
Mikebob 18:11937e78239c 94 terminal.printf("tA1(1) = %d\n", tA1[1]);
Mikebob 18:11937e78239c 95 terminal.printf("tA2(0) = %d\n", tA2[0]);
Mikebob 18:11937e78239c 96 terminal.printf("tA2(1) = %d\n", tA2[1]);
Mikebob 18:11937e78239c 97
Mikebob 18:11937e78239c 98 //Calculate the frequency of rotation
Mikebob 18:11937e78239c 99 float TA1 = 2.0f * (tA1[1]-tA1[0]);
Mikebob 18:11937e78239c 100 float TA2 = 2.0f * (tA2[1]-tA2[0]);
Mikebob 18:11937e78239c 101 float TA = (TA1 + TA2) * 0.5f;
Mikebob 18:11937e78239c 102
Mikebob 18:11937e78239c 103 dis = timer1.read_us();
Mikebob 18:11937e78239c 104 float mm = ((TA*3)*20.8)/175.9;
Mikebob 18:11937e78239c 105 trav = dis/mm;
Mikebob 18:11937e78239c 106 float fA = 1.0f/ (TA *(float)3.0E-6);
Mikebob 18:11937e78239c 107 terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f \t trav: %6.2f\n", fA, fA/20.8f, trav);
Mikebob 18:11937e78239c 108 }
Mikebob 18:11937e78239c 109
Mikebob 18:11937e78239c 110 void HallB()
Mikebob 18:11937e78239c 111 {
Mikebob 18:11937e78239c 112 //Reset timer and Start
Mikebob 18:11937e78239c 113 timerB.reset();
Mikebob 18:11937e78239c 114 timerB.start();
Mikebob 18:11937e78239c 115 bool allB = true;
Mikebob 18:11937e78239c 116 //**********************
Mikebob 18:11937e78239c 117 //TIME THE FULL SEQUENCE
Mikebob 18:11937e78239c 118 //**********************
Mikebob 18:11937e78239c 119 int HallStateB = 0;
Mikebob 18:11937e78239c 120 while(allB)
Mikebob 18:11937e78239c 121 {
Mikebob 18:11937e78239c 122 switch(HallStateB)
Mikebob 18:11937e78239c 123 {
Mikebob 18:11937e78239c 124 case 0:
Mikebob 18:11937e78239c 125 if(HEB1 == NOPULSE){
Mikebob 18:11937e78239c 126 HallStateB = 1;
Mikebob 18:11937e78239c 127 tB1[0] = timerB.read_us();
Mikebob 18:11937e78239c 128 }break;
Mikebob 18:11937e78239c 129 case 1:
Mikebob 18:11937e78239c 130 if(HEB2 == NOPULSE){
Mikebob 18:11937e78239c 131 HallStateB = 2;
Mikebob 18:11937e78239c 132 tB2[0] = timerB.read_us();
Mikebob 18:11937e78239c 133 }break;
Mikebob 18:11937e78239c 134 case 2:
Mikebob 18:11937e78239c 135 if(HEB1 == PULSE){
Mikebob 18:11937e78239c 136 HallStateB = 3;
Mikebob 18:11937e78239c 137 tB1[1] = timerB.read_us();
Mikebob 18:11937e78239c 138 }break;
Mikebob 18:11937e78239c 139 case 3:
Mikebob 18:11937e78239c 140 if(HEB2 == PULSE){
Mikebob 18:11937e78239c 141 HallStateB = 0;
Mikebob 18:11937e78239c 142 allB = false;
Mikebob 18:11937e78239c 143 tB2[1] = timerB.read_us();
Mikebob 18:11937e78239c 144 }break;
Mikebob 18:11937e78239c 145 }
Mikebob 18:11937e78239c 146 }
Mikebob 18:11937e78239c 147
Mikebob 18:11937e78239c 148
Mikebob 18:11937e78239c 149 terminal.printf("tB1(0) = %d\n", tB1[0]);
Mikebob 18:11937e78239c 150 terminal.printf("tB1(1) = %d\n", tB1[1]);
Mikebob 18:11937e78239c 151 terminal.printf("tB2(0) = %d\n", tB2[0]);
Mikebob 18:11937e78239c 152 terminal.printf("tB2(1) = %d\n", tB2[1]);
Mikebob 18:11937e78239c 153
Mikebob 18:11937e78239c 154 //Calculate the frequency of rotation
Mikebob 18:11937e78239c 155 float TB1 = 2.0f * (tB1[1]-tB1[0]);
Mikebob 18:11937e78239c 156 float TB2 = 2.0f * (tB2[1]-tB2[0]);
Mikebob 18:11937e78239c 157 float TB = (TB1 + TB2) * 0.5f;
Mikebob 18:11937e78239c 158 float fB = 1.0f/ (TB *(float)3.0E-6);
Mikebob 18:11937e78239c 159 }
Mikebob 18:11937e78239c 160
Mikebob 18:11937e78239c 161 void reset()
Mikebob 17:a92d96b65cbc 162 {
Mikebob 18:11937e78239c 163 timer1.reset();
Mikebob 18:11937e78239c 164 HallA();
Mikebob 14:e66cf781f5b9 165 }
Mikebob 18:11937e78239c 166
Mikebob 14:e66cf781f5b9 167 int main()
Mikebob 14:e66cf781f5b9 168 {
Mikebob 18:11937e78239c 169
Mikebob 14:e66cf781f5b9 170 //Configure the terminal to high speed
Mikebob 18:11937e78239c 171 terminal.baud(115200);
Mikebob 18:11937e78239c 172
Mikebob 14:e66cf781f5b9 173 //Set initial motor direction
Mikebob 14:e66cf781f5b9 174 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 175 DIRB = FORWARD;
Mikebob 18:11937e78239c 176
Mikebob 14:e66cf781f5b9 177 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 178 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 179 PWMB.period_ms(10);
Mikebob 18:11937e78239c 180
Mikebob 14:e66cf781f5b9 181 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 182 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 183 PWMB.write(0.0f); //0% duty cycle
Mikebob 18:11937e78239c 184
Mikebob 14:e66cf781f5b9 185 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 186 terminal.puts("Press USER button to start");
Mikebob 18:11937e78239c 187 led = 0;
Mikebob 14:e66cf781f5b9 188 while (SW1 == RELEASED);
Mikebob 18:11937e78239c 189 led = 1;
Mikebob 18:11937e78239c 190
Mikebob 18:11937e78239c 191 //Set initial motor speed to stop
Mikebob 18:11937e78239c 192 PWMA.write(0.0f); //Set duty cycle (%)
Mikebob 18:11937e78239c 193 PWMB.write(0.0f); //Set duty cycle (%)
Mikebob 18:11937e78239c 194
Mikebob 14:e66cf781f5b9 195 //Wait - give time to start running
Mikebob 14:e66cf781f5b9 196 wait(1.0);
Mikebob 14:e66cf781f5b9 197 timer1.reset();
Mikebob 14:e66cf781f5b9 198 timer1.start();
Mikebob 18:11937e78239c 199 //Main polling loop
Mikebob 14:e66cf781f5b9 200 while(1)
Mikebob 14:e66cf781f5b9 201 {
Mikebob 18:11937e78239c 202 while(trav <= 1250)
Mikebob 18:11937e78239c 203 {
Mikebob 18:11937e78239c 204 PWMA.write(dutyA); //Set duty cycle y
Mikebob 18:11937e78239c 205 PWMB.write(dutyB);
Mikebob 18:11937e78239c 206 HallA();
Mikebob 18:11937e78239c 207 HallB();
Mikebob 18:11937e78239c 208 }
Mikebob 18:11937e78239c 209 reset();
Mikebob 18:11937e78239c 210 while(trav <= 330)
Mikebob 18:11937e78239c 211 {
Mikebob 18:11937e78239c 212 PWMA.write(dutyA);
Mikebob 18:11937e78239c 213 PWMB.write(0.0f);
Mikebob 18:11937e78239c 214 HallA();
Mikebob 18:11937e78239c 215 HallB();
Mikebob 18:11937e78239c 216 }
Mikebob 18:11937e78239c 217 reset();
Mikebob 18:11937e78239c 218 while(trav <= 1457)
Mikebob 18:11937e78239c 219 {
Mikebob 18:11937e78239c 220 PWMA.write(dutyA);
Mikebob 18:11937e78239c 221 PWMB.write(dutyB);
Mikebob 18:11937e78239c 222 HallA();
Mikebob 18:11937e78239c 223 HallB();
Mikebob 18:11937e78239c 224 }
Mikebob 18:11937e78239c 225 reset();
Mikebob 18:11937e78239c 226 while(trav <= 268)
Mikebob 18:11937e78239c 227 {
Mikebob 18:11937e78239c 228 PWMA.write(dutyA);
Mikebob 18:11937e78239c 229 PWMB.write(0.0f);
Mikebob 18:11937e78239c 230 HallA();
Mikebob 18:11937e78239c 231 HallB();
Mikebob 18:11937e78239c 232 }
Mikebob 18:11937e78239c 233 reset();
Mikebob 18:11937e78239c 234 while(trav <= 750)
Mikebob 18:11937e78239c 235 {
Mikebob 18:11937e78239c 236 PWMA.write(dutyA);
Mikebob 18:11937e78239c 237 PWMB.write(dutyB);
Mikebob 18:11937e78239c 238 HallA();
Mikebob 18:11937e78239c 239 HallB();
Mikebob 18:11937e78239c 240 }
Mikebob 18:11937e78239c 241 reset();
Mikebob 18:11937e78239c 242 while(trav <= 200)
Mikebob 18:11937e78239c 243 {
Mikebob 18:11937e78239c 244 PWMA.write(dutyA);
Mikebob 18:11937e78239c 245 PWMB.write(0.0f);
Mikebob 18:11937e78239c 246 HallA();
Mikebob 18:11937e78239c 247 HallB();
Mikebob 18:11937e78239c 248 }
Mikebob 18:11937e78239c 249 timerA.stop();
Mikebob 18:11937e78239c 250 timerB.stop();
Mikebob 18:11937e78239c 251 break;
Mikebob 14:e66cf781f5b9 252 }
Mikebob 18:11937e78239c 253 PWMA.write(0.0f);
Mikebob 18:11937e78239c 254 PWMB.write(0.0f);
Mikebob 18:11937e78239c 255 }
Mikebob 18:11937e78239c 256