File content as of revision 10:0ee2d6d8d965:

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

#include "mbed.h"
#include "motordriver.h"  //additional library to expand capabilities of PWMOut for use with motor drivers written by 

//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 t;                       //initializes timer

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


int rev = 0, fwd = 0;          //flag for fwd and rev
int t1, t2,                    //global variables for the echo pulse. c
    t3, t4;                    //timer within flip
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_us();
}
void endechotimer()
{
    t2 = t.read_us();
}

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

float read_ultrasonic(int number_of_samples) //function that takes returns 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
        //wait(.001);     //wait for return pulse
        dist+= (t2-t1)/58.0; //converts time to distance in cm
    }

    dist = dist/number_of_samples;   //take average

    return dist;
} //end distance() function

void flip()   //all flip function stuff in here
{
    t3 = t.read();
    if (t3 - t4 >= 3) {          //ensures that this can only run once every six seconds
        t3 = t.read();
        testled = 1;
        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
        t4 = t.read();
        testled = 0;
   } else 
        testled = 0;
    

} //end flip function



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

int main()
{


    float PWM_drive = 0.0;      //initialize PWM speed for drive wheels
    float range = 0.0;         // variable to hold the range sensor value

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

    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
    drive.speed(0.0); //drive motor initially off

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

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

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

        if (range > 6.0) {     //when distance is greater than 6.0 cm it will do the flip function
            jumpled = 1; // lights LED 3 when in jump flag is true
            flip();              
        } else  {
            jumpled = 0;     // if not in jump mode led is off
        }

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