Anonymous Anonymous - 12 days ago
748 0

No description

C++

stuffzAwesome

#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;

//Enumerated types
enum DIRECTION   {FORWARD=0, REVERSE};
enum PULSE       {NOPULSE=0, PULSE};
enum SWITCHSTATE {PRESSED=0, RELEASED};

//Debug GPIO
DigitalOut probe(D10);

//Duty cycles
float dutyA = 1.0f; //100%
float dutyB = 1.0f; //100%

void driveBuggy(float dutyA, float dutyB, float x, float y)   //void as no data returned, passing duties and x & y as parameters
{
    PWMA.write(dutyA); //Write max value to dutyA to turn left wheel and drive forwards
    PWMB.write(dutyB); //Write max value to dutyB to turn right wheel and drive forwards
    wait(x); //Time to drive forwards for
    PWMA.write(dutyA);  //Maintain full speed of left wheel to enable smooth turns
    PWMB.write(dutyB/3);//Lower speed of right wheel to acheive correct angle for turn while maintaing smooth movement
    wait(y); //How long to turn for
    return;  //end function
}

int main()
{
//Declaring x and y which are two wait values used to determine how long buggy drives forward/turns.
    float x;//Buggy driving straight duration
    float y;//Buggy turn duration

    //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;

    //Set initial motor speed to stop
    PWMA.write(dutyA);          //Set duty cycle (%)
    PWMB.write(dutyB);          //Set duty cycle (%)

    //Wait - give time to start running
    wait(1.0);

    //Array of sensor data
    int tA1[2];
    int tA2[2];

//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);

    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
    float TA1 = 2.0f * (tA1[1]-tA1[0]);
    float TA2 = 2.0f * (tA2[1]-tA2[0]);
    float TA = (TA1 + TA2) * 0.5f;

    float fA = 1.0f/ (TA *(float)3.0E-6);
    terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA, fA/20.2f);


    for(int n=0; n<4; n++) {    //For loop controlling number of sides of circuit to complete
        dutyA = 1.0f;
        dutyB = 1.0f;           //Resetting duty to max value to ensure buggy is back to maximum speed after each iteration
        switch(n) {
            case 0:
                x = 2.0;        //Setting value of x to determine how long buggy travels forwards for, in this instance travels forward for 2s when doStuff is called.
                y = 1.65;       //Setting value of turn time
                driveBuggy(dutyA, dutyB, x, y);            //Calling function driveBuggy passing parameters of both duties and x and y to control the speed of the buggy in the function and the time to drive forwards/turn respectively.
                break;
            case 1:
                x = 2.2;
                y = 1.13;
                driveBuggy(dutyA, dutyB, x, y);
                break;
            case 2:
                x = 1;
                y = 0.95;
                driveBuggy(dutyA, dutyB, x, y);
                break;
            default:           //Default case reached means either an error has occured due to something messing with value/memory location of n (unlikely) or the program has reached its end and so the buggy will come to a stop.
                PWMA.write(0.0f);//Duty set to 0 so buggy stops
                PWMB.write(0.0f);
                wait(1000000000);//(infinte) wait stops buggy from driving
        }



    }
}



}
Recommended from our users: Dynamic Network Monitoring from WhatsUp Gold from IPSwitch. Free Download