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@31:19bb96d3113e, 2020-01-03 (annotated)

Committer:
jaffacat
Date:
Fri Jan 03 16:36:19 2020 +0000
Revision:
31:19bb96d3113e
Parent:
30:9aeca82187f5
Child:
32:b7a08eb0408d
changed 9 to 4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mikebob 26:072ab2309eec 1 //Enhancement 3//
Mikebob 21:d553c43a5a26 2
Mikebob 14:e66cf781f5b9 3 #include "mbed.h"
Mikebob 14:e66cf781f5b9 4 //Motor PWM (speed)
Mikebob 14:e66cf781f5b9 5 PwmOut PWMA(PA_8);
Mikebob 14:e66cf781f5b9 6 PwmOut PWMB(PB_4);
Mikebob 14:e66cf781f5b9 7 //Motor Direction
Mikebob 14:e66cf781f5b9 8 DigitalOut DIRA(PA_9);
Mikebob 14:e66cf781f5b9 9 DigitalOut DIRB(PB_10);
Mikebob 14:e66cf781f5b9 10 //Hall-Effect Sensor Inputs
Mikebob 14:e66cf781f5b9 11 DigitalIn HEA1(PB_2);
Mikebob 14:e66cf781f5b9 12 DigitalIn HEA2(PB_1);
Mikebob 14:e66cf781f5b9 13 DigitalIn HEB1(PB_15);
Mikebob 14:e66cf781f5b9 14 DigitalIn HEB2(PB_14);
Mikebob 14:e66cf781f5b9 15 //On board switch
Mikebob 14:e66cf781f5b9 16 DigitalIn SW1(USER_BUTTON);
Mikebob 14:e66cf781f5b9 17 //Use the serial object so we can use higher speeds
Mikebob 14:e66cf781f5b9 18 Serial terminal(USBTX, USBRX);
Mikebob 14:e66cf781f5b9 19 //Timer used for measuring speeds
Mikebob 18:11937e78239c 20 Timer timerA;
Mikebob 18:11937e78239c 21 Timer timerB;
Mikebob 14:e66cf781f5b9 22 //Enumerated types
Mikebob 14:e66cf781f5b9 23 enum DIRECTION {FORWARD=0, REVERSE};
Mikebob 14:e66cf781f5b9 24 enum PULSE {NOPULSE=0, PULSE};
Mikebob 14:e66cf781f5b9 25 enum SWITCHSTATE {PRESSED=0, RELEASED};
Mikebob 14:e66cf781f5b9 26 //Duty cycles
dparmenter 30:9aeca82187f5 27 float dutyA, dutyB = 0.0f;
dparmenter 30:9aeca82187f5 28 //Speed Calculations
Mikebob 22:568a6d19b98a 29 float fA, fB = 0.0f;
dparmenter 30:9aeca82187f5 30 float speedA, speedB = 0.0f;
Mikebob 22:568a6d19b98a 31 float TA[2];
Mikebob 22:568a6d19b98a 32 float TB[2];
Mikebob 22:568a6d19b98a 33 float TAA, TBB = 0.0f;
dparmenter 30:9aeca82187f5 34 //Distance Calculations
Mikebob 26:072ab2309eec 35 float pulseA, pulseB = 0.0f;
Mikebob 26:072ab2309eec 36 float travA, travB = 0.0f;
dparmenter 30:9aeca82187f5 37
dparmenter 27:44ab9ebf07eb 38 void timeA() // this funtion calulates the rotation speed and distance of wheel A
Mikebob 21:d553c43a5a26 39 {
Mikebob 21:d553c43a5a26 40 static int n=0; //Number of pulse sets
dparmenter 27:44ab9ebf07eb 41 static int HallState = 0; //the hall effect current state
Mikebob 21:d553c43a5a26 42 if (n==0) {
Mikebob 21:d553c43a5a26 43 //Reset timer and Start
dparmenter 30:9aeca82187f5 44 timerA.reset(); //Resets timerA
dparmenter 30:9aeca82187f5 45 timerA.start(); //Starts timerA
dparmenter 30:9aeca82187f5 46 TA[0] = timerA.read_us(); //Reads timer from the beginning of the first pulse
Mikebob 21:d553c43a5a26 47 }
Mikebob 26:072ab2309eec 48 switch(HallState) {
Mikebob 26:072ab2309eec 49 case 0:
Mikebob 28:b650e7f6c269 50 if(HEA1 == PULSE) {
dparmenter 30:9aeca82187f5 51 HallState = 1; //Change state
Mikebob 18:11937e78239c 52 }
Mikebob 26:072ab2309eec 53 break;
Mikebob 26:072ab2309eec 54 case 1:
Mikebob 28:b650e7f6c269 55 if(HEA1 == NOPULSE) {
dparmenter 30:9aeca82187f5 56 HallState = 0; //Change state
jaffacat 31:19bb96d3113e 57 n++; //Add 1 to the number of pulses counted (resets after 4 pulsees)
dparmenter 30:9aeca82187f5 58 pulseA++; //Add 1 to the number of pulses counted for the duration
dparmenter 30:9aeca82187f5 59 travA = ((176/20.8)/3)*pulseA; //Gives the distance travelled by wheel A in mm
Mikebob 26:072ab2309eec 60 }
Mikebob 26:072ab2309eec 61 break;
Mikebob 26:072ab2309eec 62 }
Mikebob 28:b650e7f6c269 63 if (n < 4) return;
dparmenter 30:9aeca82187f5 64 TA[1] = timerA.read_us(); //Reads timer at the end of one shaft rotation
Mikebob 28:b650e7f6c269 65 timerA.stop();
dparmenter 30:9aeca82187f5 66 TAA = (TA[1]-TA[0]); //Time taken to do one shaft rotation
dparmenter 27:44ab9ebf07eb 67
dparmenter 30:9aeca82187f5 68 fA = 1.0f/ (TAA *(float)1.0E-6); //Frequency of shaft rotation
dparmenter 30:9aeca82187f5 69 speedA = fA/20.8; //wheel RPS
Mikebob 21:d553c43a5a26 70 //Reset count
Mikebob 28:b650e7f6c269 71 n=0;
Mikebob 26:072ab2309eec 72 }
dparmenter 30:9aeca82187f5 73 void timeB() //This funtion calulates the rotation speed and distance of wheel A
Mikebob 21:d553c43a5a26 74 {
Mikebob 22:568a6d19b98a 75 static int nB=0; //Number of pulse sets
dparmenter 30:9aeca82187f5 76 static int HallStateB = 0; //The hall effect current state
Mikebob 22:568a6d19b98a 77 if (nB==0) {
Mikebob 21:d553c43a5a26 78 //Reset timer and Start
dparmenter 30:9aeca82187f5 79 timerB.reset(); //Resets timerB
dparmenter 30:9aeca82187f5 80 timerB.start(); //Starts timerB
dparmenter 30:9aeca82187f5 81 TB[0] = timerB.read_us(); //Reads time from the beginning of the first pulse
Mikebob 21:d553c43a5a26 82 }
Mikebob 26:072ab2309eec 83 switch(HallStateB) {
Mikebob 26:072ab2309eec 84 case 0:
Mikebob 26:072ab2309eec 85 if(HEB1 == PULSE) {
dparmenter 30:9aeca82187f5 86 HallStateB = 1; //Change state
Mikebob 18:11937e78239c 87 }
Mikebob 26:072ab2309eec 88 break;
Mikebob 26:072ab2309eec 89 case 1:
Mikebob 26:072ab2309eec 90 if(HEB1 == NOPULSE) {
dparmenter 30:9aeca82187f5 91 HallStateB = 0; //Change state
jaffacat 31:19bb96d3113e 92 nB++; //Add 1 to the number of pulses counted (resets after 4 pulsees)
dparmenter 30:9aeca82187f5 93 pulseB++; //Add 1 to the number of pulses counted for the duration
dparmenter 30:9aeca82187f5 94 travB = ((176/20.8)/3)*pulseB; //Gives the distance travelled by wheel B in mm
Mikebob 26:072ab2309eec 95 }
Mikebob 26:072ab2309eec 96 break;
Mikebob 26:072ab2309eec 97 }
Mikebob 28:b650e7f6c269 98 if (nB < 4) return;
dparmenter 30:9aeca82187f5 99 TB[1] = timerB.read_us(); //Reads timer at the end of one shaft rotation
Mikebob 28:b650e7f6c269 100 timerB.stop();
dparmenter 30:9aeca82187f5 101 TBB = (TB[1]-TB[0]); //Time taken to do one shaft rotation
dparmenter 30:9aeca82187f5 102 fB = 1.0f/ (TBB *(float)1.0E-6); //Frequency of shaft rotation
dparmenter 30:9aeca82187f5 103 speedB = fB/20.8; //Wheel RPS
Mikebob 21:d553c43a5a26 104 //Reset count
dparmenter 30:9aeca82187f5 105 nB=0;
Mikebob 26:072ab2309eec 106 }
Mikebob 26:072ab2309eec 107 void oneRPS()
Mikebob 26:072ab2309eec 108 {
dparmenter 30:9aeca82187f5 109 float deltaA = 1.0f-speedA; //Error
dparmenter 30:9aeca82187f5 110 float deltaB = 1.0f-speedB; //Error
Mikebob 28:b650e7f6c269 111 dutyA = dutyA + deltaA*0.01f; //Increase duty in proportion to the error
Mikebob 28:b650e7f6c269 112 dutyB = dutyB + deltaB*0.01f; //Increase duty in proportion to the error
dparmenter 30:9aeca82187f5 113 //Lock the max and min values of duty
Mikebob 22:568a6d19b98a 114 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 22:568a6d19b98a 115 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 22:568a6d19b98a 116 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 22:568a6d19b98a 117 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
dparmenter 30:9aeca82187f5 118 //Update duty cycle to new value
Mikebob 22:568a6d19b98a 119 PWMA.write(dutyA);
Mikebob 22:568a6d19b98a 120 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 121 }
Mikebob 26:072ab2309eec 122 void cornerRPS()
Mikebob 26:072ab2309eec 123 {
dparmenter 30:9aeca82187f5 124 float deltaA = 1.2f-speedA; //Error
dparmenter 30:9aeca82187f5 125 float deltaB = 0.55f-speedB; //Error
Mikebob 28:b650e7f6c269 126 dutyA = dutyA + deltaA*0.001f; //Increase duty in proportion to the error
Mikebob 28:b650e7f6c269 127 dutyB = dutyB + deltaB*0.001f; //Increase duty in proportion to the error
dparmenter 30:9aeca82187f5 128 //Lock the maximum and minimum values of duty
Mikebob 25:50d3f80cb763 129 dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
Mikebob 24:b05cb3dd943e 130 dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
Mikebob 25:50d3f80cb763 131 dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
Mikebob 24:b05cb3dd943e 132 dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
dparmenter 30:9aeca82187f5 133 //Update duty cycle to new value
Mikebob 26:072ab2309eec 134 PWMA.write(dutyA);
Mikebob 26:072ab2309eec 135 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 136 }
dparmenter 30:9aeca82187f5 137 void reset() //This fuction resets the distnace travelled and pulses to 0 allowing fo a new distance measurement
Mikebob 26:072ab2309eec 138 {
Mikebob 26:072ab2309eec 139 travA = 0;
Mikebob 26:072ab2309eec 140 travB = 0;
Mikebob 26:072ab2309eec 141 pulseA = 0;
Mikebob 26:072ab2309eec 142 pulseB = 0;
Mikebob 24:b05cb3dd943e 143 }
dparmenter 30:9aeca82187f5 144 void straight() //This sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
Mikebob 26:072ab2309eec 145 {
Mikebob 26:072ab2309eec 146 dutyA = 0.463f;
Mikebob 26:072ab2309eec 147 dutyB = 0.457f;
Mikebob 26:072ab2309eec 148 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 149 PWMB.write(dutyB);
Mikebob 26:072ab2309eec 150 }
dparmenter 30:9aeca82187f5 151 void turn() //This sets the wheel speed to roughly what is needed to make the turn so that the program doesnt have to make any major adjustments
Mikebob 26:072ab2309eec 152 {
Mikebob 28:b650e7f6c269 153 dutyA = 0.556f;
Mikebob 29:3284cda80b4a 154 dutyB = 0.3f;
Mikebob 26:072ab2309eec 155 PWMA.write(dutyA); //Set duty cycle (%)
Mikebob 26:072ab2309eec 156 PWMB.write(dutyB);
Mikebob 22:568a6d19b98a 157 }
Mikebob 14:e66cf781f5b9 158 int main()
Mikebob 14:e66cf781f5b9 159 {
Mikebob 14:e66cf781f5b9 160 //Configure the terminal to high speed
Mikebob 21:d553c43a5a26 161 terminal.baud(115200);
Mikebob 14:e66cf781f5b9 162 //Set initial motor direction
Mikebob 14:e66cf781f5b9 163 DIRA = FORWARD;
Mikebob 14:e66cf781f5b9 164 DIRB = FORWARD;
Mikebob 14:e66cf781f5b9 165 //Set motor period to 100Hz
Mikebob 14:e66cf781f5b9 166 PWMA.period_ms(10);
Mikebob 14:e66cf781f5b9 167 PWMB.period_ms(10);
Mikebob 14:e66cf781f5b9 168 //Set initial motor speed to stop
Mikebob 14:e66cf781f5b9 169 PWMA.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 170 PWMB.write(0.0f); //0% duty cycle
Mikebob 14:e66cf781f5b9 171 //Wait for USER button (blue pull-down switch) to start
Mikebob 14:e66cf781f5b9 172 terminal.puts("Press USER button to start");
Mikebob 14:e66cf781f5b9 173 while (SW1 == RELEASED);
dparmenter 30:9aeca82187f5 174 //Wait - so buggy doesn't move immediately
Mikebob 21:d553c43a5a26 175 wait(1.0);
dparmenter 30:9aeca82187f5 176 //Main loop
Mikebob 26:072ab2309eec 177 while(1) {
dparmenter 30:9aeca82187f5 178 straight(); //starts the straight function
dparmenter 30:9aeca82187f5 179 oneRPS(); //starts oneRPS function
dparmenter 30:9aeca82187f5 180 timeA(); //TimeA function to get wheel speed and distance
dparmenter 30:9aeca82187f5 181 timeB(); //TimeB function to get wheel speed and distance
dparmenter 30:9aeca82187f5 182 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
dparmenter 30:9aeca82187f5 183 while(travA <= 1250) { //starts moving from 0mm to 1250mm (distance of wheel travel)
dparmenter 30:9aeca82187f5 184 timeA(); //Starts time A
dparmenter 30:9aeca82187f5 185 timeB(); //starts time B
dparmenter 30:9aeca82187f5 186 oneRPS(); //starts oneRPS function
dparmenter 30:9aeca82187f5 187 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 188 }
dparmenter 30:9aeca82187f5 189 reset(); //activates reset function
dparmenter 30:9aeca82187f5 190 turn(); //starts turn function
dparmenter 30:9aeca82187f5 191 while(travA <= 597) { //moves from 0mm to 597mm (distance of outside wheel travel)
dparmenter 30:9aeca82187f5 192 timeA(); //starts time A
dparmenter 30:9aeca82187f5 193 timeB(); //starts time B
dparmenter 30:9aeca82187f5 194 cornerRPS(); //starts CornerRPS function
dparmenter 30:9aeca82187f5 195 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 196 }
dparmenter 30:9aeca82187f5 197 reset(); //activates reset function
dparmenter 30:9aeca82187f5 198 straight(); //starts the straight function
dparmenter 30:9aeca82187f5 199 while(travA <= 1457) { //starts moving from 0mm to 1457mm (distance of wheel travel)
dparmenter 30:9aeca82187f5 200 timeB(); //starts time A
dparmenter 30:9aeca82187f5 201 timeA(); //starts time B
dparmenter 30:9aeca82187f5 202 oneRPS(); //starts oneRPS function
dparmenter 30:9aeca82187f5 203 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 204 }
dparmenter 30:9aeca82187f5 205 reset(); //activates reset function
dparmenter 30:9aeca82187f5 206 turn(); //starts turn function
dparmenter 30:9aeca82187f5 207 while(travA <= 453.8) { //starts moving from 0mm to 453.8mm (distance of wheel travel)
dparmenter 30:9aeca82187f5 208 timeA(); //starts time A
dparmenter 30:9aeca82187f5 209 timeB(); //starts time B
dparmenter 30:9aeca82187f5 210 cornerRPS(); ///starts CornerRPS function
dparmenter 30:9aeca82187f5 211 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 212 }
dparmenter 30:9aeca82187f5 213 reset(); //activates reset function
dparmenter 30:9aeca82187f5 214 straight(); //starts the straight function
dparmenter 30:9aeca82187f5 215 while(travA <= 750) { //starts moving from 0mm to 750mm (distance of wheel travel)
dparmenter 30:9aeca82187f5 216 timeB(); //starts time A
dparmenter 30:9aeca82187f5 217 timeA(); //starts time B
dparmenter 30:9aeca82187f5 218 oneRPS(); //starts oneRPS function
dparmenter 30:9aeca82187f5 219 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 220 }
dparmenter 30:9aeca82187f5 221 reset(); //activates reset function
dparmenter 30:9aeca82187f5 222 turn(); //starts turn function
dparmenter 30:9aeca82187f5 223 while(travA <= 350.3) { //starts moving from 0mm to 349mm (distance of wheel travel)
dparmenter 30:9aeca82187f5 224 timeA(); //starts time A
dparmenter 30:9aeca82187f5 225 timeB(); //starts time B
dparmenter 30:9aeca82187f5 226 cornerRPS(); //starts CornerRPS function
dparmenter 30:9aeca82187f5 227 terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
Mikebob 26:072ab2309eec 228 }
Mikebob 22:568a6d19b98a 229 break;
Mikebob 14:e66cf781f5b9 230 }
dparmenter 30:9aeca82187f5 231 PWMA.write(0.0f); //tells wheel A to stop
dparmenter 30:9aeca82187f5 232 PWMB.write(0.0f); //tells wheel B to stop
Mikebob 26:072ab2309eec 233 }