File content as of revision 2:72f9b88d0e7c:

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

#include "mbed.h"
#include "motordriver.h"

//Initialization//
Motor drive(p26, p25, p24, 1); //drive motor (PWM, fwd, rev, brake
Motor wheel(p23, p22, p21, 1); //flywheel (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 fo the trigger

Timer time;

DigitalOut fwdled(LED1);       //for testing interrupts, etc
DigitalOut revled(LED2);       // ""


int rev = 0, fwd = 0;          //flag for fwd and rev
float = t1, t2, dist;        //global variables for the range sensor
int jump = 0;               //flag for wether or not car is jumping

//Interrupt Service Routines
void driveforward()
{
    fwd = 1;
    rev = 0;
}
void drivereverse()
{
    rev=1;
    fwd = 0;
}
void stop()
{
    fwd = rev = 0;
}
void startechotimer()
{
    t1 = t.read;
}
void endechotime()
{
    t2 = t.read;
}

//Functions
void trigger_pulse()  //trigger pulse for range sensor
{
    trigger = 1;
    for (int i=0; i<3; i++) {
    }
    trigger = 0;
}

void distance()
{
    dist = t2-t1; ///*****convert delay to distance in cm here*****
    if (distance > 4.0)
        jump = 1;
    else
        jump = 0;
}

void flip()  //***** all flip function stuff in here*******
{
    float PWM_wheel = 0.0;      //initialize PWM speed for reactionwheel
    PWM_wheel = 0.20;  //***control law here***
    wheel.speed(PWM_wheel);  //sets PWM for flywheel
}



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

int main()
{

    float PWM_drive = 0.0;      //initialize PWM speed for drive wheels


    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

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

    t.start(); //atart timer

    while(1) {
        trigger_pulse(); //send triggerpulse

        if (jump)
            flip();

        PWM_drive = 0.50;  //***control law here for drive speed***


        if(fwd) {                    //reads flags for drive/rev
            drive.speed(PWM_drive);
            fwdled = 1;     //led1 on
            revled = 0;     //led2 off
        } else if(rev) {
            drive.speed(-PWM_drive);
            fwdled = 0;     //led1 off
            revled = 1;     //led2 on
        } else {
            drive.coast();
            fwdled = revled = 0;
        }
    }
}