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
- Committer:
- Mikebob
- Date:
- 2019-11-19
- Revision:
- 15:b7ca53716b05
- Parent:
- 14:e66cf781f5b9
- Child:
- 16:afe936377d66
File content as of revision 15:b7ca53716b05:
//Enhancement 2// #include "mbed.h" //Status LED DigitalOut led(LED1); //Motor PWM (speed) PwmOut PWMA(PA_8); PwmOut PWMB(PB_4); //Motor Direction DigitalOut DIRA(PA_9); DigitalOut DIRB(PB_10); //Hall-Effect Sensor Inputs DigitalIn HEA1(PB_2); DigitalIn HEA2(PB_1); DigitalIn HEB1(PB_15); DigitalIn HEB2(PB_14); //On board switch DigitalIn SW1(USER_BUTTON); //Use the serial object so we can use higher speeds Serial terminal(USBTX, USBRX); //Timer used for measuring speeds Timer timer; Timer timer1; //Enumerated types enum DIRECTION {FORWARD=0, REVERSE}; enum PULSE {NOPULSE=0, PULSE}; enum SWITCHSTATE {PRESSED=0, RELEASED}; //Duty cycles float dutyA = 1.0f; //100% float dutyB = 1.0f; //100% //Array of sensor data int tA1[2]; int tA2[2]; int tB1[2]; int tB2[2]; float dis; float trav =0; float speedA, speedB = 1.0f; float paceA, paceB = 1.0f; void time() { //Reset timer and Start timer.reset(); timer.start(); //********************** //TIME THE FULL SEQUENCE //********************** //Wait for rising edge of A1 and log time while (HEA1 == NOPULSE); tA1[0] = timer.read_us(); //Wait for rising edge of A2 and log time (30 degrees?) while (HEA2 == NOPULSE); tA2[0] = timer.read_us(); //Wait for falling edge of A1 while (HEA1 == PULSE); tA1[1] = timer.read_us(); //Wait for falling edge of A2 while (HEA2 == PULSE); tA2[1] = timer.read_us(); //Wait for rising edge of B1 and log time while (HEB1 == NOPULSE); tB1[0] = timer.read_us(); //Wait for rising edge of B2 and log time (30 degrees?) while (HEB2 == NOPULSE); tB2[0] = timer.read_us(); //Wait for falling edge of B1 while (HEB1 == PULSE); tB1[1] = timer.read_us(); //Wait for falling edge of B2 while (HEB2 == PULSE); tB2[1] = timer.read_us(); terminal.printf("tA1(0) = %d\n", tA1[0]); terminal.printf("tA1(1) = %d\n", tA1[1]); terminal.printf("tA2(0) = %d\n", tA2[0]); terminal.printf("tA2(1) = %d\n", tA2[1]); //Calculate the frequency of rotation for A float TA1 = 2.0f * (tA1[1]-tA1[0]); float TA2 = 2.0f * (tA2[1]-tA2[0]); float TA = (TA1 + TA2) * 0.5f; //Calculate the frequency of rotation for B float TB1 = 2.0f * (tB1[1]-tB1[0]); float TB2 = 2.0f * (tB2[1]-tB2[0]); float TB = (TB1 + TB2) * 0.5f; dis = timer1.read_us(); float mm = ((TA*3)*20.8)/175.9; trav = dis/mm; float fA = 1.0f/ (TA *(float)3.0E-6); // FOR A float fB = 1.0f/ (TB *(float)3.0E-6); // FOR B terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n", fA/20.8f, fB/20.8f, trav); speedA = fA/20.8f; speedB = fB/20.8f; } void reset() { timer1.reset(); time(); } void oneRPS () { time(); if (speedA <= 1.0f) { paceA +=0.01f; wait_ms(25); } else if (speedA >= 1.0f) { paceA -=0.01f; wait_ms(25); } else if (speedB <= 1.0f) { paceB +=0.01f; wait_ms(25); } else if (speedB >= 1.0f) { paceB -=0.01f; wait_ms(25); } } int main() { //Configure the terminal to high speed terminal.baud(115200); //Set initial motor direction DIRA = FORWARD; DIRB = FORWARD; //Set motor period to 100Hz PWMA.period_ms(10); PWMB.period_ms(10); //Set initial motor speed to stop PWMA.write(0.0f); //0% duty cycle PWMB.write(0.0f); //0% duty cycle //Wait for USER button (blue pull-down switch) to start terminal.puts("Press USER button to start"); led = 0; while (SW1 == RELEASED); led = 1; wait(0.5); //Set align wheels PWMA.write(1.0f); //Set duty cycle (%) PWMB.write(1.0f); //Set duty cycle (%) //********************************************************************* //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING) //********************************************************************* //Wait for rising edge of A1 and log time while (HEA1 == NOPULSE); //Wait for rising edge of A2 and log time (30 degrees?) while (HEA2 == NOPULSE); //Wait for falling edge of A1 while (HEA1 == PULSE); //Wait for falling edge of A2 while (HEA2 == PULSE); //Wait for rising edge of B1 and log time while (HEB1 == NOPULSE); //Wait for rising edge of B2 and log time (30 degrees?) while (HEB2 == NOPULSE); //Wait for falling edge of B1 while (HEB1 == PULSE); //Wait for falling edge of B2 while (HEB2 == PULSE); //Set initial motor speed to stop PWMA.write(0.0f); //Set duty cycle (%) PWMB.write(0.0f); //Set duty cycle (%) //Wait - give time to start running wait(1.0); timer1.reset(); timer1.start(); //Main polling loop float paceA = 1.0f; float paceB = 1.0f; PWMA.write(paceA); //Set duty cycle (%) PWMB.write(paceB); while(1) { oneRPS(); while(trav <= 1250) { PWMA.write(paceA); //Set duty cycle y PWMB.write(paceB); time(); oneRPS(); } reset(); while(trav <= 330) { PWMA.write(paceA); PWMB.write(0.0f); time(); oneRPS(); } reset(); while(trav <= 1457) { PWMA.write(paceA); PWMB.write(paceB); time(); oneRPS(); } reset(); while(trav <= 268) { PWMA.write(paceA); PWMB.write(0.0f); time(); oneRPS(); } reset(); while(trav <= 750) { PWMA.write(paceA); PWMB.write(paceB); time(); oneRPS(); } reset(); while(trav <= 200) { PWMA.write(paceA); PWMB.write(0.0f); time(); oneRPS(); } timer.stop(); break; } PWMA.write(0.0f); PWMB.write(0.0f); }