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-12-11
- Revision:
- 22:568a6d19b98a
- Parent:
- 21:d553c43a5a26
- Child:
- 24:b05cb3dd943e
File content as of revision 22:568a6d19b98a:
//Enhancement 2// //Enhancement 2// #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 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 timerA; Timer timerB; //Enumerated types enum DIRECTION {FORWARD=0, REVERSE}; enum PULSE {NOPULSE=0, PULSE}; enum SWITCHSTATE {PRESSED=0, RELEASED}; //Duty cycles float dutyA = 0.2f; //100% float dutyB = 0.2f; //100% //Array of sensor data int tA1[2]; int tA2[2]; int tB1[2]; int tB2[2]; float speedA[3]; float speedB[3]; float fA, fB = 0.0f; float sumA, sumB = 0.0f; int durA, durB = 0; float TA[2]; float TB[2]; float TAA, TBB = 0.0f; float pulse = 0.0f; float trav = 0.0f; void timeA() { static int n=0; //Number of pulse sets static int HallState = 0; //********************** //TIME THE FULL SEQUENCE //********************** if (n==0) { //Reset timer and Start timerA.reset(); timerA.start(); TA[0] = timerA.read_us(); } switch(HallState) { case 0: if(HEA1 == PULSE){ HallState = 1; }break; case 1: if(HEA1 == NOPULSE){ HallState = 0; n++; pulse++; trav = ((176/20.8)/3)*pulse; }break; } if (n < 9) return; TA[1] = timerA.read_us(); TAA = (TA[1]-TA[0]); // Calculate speeed fA = 2.0f/ (TAA *(float)1.0E-6); if(durA == 0){ speedA[durA] = fA/20.8; durA++; return; } else if(durA == 1){ speedA[durA] = fA/20.8; durA++; return; } else if(durA == 2){ speedA[durA] = fA/20.8; durA = 0; } for(int xA=0;xA<3;xA++){ sumA+=speedA[xA]; } sumA = sumA/3; //Reset count n=0; } void timeB() { static int nB=0; //Number of pulse sets static int HallStateB = 0; //********************** //TIME THE FULL SEQUENCE //********************** if (nB==0) { //Reset timer and Start timerB.reset(); timerB.start(); TB[0] = timerB.read_us(); } switch(HallStateB) { case 0: if(HEB1 == PULSE){ HallStateB = 1; }break; case 1: if(HEB1 == NOPULSE){ HallStateB = 0; nB++; }break; } if (nB < 9) return; TB[1] = timerB.read_us(); TBB = (TB[1]-TB[0]); // Calculate speeed fB = 2.0f/ (TBB *(float)1.0E-6); if(durB == 0){ speedB[durB] = fB/20.8; durB++; return; } else if(durB == 1){ speedB[durB] = fB/20.8; durB++; return; } else if(durB == 2){ speedB[durB] = fB/20.8; durB = 0; } for(int xB=0;xB<3;xB++){ sumB+=speedB[xB]; } sumB = sumB/3; //Reset count nB=0; } void oneRPS(){ float deltaA = 1.0f-sumA; //Error float deltaB = 1.0f-sumB; dutyA = dutyA + deltaA*0.001f; //Increase duty in proportion to the error dutyB = dutyB + deltaB*0.001f; //Increase duty in proportion to the error //Clamp the max and min values of duty and 0.0 and 1.0 respectively dutyA = (dutyA>1.0f) ? 1.0f : dutyA; dutyA = (dutyA<0.05f) ? 0.05f : dutyA; dutyB = (dutyB>1.0f) ? 1.0f : dutyB; dutyB = (dutyB<0.05f) ? 0.05f : dutyB; //Update duty cycle to correct in the first direction PWMA.write(dutyA); PWMB.write(dutyB); } void reset(){ trav = 0; pulse = 0; } 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"); while (SW1 == RELEASED); 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); //Main polling loop PWMA.write(0.2f); //Set duty cycle (%) PWMB.write(0.2f); while(1) { timeA(); timeB(); oneRPS(); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); while(trav <= 1250) { timeA(); timeB(); oneRPS(); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } reset(); while(trav <= 330) { timeA(); PWMB.write(0.0f); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } reset(); while(trav <= 1457) { timeB(); timeA(); oneRPS(); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } reset(); while(trav <= 278.5) { timeA(); PWMB.write(0.0f); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } reset(); while(trav <= 750) { timeB(); timeA(); oneRPS(); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } reset(); while(trav <= 200) { timeA(); PWMB.write(0.0f); terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav); } break; } PWMA.write(0.0f); PWMB.write(0.0f); }