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@20:c89685cd0b02, 2019-12-10 (annotated)

Committer:
Mikebob
Date:
Tue Dec 10 10:51:17 2019 +0000
Revision:
20:c89685cd0b02
Parent:
19:d3b82416df50
Child:
21:d553c43a5a26
Distance with switch case;

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 19:d3b82416df50 5
Mikebob 18:11937e78239c 6 //Status LED
Mikebob 18:11937e78239c 7 DigitalOut led(LED1);
Mikebob 19:d3b82416df50 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 19:d3b82416df50 12
Mikebob 14:e66cf781f5b9 13 //Motor Direction
Mikebob 14:e66cf781f5b9 14 DigitalOut DIRA(PA_9);
Mikebob 14:e66cf781f5b9 15 DigitalOut DIRB(PB_10);
Mikebob 19:d3b82416df50 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 19:d3b82416df50 23
Mikebob 14:e66cf781f5b9 24 //On board switch
Mikebob 14:e66cf781f5b9 25 DigitalIn SW1(USER_BUTTON);
Mikebob 19:d3b82416df50 26
Mikebob 14:e66cf781f5b9 27 //Use the serial object so we can use higher speeds
Mikebob 14:e66cf781f5b9 28 Serial terminal(USBTX, USBRX);
Mikebob 19:d3b82416df50 29
Mikebob 14:e66cf781f5b9 30 //Timer used for measuring speeds
Mikebob 18:11937e78239c 31 Timer timerA;
Mikebob 18:11937e78239c 32 Timer timerB;
Mikebob 19:d3b82416df50 33
Mikebob 14:e66cf781f5b9 34 //Enumerated types
Mikebob 14:e66cf781f5b9 35 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 14:e66cf781f5b9 36 enum PULSE {NOPULSE=0, PULSE};
Mikebob 14:e66cf781f5b9 37 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 19:d3b82416df50 38
Mikebob 18:11937e78239c 39 //Debug GPIO
Mikebob 18:11937e78239c 40 DigitalOut probe(D10);
Mikebob 19:d3b82416df50 41
Mikebob 14:e66cf781f5b9 42 //Duty cycles
Mikebob 18:11937e78239c 43 float dutyA = 1.0f; //100%
Mikebob 18:11937e78239c 44 float dutyB = 1.0f; //100%
Mikebob 18:11937e78239c 45 //Array of sensor data
Mikebob 18:11937e78239c 46 int tA1[2];
Mikebob 18:11937e78239c 47 int tA2[2];
Mikebob 18:11937e78239c 48 int tB1[2];
Mikebob 18:11937e78239c 49 int tB2[2];
Mikebob 14:e66cf781f5b9 50 float dis;
Mikebob 14:e66cf781f5b9 51 float trav =0;
Mikebob 19:d3b82416df50 52 float speedA, speedB = 0;
Mikebob 19:d3b82416df50 53 int countA, countB = 0;
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 19:d3b82416df50 91 countA++;
Mikebob 18:11937e78239c 92 //Calculate the frequency of rotation
Mikebob 18:11937e78239c 93 float TA1 = 2.0f * (tA1[1]-tA1[0]);
Mikebob 18:11937e78239c 94 float TA2 = 2.0f * (tA2[1]-tA2[0]);
Mikebob 18:11937e78239c 95 float TA = (TA1 + TA2) * 0.5f;
Mikebob 20:c89685cd0b02 96 trav = (20.8*countA);
Mikebob 18:11937e78239c 97 float fA = 1.0f/ (TA *(float)3.0E-6);
Mikebob 19:d3b82416df50 98 speedA = fA/20.8;
Mikebob 20:c89685cd0b02 99 terminal.printf("WheelA: %6.2f \t WheelB: %6.2f \t pulses: %d \t trav: %6.2f\n", speedA, speedB, countA, trav);
Mikebob 20:c89685cd0b02 100 if(countA >=48){
Mikebob 19:d3b82416df50 101 PWMA.write(0.0f);
Mikebob 19:d3b82416df50 102 PWMB.write(0.0f);
Mikebob 19:d3b82416df50 103 wait(10000);
Mikebob 19:d3b82416df50 104 }
Mikebob 18:11937e78239c 105 }
Mikebob 18:11937e78239c 106
Mikebob 18:11937e78239c 107 void HallB()
Mikebob 18:11937e78239c 108 {
Mikebob 18:11937e78239c 109 //Reset timer and Start
Mikebob 18:11937e78239c 110 timerB.reset();
Mikebob 18:11937e78239c 111 timerB.start();
Mikebob 18:11937e78239c 112 bool allB = true;
Mikebob 18:11937e78239c 113 //**********************
Mikebob 18:11937e78239c 114 //TIME THE FULL SEQUENCE
Mikebob 18:11937e78239c 115 //**********************
Mikebob 18:11937e78239c 116 int HallStateB = 0;
Mikebob 18:11937e78239c 117 while(allB)
Mikebob 18:11937e78239c 118 {
Mikebob 18:11937e78239c 119 switch(HallStateB)
Mikebob 18:11937e78239c 120 {
Mikebob 18:11937e78239c 121 case 0:
Mikebob 18:11937e78239c 122 if(HEB1 == NOPULSE){
Mikebob 18:11937e78239c 123 HallStateB = 1;
Mikebob 18:11937e78239c 124 tB1[0] = timerB.read_us();
Mikebob 18:11937e78239c 125 }break;
Mikebob 18:11937e78239c 126 case 1:
Mikebob 18:11937e78239c 127 if(HEB2 == NOPULSE){
Mikebob 18:11937e78239c 128 HallStateB = 2;
Mikebob 18:11937e78239c 129 tB2[0] = timerB.read_us();
Mikebob 18:11937e78239c 130 }break;
Mikebob 18:11937e78239c 131 case 2:
Mikebob 18:11937e78239c 132 if(HEB1 == PULSE){
Mikebob 18:11937e78239c 133 HallStateB = 3;
Mikebob 18:11937e78239c 134 tB1[1] = timerB.read_us();
Mikebob 18:11937e78239c 135 }break;
Mikebob 18:11937e78239c 136 case 3:
Mikebob 18:11937e78239c 137 if(HEB2 == PULSE){
Mikebob 18:11937e78239c 138 HallStateB = 0;
Mikebob 18:11937e78239c 139 allB = false;
Mikebob 19:d3b82416df50 140 countB++;
Mikebob 18:11937e78239c 141 tB2[1] = timerB.read_us();
Mikebob 18:11937e78239c 142 }break;
Mikebob 18:11937e78239c 143 }
Mikebob 18:11937e78239c 144 }
Mikebob 18:11937e78239c 145 //Calculate the frequency of rotation
Mikebob 18:11937e78239c 146 float TB1 = 2.0f * (tB1[1]-tB1[0]);
Mikebob 18:11937e78239c 147 float TB2 = 2.0f * (tB2[1]-tB2[0]);
Mikebob 18:11937e78239c 148 float TB = (TB1 + TB2) * 0.5f;
Mikebob 18:11937e78239c 149 float fB = 1.0f/ (TB *(float)3.0E-6);
Mikebob 19:d3b82416df50 150 speedB = fB/20.8;
Mikebob 18:11937e78239c 151 }
Mikebob 19:d3b82416df50 152
Mikebob 19:d3b82416df50 153 void oneRPS(){
Mikebob 19:d3b82416df50 154 float deltaA = 1.0f-speedA; //Error
Mikebob 19:d3b82416df50 155 float deltaB = 1.0f-speedB;
Mikebob 19:d3b82416df50 156 dutyA = dutyA + deltaA*0.1f; //Increase duty in proportion to the error
Mikebob 19:d3b82416df50 157 dutyB = dutyB + deltaB*0.1f; //Increase duty in proportion to the error
Mikebob 19:d3b82416df50 158 //Clamp the max and min values of duty and 0.0 and 1.0 respectively
Mikebob 19:d3b82416df50 159 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 19:d3b82416df50 160 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 19:d3b82416df50 161 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 19:d3b82416df50 162 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
Mikebob 19:d3b82416df50 163 //Update duty cycle to correct in the first direction
Mikebob 19:d3b82416df50 164 PWMA.write(dutyA);
Mikebob 19:d3b82416df50 165 PWMB.write(dutyB);
Mikebob 14:e66cf781f5b9 166 }
Mikebob 14:e66cf781f5b9 167 int main()
Mikebob 14:e66cf781f5b9 168 {
Mikebob 19:d3b82416df50 169
Mikebob 14:e66cf781f5b9 170 //Configure the terminal to high speed
Mikebob 19:d3b82416df50 171 terminal.baud(9600);
Mikebob 19:d3b82416df50 172
Mikebob 19:d3b82416df50 173 terminal.printf("Hello\n\r");
Mikebob 19:d3b82416df50 174
Mikebob 19:d3b82416df50 175
Mikebob 19:d3b82416df50 176
Mikebob 14:e66cf781f5b9 177 //Set initial motor direction
Mikebob 14:e66cf781f5b9 178 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 179 DIRB = FORWARD;
Mikebob 19:d3b82416df50 180
Mikebob 14:e66cf781f5b9 181 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 182 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 183 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 184
Mikebob 14:e66cf781f5b9 185 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 186 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 187 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 188
Mikebob 14:e66cf781f5b9 189 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 190 terminal.puts("Press USER button to start");
Mikebob 18:11937e78239c 191 led = 0;
Mikebob 14:e66cf781f5b9 192 while (SW1 == RELEASED);
Mikebob 18:11937e78239c 193 led = 1;
Mikebob 14:e66cf781f5b9 194 //Wait - give time to start running
Mikebob 14:e66cf781f5b9 195 wait(1.0);
Mikebob 19:d3b82416df50 196 //Set initial motor speed to stop
Mikebob 20:c89685cd0b02 197 PWMA.write(1.0f); //Set duty cycle (%)
Mikebob 20:c89685cd0b02 198 PWMB.write(1.0f); //Set duty cycle (%)
Mikebob 19:d3b82416df50 199
Mikebob 18:11937e78239c 200 //Main polling loop
Mikebob 14:e66cf781f5b9 201 while(1)
Mikebob 14:e66cf781f5b9 202 {
Mikebob 19:d3b82416df50 203 HallA();
Mikebob 19:d3b82416df50 204 HallB();
Mikebob 19:d3b82416df50 205 oneRPS();
Mikebob 14:e66cf781f5b9 206 }
Mikebob 19:d3b82416df50 207 PWMA.write(0.0f);
Mikebob 18:11937e78239c 208 PWMB.write(0.0f);
Mikebob 18:11937e78239c 209 }