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@15:b7ca53716b05, 2019-11-19 (annotated)

Committer:
Mikebob
Date:
Tue Nov 19 19:49:13 2019 +0000
Revision:
15:b7ca53716b05
Parent:
14:e66cf781f5b9
Child:
16:afe936377d66
Triangle?

Who changed what in which revision?

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