File content as of revision 32:8a7a72c360ed:

#include "mbed.h"
//#include "TCPSocket.h"
#include "ESP8266Interface.h"
#include "Motor.h"
#include "rgbled.h"
#include "rgbSensor.h"
#include "universal.h"
#include <ctime>

// Define threads and mutexes
Thread thread1;
Thread thread2;
Thread thread3;
Thread thread4;

// Global game actions
bool paused = false;
void check_unpause() {
    char bnum=0;
    char bhit=0;
    while (true) {
        if (blue.getc()=='B') { //button data packet
            bnum = blue.getc(); //button number
            bhit = blue.getc(); //1=hit, 0=release
            if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                if (bnum == '1') { //number button 1, pause
                    if (bhit=='1') paused = false;
                }
            }
        }
    }
}

void game_paused() {
    // Cycle through LEDs
    Thread pause_thread;
    pause_thread.start(check_unpause);
    while (paused) {
        int num = 1;
        for (int i = 0; i < 3; i++) {
            num *= 2;
            myled = num;
            ThisThread::sleep_for(100);
        }
        ThisThread::sleep_for(500);
        for (int i = 3; i > 0; i--) {
            num /= 2;
            myled = num;
            ThisThread::sleep_for(100);
        }
        ThisThread::sleep_for(500);
    }
}

// Thread to control speed
void speed_control() {
    // The kart has to be in one of three states at all times
    while(1) {
        while(sstate == coasting) {
            if (speed_cmd > 0.0) speed_cmd -= coast_rate;
            else speed_cmd = 0.0;
            left.speed(speed_cmd * left_multiplier);
            right.speed(speed_cmd * right_multiplier);
            ThisThread::sleep_for(200);
        }
        while(sstate == accelerating) {
            if (speed_cmd < max_speed) speed_cmd += acceleration_rate;
            else speed_cmd = max_speed;
            left.speed(speed_cmd * left_multiplier);
            right.speed(speed_cmd * right_multiplier);
            ThisThread::sleep_for(200);
        }
        while(sstate == braking) {
            if (speed_cmd > -1.0) speed_cmd -= brake_rate;
            else speed_cmd = -1.0;
            left.speed(speed_cmd * left_multiplier);
            right.speed(speed_cmd * right_multiplier);
            ThisThread::sleep_for(200);
        }
    }
}

void powerupthread() {
    while(1){
        if(powerup){
            max_speed = 1.0;
            ThisThread::sleep_for(5000);
            max_speed = 0.8;
        }
        ThisThread::sleep_for(300);
    }
}

// Thread for checking rgb sensor values and updating game variables
// Change later depending on behavior of RGB sensor and colors used
int t = 8000;   //change this depending on RGB values
void check_RGB() {
    while(true){
        rgbsensor.update();
        int C_value = rgbsensor.get_C();
        int R_value = rgbsensor.get_R();
        int G_value = rgbsensor.get_G();
        int B_value = rgbsensor.get_B();
        if(C_value < t && R_value < t && G_value > t && B_value < t && current_item == 0) { //Check for if cart runs over an item box
            itembox = true;
            cstate = 0;
        }
        else if(C_value < t && R_value < t && G_value < t && B_value > t) {     //Check for if cart runs over a speed boost panel
            powerup = true;      //change this depending on speed up panel algs
            cstate = 0;
        }
        else if(C_value < t && R_value > t && G_value < t && B_value < t) {     //Check if cart reaches next checkpoint. cstate is set to 1 upon seeing first color of checkpoint
            victory = true;
        }
        pc.printf("[C: %d, R: %d, G: %d, B: %d]", C_value, R_value, G_value, B_value); 
        ThisThread::sleep_for(500);
    }
}

void set_item() {
    while(true){
        if(itembox && current_item == 0) {
            current_item = 1;
            myRGBled.write(1.0,0.0,0.0);
            itembox = false;
        }
        ThisThread::sleep_for(500);
    }
}

void hold_item() {
    if(current_item == 0){
        return;
    }
    if(current_item == 1){
        powerup = true;
        current_item = 0;
        return;
    }
    else{
        holding_item = true;
    }
    return;
}

void release_item(){
    holding_item = false;
    if(current_item == 2){
        //hit player in front
        return;
    }
    if(current_item == 3){
        //hit player in first
        return;
    }
    if(current_item == 4){
        //hit player behind
        return;
    }
}

int main() {
    pc.printf("Initializing ");
    /*
    // Connect to Wifi
    int ret = wifi.connect(MBED_CONF_APP_WIFI_SSID, MBED_CONF_APP_WIFI_PASSWORD, NSAPI_SECURITY_WPA_WPA2);
    if (ret != 0) {
        pc.printf("\r\nConnection error\r\n");
    } else {
        printf("Success\r\n\r\n");
        printf("MAC: %s\r\n", wifi.get_mac_address());
        printf("IP: %s\r\n", wifi.get_ip_address());
        printf("Netmask: %s\r\n", wifi.get_netmask());
        printf("Gateway: %s\r\n", wifi.get_gateway());
        printf("RSSI: %d\r\n\r\n", wifi.get_rssi());
    }*/
    
    // Start threads
    thread1.start(check_RGB);
    thread2.start(speed_control); // Since we're stopped, this won't do anything
    thread3.start(powerupthread);
    thread4.start(set_item);
    
    
    std::clock_t start;
    double duration;
    start = std::clock();
    
    // Bluetooth controller code
    char bnum=0;
    char bhit=0;
    pc.printf("running");
    while(running) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    switch (bnum) {
                        case '1': //number button 1, pause
                            if (bhit=='1') {
                                paused = true;
                                game_paused();
                            }
                            break;
                        case '2': //number button 2, accelerate
                            if (bhit=='1') {
                                sstate = accelerating;
                                pc.printf("accelerating");
                            } else {
                                sstate = coasting;
                            }
                            break;
                        case '3': //number button 3
                            if (bhit=='1') {
                            } else {
                            }
                            break;
                        case '4': //number button 4, brakes
                            if (bhit=='1') {
                                sstate = braking;
                            } else {
                                sstate = coasting;
                            }
                            break;
                        case '5': //button 5 up arrow
                            if (bhit=='1') {
                                pc.printf("up");
                                hold_item();        //holds item to protect player from incoming hazards
                            } else {
                                release_item();     //release item to attack
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                hold_item();        //holds item to protect player from incoming hazards
                            } else {
                                release_item();     //release item to attack
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                pc.printf("left");
                                left_multiplier = 0.5;
                                right_multiplier = 1.0;
                            } else {
                                left_multiplier = 1.0;
                                right_multiplier = 1.0;
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                pc.printf("right");
                                left_multiplier = 1.0;
                                right_multiplier = 0.5;
                            } else {
                                left_multiplier = 1.0;
                                right_multiplier = 1.0;
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
            if(victory){
                running = false;
                duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
                pc.printf("%d",duration);
            }
        }
    }
    wifi.disconnect();
}