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.
Dependencies: mbed
Revision 0:b6d1ab1ace48, committed 2018-11-30
- Comitter:
- Pabs44
- Date:
- Fri Nov 30 13:34:17 2018 +0000
- Commit message:
- Test code for class
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r b6d1ab1ace48 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 30 13:34:17 2018 +0000 @@ -0,0 +1,295 @@ +#include "mbed.h" + +//Motor PWM (speed) +PwmOut PWMA(PA_8); +PwmOut PWMB(PB_4); +//Motor Direction +DigitalOut DIRA(PA_9); +DigitalOut DIRB(PB_10); +//Hall-Effect Sensor Input +DigitalIn HEA1(PB_2); +DigitalIn HEA2(PB_1); +DigitalIn HEB1(PB_15); +DigitalIn HEB2(PB_14); +//Use the serial object so we can use higher speeds +Serial terminal(USBTX, USBRX); +//Enumerated types +enum DIRECTION {FORWARD=0, REVERSE}; +enum PULSE {NOPULSE=0, PULSE}; +//Counter Set +int cA1 = 0; +int cB1 = 0; +int cws = 0; +//Timer Set +Timer timer; +//Set Duty Cycles +float dutyA = 1.0f; +float dutyB = 1.0f; +float dA; +float dB; + +//Pulse Counter for wheel A +void pulsecountA(){ + while(HEA1 == PULSE); + while(HEA1 == NOPULSE); + while(HEA1 == PULSE); + while(HEA1 == NOPULSE); + cA1 = cA1 + 1; + cws = cws + 1; +} +//Pulse Counter for wheel B +void pulsecountB(){ + while(HEB1 == PULSE); + while(HEB1 == NOPULSE); + while(HEB1 == PULSE); + while(HEB1 == NOPULSE); + cB1 = cB1 + 1; +} + +//Speed calculation for wheel A +void wsA(){ + int tA1[2]; + int tA2[2]; + + while(HEA1 == PULSE); + + while(HEA1 == NOPULSE); + tA1[0] = timer.read_us(); + while(HEA2 == NOPULSE); + tA2[0] = timer.read_us(); + + while(HEA1 == PULSE); + while(HEA2 == PULSE); + + while(HEA1 == NOPULSE); + tA1[1] = timer.read_us(); + while(HEA2 == NOPULSE); + tA2[1] = timer.read_us(); + + float fA1 = 1.0f/((tA1[1]-tA1[0])*(float)3.0E-6); + float fA2 = 1.0f/((tA2[1]-tA2[0])*(float)3.0E-6); + + float avA = (fA1 + fA2)/2.0f; + + float wsA = avA/20.8f; + + terminal.printf("\n\rA Wheel Speed = %6.2f", wsA); + + dA = 1.0f - wsA; +} +//Speed calculation for wheel B +void wsB(){ + int tB1[2]; + int tB2[2]; + + while(HEB1 == PULSE); + + while(HEB1 == NOPULSE); + tB1[0] = timer.read_us(); + while(HEB2 == NOPULSE); + tB2[0] = timer.read_us(); + + while(HEB1 == PULSE); + while(HEB2 == PULSE); + + while(HEB1 == NOPULSE); + tB1[1] = timer.read_us(); + while(HEB2 == NOPULSE); + tB2[1] = timer.read_us(); + + float fB1 = 1.0f/((tB1[1]-tB1[0])*(float)3.0E-6); + float fB2 = 1.0f/((tB2[1]-tB2[0])*(float)3.0E-6); + + float avB = (fB1 + fB2)/2.0f; + + float wsB = avB/20.8f; + + terminal.printf("\n\rB Wheel Speed = %6.2f", wsB); + + dB = 1.0f - wsB; +} + +//Calculate Duty Cycle Correction +void correction(){ + dutyA = dutyA + dA*0.1f; + dutyB = dutyB + dB*0.1f; + dutyA = (dutyA>1.0f) ? 1.0f : dutyA; + dutyA = (dutyA<0.01f) ? 0.01f : dutyA; + dutyB = (dutyB>1.0f) ? 1.0f : dutyB; + dutyB = (dutyB<0.01f) ? 0.01f : dutyB; + PWMA.write(dutyA); + PWMB.write(dutyB); + terminal.printf("\n\rDuty A = %6.2f", dutyA); + terminal.printf("\n\rDuty B = %6.2f", dutyB); + } + +//Movement Instructions +//Drive function +void drive(){ + PWMA.write(dutyA); + PWMB.write(dutyB); +} +//Turn function +void turn(){ + PWMA.write(dutyA); + PWMB.write(0.0f); +} +//Victory dance function +void victoryDance(){ + PWMA.write(dutyA/4); + PWMB.write(dutyB/4); + DIRA = FORWARD; + DIRB = REVERSE; +} + +//Start code------------------------------------ +int main() { + //Setup + terminal.baud(115200); + PWMA.period_ms(10); + PWMB.period_ms(10); + DIRA = FORWARD; + DIRB = FORWARD; + + //Start + wait(2); + terminal.printf("\n\rNew Test\n\r"); + timer.reset(); + timer.start(); + drive(); + + //Move 177 pulses (1 meter)----------------- + while(cA1 < 177 && cB1 < 177){ + drive(); + pulsecountA(); + pulsecountB(); + if(cws == 4){ + wsA(); + wsB(); + correction(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + terminal.printf("\n\rB Pulses = %d", cB1); + } + cA1 = 0; + cB1 = 0; + + //Move 24 pulses (turn)--------------------- + terminal.printf("\n\r"); + while (cA1 < 24){ + turn(); + pulsecountA(); + if(cws == 4){ + wsA(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + } + cA1 = 0; + cB1 = 0; + + //Move 89 pulses (0.5 meters)--------------- + terminal.printf("\n\r"); + while(cA1 < 89 && cB1 < 89){ + drive(); + pulsecountA(); + pulsecountB(); + if(cws == 4){ + wsA(); + wsB(); + correction(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + terminal.printf("\n\rB Pulses = %d", cB1); + } + cA1 = 0; + cB1 = 0; + + //Move 24 pulses (turn)--------------------- + terminal.printf("\n\r"); + while (cA1 < 24){ + turn(); + pulsecountA(); + if(cws == 4){ + wsA(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + } + cA1 = 0; + cB1 = 0; + + //Move 177 pulses (1 meter)----------------- + terminal.printf("\n\r"); + while(cA1 < 177 && cB1 < 177){ + drive(); + pulsecountA(); + pulsecountB(); + if(cws == 4){ + wsA(); + wsB(); + correction(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + terminal.printf("\n\rB Pulses = %d", cB1); + } + cA1 = 0; + cB1 = 0; + + //Move 24 pulses (turn)--------------------- + terminal.printf("\n\r"); + while (cA1 < 24){ + turn(); + pulsecountA(); + if(cws == 4){ + wsA(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + } + cA1 = 0; + cB1 = 0; + + //Move 89 pulses (0.5 meters)--------------- + terminal.printf("\n\r"); + while(cA1 < 89 && cB1 < 89){ + drive(); + pulsecountA(); + pulsecountB(); + if(cws == 4){ + wsA(); + wsB(); + correction(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + terminal.printf("\n\rB Pulses = %d", cB1); + } + cA1 = 0; + cB1 = 0; + + //Move 24 pulses (turn)--------------------- + terminal.printf("\n\r"); + while (cA1 < 24){ + turn(); + pulsecountA(); + if(cws == 4){ + wsA(); + cws = 0; + } + terminal.printf("\n\rA Pulses = %d", cA1); + } + cA1 = 0; + cB1 = 0; + + //Vicorty Dance----------------------------- + victoryDance(); + wait(3); + + //Finish------------------------------------ + PWMA.write(0.0f); + PWMB.write(0.0f); +}
diff -r 000000000000 -r b6d1ab1ace48 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 30 13:34:17 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file