File content as of revision 16:9ce3821d4550:

// Jared Wilkins, Katie MacVarish, Grant LaTerza //
// Capstone Project Code                         //
///////////////////////////////////////////////////

#include "mbed.h"
#include "motordriver.h"  //expand capabilities of PWMOut for use with motor drivers written by Christopher Hasler

//Initialization/////////////////////////////
Motor wheel(p24, p26, p25, 1); //flywheel (PWM, fwd, rev, brake)
Motor drive(p21, p23, p22, 1); //drive motor (PWM, fwd, rev, brake]

InterruptIn forward(p15);      //remote signal to drive car forward
InterruptIn reverse(p16);      //signal to drive car in reverse
InterruptIn echo(p29);         //interrupt for range echo pulse

DigitalOut trigger(p30);       //Dout for the trigger pulse

DigitalOut fwdled(LED1);       //for testing interrupts, functions, etc
DigitalOut revled(LED2);       // ""
DigitalOut jumpled(LED3);      // ""
DigitalOut testled(LED4);      // ""

Timer t;                       //initializes timer t


//Global Variables////////////////
int jump = 0;          //flag for whether jumping or not
int t1, t2,                    //global variables for the echo pulse. c
    t3, t4;                    //timer variables within flip function
float PWM_drive = 0.0;      //initialize PWM speed for drive wheels

//Interrupt Service Routines////////////////
void driveforward()
{
    fwdled = 1;
    revled = 0;
    drive.speed(PWM_drive);
}
void drivereverse()
{
    revled =1;
    fwdled = 0;
    drive.speed(-PWM_drive);
}
void stop()
{
    fwdled = revled = 0;
    drive.coast();
}
void startechotimer()
{
    t1 = t.read_us();
}
void endechotimer()
{
    t2 = t.read_us();
}

//Functions//////////////////////////

float read_ultrasonic(int number_of_samples) //function that an average of multiple distance samples
{
    float dist = 0.0;

    for (int i = 0; i< number_of_samples; i++) {
        trigger = 1;   // begin pulse
        wait(.000015); // 15 us
        trigger = 0;   // end pulse

        // return pulse captured by interrupts

        dist+= (t2-t1)/58.0; //converts time to distance in cm
    }

    dist = dist/number_of_samples;   //take average

    return dist;
} //end read_ultrasonic() function

void flip()   //all flip function stuff in here
{
    float PWM_wheel = 0.0;  //initialize PWM speed for reactionwheel
    t3 = t.read();
    if (t3 - t4 >= 5) {          //ensures that this can only run once every six seconds
        t3 = t.read();           // read (probably unnecessary)
        testled = 1;            // turns on led 3 (during flip)

        PWM_wheel = 1.0;        //***control law here***
        wheel.speed(PWM_wheel); //sets PWM for flywheel
        wait(2);                //settling time
        wheel.stop(1.0);        //regenerative braking

        t4 = t.read();          // read current time
        testled = 0;            // turn off led (not flipping)
    } else
        testled = 0;
} //end flip function



//Main Program///////////////////////////

int main()
{
    float range = 0.0;     // variable to hold the range sensor value

    fwdled = revled = testled = jumpled = 0;   //intializes all LEDS to off

    wheel.speed(0.0); // reaction wheel is initially off
    drive.speed(0.0); //drive motor initially off

    forward.rise(&driveforward); // rising edge on p15 triggers interrupt to go forward
    forward.fall(&stop);         // falling edge p15 triggers a stop

    reverse.rise(&drivereverse); // rising edge p16 triggers go in reverse
    reverse.fall(&stop);         // falling edge p16 triggers stop

    echo.rise(&startechotimer);  //rising edge of echo
    echo.fall(&endechotimer);    //falling edge of echo

    t1 = t2 = t4 = 0;      //initialize timers variables to 0
    t3 = 10;
    t.start();             //start timer

    while(1) {
        PWM_drive = 1.0;   //****control law here for drive speed****

        range = read_ultrasonic(20);      //read distance for average of 20 samples

        if (range > 6.0) {    //when distance is greater than 6.0 cm it will do the flip function (i.e. the car is 6cm above th ground)
            jumpled = 1;      // lights LED 3 when in jump flag is true
            flip();           //call flip function
        } else  {
            jumpled = 0;      // if not in jump mode, LED3 is off
        }

    }
}