File content as of revision 1:cc2a9eb0bd55:

#include "mbed.h"
#include "rtos.h"
#include "TSH.h"
#include "Kalman.h"
#include "globals.h"
#include "motors.h"
#include "math.h"
#include "system.h"
#include "geometryfuncs.h"
#include "motion.h"
#include "ai.h"
#include "ui.h"

//#include <iostream>

//Interface declaration
Serial pc(USBTX, USBRX); // tx, rx

bool Colour = 1; // 1 for red, 0 for blue
pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start

DigitalIn StartTrig(p11);
DigitalIn ColourToggle(p19); //high for red, low for blue(purple)
Ticker StopTicker;

TSI2C i2c(p28, p27);
Motors motors(i2c);
UI ui;
Kalman kalman(motors,ui,p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
AI ai;
Motion motion(motors, ai, kalman);


void vMotorThread(void const *argument);
void vPrintState(void const *argument);
void motion_thread(void const *argument);
void vStop (void);

//bool flag_terminate = false;

float temp = 0;

//Main loop
int main() {
ai.flag_manOverride = true;
    // no motor motions till we pull the trig
    ai.flag_motorStop = true;

    Colour = ColourToggle;
    OLED3 = Colour;
    // re-defines beacon positions by the toggle switch
    kalman.statelock.lock();
    if (true) {
        beaconpos[0].x = 3000;
        beaconpos[0].y = 1000;
        beaconpos[1].x = 0;
        beaconpos[1].y = 0;
        beaconpos[2].x = 0;
        beaconpos[2].y = 2000;
        //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
    } else {
        beaconpos[0].x = 0;
        beaconpos[0].y = 1000;
        beaconpos[1].x = 3000;
        beaconpos[1].y = 0;
        beaconpos[2].x = 3000;
        beaconpos[2].y = 2000;
        //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}};
    }
    kalman.statelock.unlock();
    pc.baud(115200);


    //Init kalman, this should be done in the mid of the arena before the game starts
    kalman.KalmanInit();

    //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
    //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);

    //measure cpu usage. output updated once per second to symbol cpupercent
    //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack

    pc.printf("We got to main! ;D\r\n");

    if (Colour)
        printf("I'm in Red \n\r");
    else
        printf("I'm in Blue \n\r");

    //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
    while (1) {

        osThreadSetPriority (osThreadGetId(), osPriorityIdle);

        Timer timer;
        ui.regid(10, 1);

        while (1) {
            timer.reset();
            timer.start();
            nopwait(1000);

            ui.updateval(10, timer.read_us());
        }

        // do nothing
        //Thread::wait(osWaitForever);
    }
}

void AI::ai_thread () {
    //Thread::wait(1000);

    printf("Waiting for the trigger pull ....\r\n");

    // wait for the start triger
    while (!StartTrig) {
        Thread::wait(10);
    };


    //printf("GO! \r\n");
    kalman.KalmanReset();
    Thread::wait(100);
    

    // attach a 87 seconds stop timer
    StopTicker.attach(&vStop, 87);




    // starts motors
    ai.flag_motorStop = false;
    
//    ai.flag_manOverride = true;
//    motors.setSpeed(20);
//    Thread::wait(5000);
    ai.flag_manOverride = false;



//    if (Colour) {
        // strat 1 RED ==================================

        // run out to black line
        settarget(640,300,PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);

/*
//prep to move block
settarget(640, 860,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);

//move block to ship
settarget(173, 860,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);

//back out
settarget(640,860,PI/2,false,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);
*/

 // intermediate
        settarget(640,1600,-PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);

 //push first button
        settarget(640,2200,-PI/2,false,Colour, MOVE_SPEED);
        //Thread::signal_wait(0x01,10000);
        Thread::wait(5000);


//back out from button
        settarget(640,1500,-PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);

 // go get middle bullion
        settarget(1500,1320,0,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);


        // go to second button
        settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);


        // push button
        settarget(1920,2200,-PI/2,false,Colour, MOVE_SPEED);
        Thread::wait(5000);

        // back off
        settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);

        // go somewhere
        settarget(2400,1000,-PI/2,true,Colour, MOVE_SPEED);
        Thread::signal_wait(0x01);

//go to opponents captain area
settarget(2200,280,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);

//back to own captain area
settarget(260,275,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//back to black line
settarget(640,300,0,false,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);


//grab CD
settarget(1000,525,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);


//back to own captain area
settarget(260,275,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//to opponent's totem
settarget(1750,850,0,false,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//collect their CDs
settarget(2000,750,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//collect more
settarget(2190,1155,0,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);

//go to opponents captain area
settarget(2200,280,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//go back to ship
settarget(210,900,PI,true,Colour, MOVE_SPEED);
Thread::signal_wait(0x01);



//END

    // terminate thread, stopps motors permanently
    ai.flag_terminate = true;
    while (true) {
        Thread::wait(osWaitForever);
    }


    // end of strat 1 ===========================
}


void vMotorThread(void const *argument) {
    motors.resetEncoders();
    while (1) {
        motors.setSpeed(20,20);
        Thread::wait(2000);
        motors.stop();
        Thread::wait(5000);
        motors.setSpeed(-20,-20);
        Thread::wait(2000);
        motors.stop();
        Thread::wait(5000);
        motors.setSpeed(-20,20);
        Thread::wait(2000);
        motors.stop();
        Thread::wait(5000);
        motors.setSpeed(20,-20);
        Thread::wait(2000);
        motors.stop();
        Thread::wait(5000);
    }
}


void vPrintState(void const *argument) {
    float state[3];
    float SonarMeasures[3];
    float IRMeasures[3];


    while (1) {
        kalman.statelock.lock();
        state[0] = kalman.X(0);
        state[1] = kalman.X(1);
        state[2] = kalman.X(2);
        SonarMeasures[0] = kalman.SonarMeasures[0];
        SonarMeasures[1] = kalman.SonarMeasures[1];
        SonarMeasures[2] = kalman.SonarMeasures[2];
        IRMeasures[0] = kalman.IRMeasures[0];
        IRMeasures[1] = kalman.IRMeasures[1];
        IRMeasures[2] = kalman.IRMeasures[2];
        kalman.statelock.unlock();
        pc.printf("\r\n");
        pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]*180/PI);
        pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
        pc.printf("IR   : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
        Thread::wait(100);
    }
}

void vStop (void) {
//    while (true) {
    motors.coastStop();
    ai.flag_motorStop = true;
    // terminate thread, stopps motors permanently
    ai.flag_terminate = true;
//    };
}