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

Committer:
Mikebob
Date:
Tue Dec 10 12:12:19 2019 +0000
Revision:
21:d553c43a5a26
Parent:
20:c89685cd0b02
Child:
22:568a6d19b98a
getting there 2.0

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 18:11937e78239c 36 float dutyA = 1.0f; //100%
Mikebob 18:11937e78239c 37 float dutyB = 1.0f; //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 14:e66cf781f5b9 43 float dis;
Mikebob 14:e66cf781f5b9 44 float trav =0;
Mikebob 21:d553c43a5a26 45 float speedA, speedB = 1.0f;
Mikebob 21:d553c43a5a26 46
Mikebob 21:d553c43a5a26 47 int pulse = 0;
Mikebob 21:d553c43a5a26 48
Mikebob 21:d553c43a5a26 49 void timeA()
Mikebob 21:d553c43a5a26 50 {
Mikebob 21:d553c43a5a26 51 static int n=0; //Number of pulse sets
Mikebob 21:d553c43a5a26 52 static int HallState = 0;
Mikebob 21:d553c43a5a26 53 static float T = 0.0f;
Mikebob 21:d553c43a5a26 54 //**********************
Mikebob 21:d553c43a5a26 55 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 56 //**********************
Mikebob 21:d553c43a5a26 57 if (n==0) {
Mikebob 21:d553c43a5a26 58 //Reset timer and Start
Mikebob 18:11937e78239c 59 timerA.reset();
Mikebob 21:d553c43a5a26 60 timerA.start();
Mikebob 21:d553c43a5a26 61 T = timerA.read_us();
Mikebob 21:d553c43a5a26 62 }
Mikebob 21:d553c43a5a26 63 switch(HallState)
Mikebob 18:11937e78239c 64 {
Mikebob 18:11937e78239c 65 case 0:
Mikebob 21:d553c43a5a26 66 if(HEA1 == PULSE){
Mikebob 21:d553c43a5a26 67 HallState = 1;
Mikebob 18:11937e78239c 68 }break;
Mikebob 18:11937e78239c 69 case 1:
Mikebob 21:d553c43a5a26 70 if(HEA2 == PULSE){
Mikebob 21:d553c43a5a26 71 HallState = 2;
Mikebob 18:11937e78239c 72 }break;
Mikebob 18:11937e78239c 73 case 2:
Mikebob 21:d553c43a5a26 74 if(HEA1 == NOPULSE){
Mikebob 21:d553c43a5a26 75 HallState = 3;
Mikebob 18:11937e78239c 76 }break;
Mikebob 18:11937e78239c 77 case 3:
Mikebob 21:d553c43a5a26 78 if(HEA2 == NOPULSE){
Mikebob 21:d553c43a5a26 79 HallState = 0;
Mikebob 21:d553c43a5a26 80 n++;
Mikebob 18:11937e78239c 81 }break;
Mikebob 18:11937e78239c 82 }
Mikebob 21:d553c43a5a26 83 if (n < 9) return;
Mikebob 21:d553c43a5a26 84 T = timerA.read_us();
Mikebob 21:d553c43a5a26 85 T = T;
Mikebob 21:d553c43a5a26 86 // Calculate speeed and adapt
Mikebob 21:d553c43a5a26 87 speedA = ((175.9/20.8)/3) / T;
Mikebob 21:d553c43a5a26 88 //Reset count
Mikebob 21:d553c43a5a26 89 n=0;
Mikebob 21:d553c43a5a26 90 }
Mikebob 18:11937e78239c 91
Mikebob 21:d553c43a5a26 92 void timeB()
Mikebob 21:d553c43a5a26 93 {
Mikebob 21:d553c43a5a26 94 static int n=0; //Number of pulse sets
Mikebob 21:d553c43a5a26 95 static int HallState = 0;
Mikebob 21:d553c43a5a26 96 static float TB = 0.0f;
Mikebob 21:d553c43a5a26 97 //**********************
Mikebob 21:d553c43a5a26 98 //TIME THE FULL SEQUENCE
Mikebob 21:d553c43a5a26 99 //**********************
Mikebob 21:d553c43a5a26 100 if (n==0) {
Mikebob 21:d553c43a5a26 101 //Reset timer and Start
Mikebob 18:11937e78239c 102 timerB.reset();
Mikebob 21:d553c43a5a26 103 timerB.start();
Mikebob 21:d553c43a5a26 104 TB = timerB.read_us();
Mikebob 21:d553c43a5a26 105 }
Mikebob 21:d553c43a5a26 106 switch(HallState)
Mikebob 18:11937e78239c 107 {
Mikebob 18:11937e78239c 108 case 0:
Mikebob 21:d553c43a5a26 109 if(HEB1 == PULSE){
Mikebob 21:d553c43a5a26 110 HallState = 1;
Mikebob 18:11937e78239c 111 }break;
Mikebob 18:11937e78239c 112 case 1:
Mikebob 21:d553c43a5a26 113 if(HEB2 == PULSE){
Mikebob 21:d553c43a5a26 114 HallState = 2;
Mikebob 18:11937e78239c 115 }break;
Mikebob 18:11937e78239c 116 case 2:
Mikebob 21:d553c43a5a26 117 if(HEB1 == NOPULSE){
Mikebob 21:d553c43a5a26 118 HallState = 3;
Mikebob 18:11937e78239c 119 }break;
Mikebob 18:11937e78239c 120 case 3:
Mikebob 21:d553c43a5a26 121 if(HEB2 == NOPULSE){
Mikebob 21:d553c43a5a26 122 HallState = 0;
Mikebob 21:d553c43a5a26 123 n++;
Mikebob 18:11937e78239c 124 }break;
Mikebob 18:11937e78239c 125 }
Mikebob 21:d553c43a5a26 126 if (n < 9) return;
Mikebob 21:d553c43a5a26 127 TB = timerB.read_us();
Mikebob 21:d553c43a5a26 128 TB = TB;
Mikebob 21:d553c43a5a26 129 // Calculate speeed and adapt
Mikebob 21:d553c43a5a26 130 speedB = ((175.9/20.8)/3) / TB;
Mikebob 21:d553c43a5a26 131 //Reset count
Mikebob 21:d553c43a5a26 132 n=0;
Mikebob 21:d553c43a5a26 133 }
Mikebob 19:d3b82416df50 134
Mikebob 21:d553c43a5a26 135
Mikebob 14:e66cf781f5b9 136 int main()
Mikebob 14:e66cf781f5b9 137 {
Mikebob 19:d3b82416df50 138
Mikebob 14:e66cf781f5b9 139 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 140 terminal.baud(115200);
Mikebob 19:d3b82416df50 141
Mikebob 14:e66cf781f5b9 142 //Set initial motor direction
Mikebob 14:e66cf781f5b9 143 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 144 DIRB = FORWARD;
Mikebob 19:d3b82416df50 145
Mikebob 14:e66cf781f5b9 146 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 147 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 148 PWMB.period_ms(10);
Mikebob 19:d3b82416df50 149
Mikebob 14:e66cf781f5b9 150 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 151 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 152 PWMB.write(0.0f); //0% duty cycle
Mikebob 19:d3b82416df50 153
Mikebob 14:e66cf781f5b9 154 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 155 terminal.puts("Press USER button to start");
Mikebob 21:d553c43a5a26 156
Mikebob 14:e66cf781f5b9 157 while (SW1 == RELEASED);
Mikebob 21:d553c43a5a26 158 wait(0.5);
Mikebob 21:d553c43a5a26 159 //Set align wheels
Mikebob 20:c89685cd0b02 160 PWMA.write(1.0f); //Set duty cycle (%)
Mikebob 20:c89685cd0b02 161 PWMB.write(1.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 162 //*********************************************************************
Mikebob 21:d553c43a5a26 163 //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)
Mikebob 21:d553c43a5a26 164 //*********************************************************************
Mikebob 19:d3b82416df50 165
Mikebob 21:d553c43a5a26 166 //Wait for rising edge of A1 and log time
Mikebob 21:d553c43a5a26 167 while (HEA1 == NOPULSE);
Mikebob 21:d553c43a5a26 168 //Wait for rising edge of A2 and log time (30 degrees?)
Mikebob 21:d553c43a5a26 169 while (HEA2 == NOPULSE);
Mikebob 21:d553c43a5a26 170 //Wait for falling edge of A1
Mikebob 21:d553c43a5a26 171 while (HEA1 == PULSE);
Mikebob 21:d553c43a5a26 172 //Wait for falling edge of A2
Mikebob 21:d553c43a5a26 173 while (HEA2 == PULSE);
Mikebob 21:d553c43a5a26 174 //Wait for rising edge of B1 and log time
Mikebob 21:d553c43a5a26 175 while (HEB1 == NOPULSE);
Mikebob 21:d553c43a5a26 176 //Wait for rising edge of B2 and log time (30 degrees?)
Mikebob 21:d553c43a5a26 177 while (HEB2 == NOPULSE);
Mikebob 21:d553c43a5a26 178 //Wait for falling edge of B1
Mikebob 21:d553c43a5a26 179 while (HEB1 == PULSE);
Mikebob 21:d553c43a5a26 180 //Wait for falling edge of B2
Mikebob 21:d553c43a5a26 181 while (HEB2 == PULSE);
Mikebob 21:d553c43a5a26 182
Mikebob 21:d553c43a5a26 183 //Set initial motor speed to stop
Mikebob 21:d553c43a5a26 184 PWMA.write(0.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 185 PWMB.write(0.0f); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 186
Mikebob 21:d553c43a5a26 187 //Wait - give time to start running
Mikebob 21:d553c43a5a26 188 wait(1.0);
Mikebob 18:11937e78239c 189 //Main polling loop
Mikebob 21:d553c43a5a26 190
Mikebob 21:d553c43a5a26 191 float paceA = 1.0f;
Mikebob 21:d553c43a5a26 192 float paceB = 1.0f;
Mikebob 21:d553c43a5a26 193 PWMA.write(paceA); //Set duty cycle (%)
Mikebob 21:d553c43a5a26 194 PWMB.write(paceB);
Mikebob 14:e66cf781f5b9 195 while(1)
Mikebob 14:e66cf781f5b9 196 {
Mikebob 21:d553c43a5a26 197 timeA();
Mikebob 21:d553c43a5a26 198 timeB();
Mikebob 21:d553c43a5a26 199 terminal.printf("wheelA: %4.2f \t wheelB: %4.2f \t \n\r", speedA, speedB);
Mikebob 21:d553c43a5a26 200 if (speedA <= 1.0f)
Mikebob 21:d553c43a5a26 201 {
Mikebob 21:d553c43a5a26 202 paceA +=0.005f;
Mikebob 21:d553c43a5a26 203 }
Mikebob 21:d553c43a5a26 204 if (speedA >= 1.0f)
Mikebob 21:d553c43a5a26 205 {
Mikebob 21:d553c43a5a26 206 paceA -=0.005f;
Mikebob 21:d553c43a5a26 207 }
Mikebob 21:d553c43a5a26 208 if (speedB <= 1.0f)
Mikebob 21:d553c43a5a26 209 {
Mikebob 21:d553c43a5a26 210 paceB +=0.005f;
Mikebob 21:d553c43a5a26 211 }
Mikebob 21:d553c43a5a26 212 if (speedB >= 1.0f)
Mikebob 21:d553c43a5a26 213 {
Mikebob 21:d553c43a5a26 214 paceB -=0.005f;
Mikebob 21:d553c43a5a26 215 }
Mikebob 21:d553c43a5a26 216 PWMA.write(paceA);
Mikebob 21:d553c43a5a26 217 PWMB.write(paceB);
Mikebob 14:e66cf781f5b9 218 }
Mikebob 21:d553c43a5a26 219 }