File content as of revision 4:e62a2df0a5b5:

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

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
// Serial communication using MODSERIAL
#define SERIAL_BAUD 115200  // baud rate for serial communication
#include "MODSERIAL.h"
MODSERIAL pc(USBTX,USBRX);

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
// Timing
const float kTimeLedToggle = 0.25f; // period with which to toggle LED
const float kTimePrintSerial = 1.0f;// period with which data is printed

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
// constants
const int LedOn = 0;               // LED on if 0

// LEDs
DigitalOut led_r(LED_RED);
DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);

// ID of led that should blink; *volatile* because changed by interrupt
volatile int part_id = 0;

// counters
int num_turned_on_0 = 0;            // count number of times red LED turned on
int num_turned_on_1 = 0;            // count number of times green LED turned on
int num_turned_on_2 = 0;            // count number of times blue LED turned on


//Safety for Motor
int Brakeable; //cna the motor driver break
int sign; //prevents throwing the motor from full foward to full reverse and stuff melting.

//Motor
Motor A(D6, D7, D7, Brakeable); // pwm, fwd, rev, brake                 (right)
Motor B(D5, D4, D4, Brakeable); // pwm, fwd, rev, brake                 (left)




void SwitchPart()
{
    switch (part_id) {
        case 2: {
            // LED 0 to toggle and count:
            led_r = LedOn;
            if (led_r == LedOn) {
                num_turned_on_0++;
            }
            
            // LEDs to turn off:
            led_g = not LedOn;
            led_b = not LedOn;
            A.speed(0) == 0;
            B.speed(0) == 0;
            
            break;
        }
        case 3: {
            // LED 1 to toggle and count:
            led_g = LedOn;
            A.speed(0.5) == 0.5;
            if (led_g == LedOn) {
                num_turned_on_1++;
            }
            
            // LEDs to turn off:
            led_r = not LedOn;
            led_b = not LedOn;
            B.speed(0) == 0;
                        
            break;
        }
        case 4: {
            // LED 2 to toggle and count:
            led_b = LedOn;
            B.speed(0.5) == 0.5;
            if (led_b == LedOn) {
                num_turned_on_2++;
            }
            
            // LEDs to turn off:
            led_r = not LedOn;
            led_g = not LedOn;
            A.speed(0) == 0;
            
            break;
        }
    }
}

/**
 * Print the number of times each LED was turned on through serial communication
 */
void PrintSerial()
{
    pc.printf("*Status*\r\n\tn_r = %d\r\n\tn_g = %d\r\n\tn_b = %d\r\n\r\n",
        num_turned_on_0, num_turned_on_1, num_turned_on_2);
}

/**
 * Switch the led id that blinks 
 * led_blink_id goes from 0 -> 1 -> 2 -> 0 -> ...
 * @ensure led_blink_id = ++led_blink_id % kNumStates
 */
void SetValue2() {
    part_id = 2;
   }

void SetValue3() {
    part_id = 3;
   }


void SetValue4() {
    part_id = 4;
}


/**
 * Main loop.
 */
int main()
{
    // Serial comm baud rate
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n**RESET**\r\n");
    
    // Turn off all LEDs initially
    led_r = not LedOn;
    led_g = not LedOn;
    led_b = not LedOn;
    
    // Create ticker and attach LED toggle function
    Ticker tick_toggle_led;
    tick_toggle_led.attach(&SwitchPart,kTimeLedToggle);
    
    // Create ticker and attach Print function
    Ticker tick_print_serial;
    tick_print_serial.attach(&PrintSerial,kTimePrintSerial);
    
    // Create interrupt and attach switch function
    InterruptIn btn_cart(D2);
    btn_cart.fall(&SetValue2);
    
    InterruptIn btn_arm(D3);
    btn_arm.fall(&SetValue3);

    InterruptIn btn_claw(D4);
    btn_claw.fall(&SetValue4);

    while (true);
}