File content as of revision 44:555136de33e4:

#include "globals.h"
#include "Kalman.h"
#include "mbed.h"
#include "rtos.h"
#include "Arm.h"
#include "MainMotor.h"
#include "Encoder.h"
#include "Colour.h"
#include "CakeSensor.h"
#include "Printing.h"
#include "coprocserial.h"
#include <algorithm>
#include "motion.h"
#include "MotorControl.h"
#include "system.h"
#include "ai.h"

void motortest();
void encodertest();
void motorencodetest();
void motorencodetestline();
void motorsandservostest();
void armtest();
void motortestline();
void colourtest();
void pt_test();
void cakesensortest();
void printingtestthread(void const*);
void printingtestthread2(void const*);
void feedbacktest();

// bytes packing for peer to peer communication
typedef union {
    struct _data {
        unsigned char header[3];
        unsigned char ID;
        float value;
        float aux;
    }  data;
    unsigned char type_char[sizeof(_data)];
} bytepack_t;

Serial pc(USBTX, USBRX);

int main()
{

    /*****************
     *   Test Code   *
     *****************/
    //motortest();
    //encodertest();
    //motorencodetest();
    //motorencodetestline();
    //motorsandservostest();
    //armtest();
    //motortestline();
    //ledtest();
    //phototransistortest();
    //ledphototransistortest();
    //colourtest(); // Red SnR too low
    //cakesensortest();
    //feedbacktest();

    /*
    DigitalOut l1(LED1);
    Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
    l1=1;
    Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
    Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
    Thread::wait(osWaitForever);
    */
    
    SystemTime.start();
    
    
    pc.baud(115200);
    
    InitSerial();
    wait(1);
    Kalman::KalmanInit();
    
    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
    Kalman::start_predict_ticker(&predictthread);
    
    Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
    
    Ticker motorcontroltestticker;
    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
    // ai layer thread
    Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
    
    // motion layer periodic callback
    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
    motion_timer.start(50);

    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);

    measureCPUidle(); //repurpose thread for idle measurement
    //colourtest();
    //pt_test();
    while(true) {
    Thread::wait(osWaitForever);
    }
}

#include <cstdlib>
using namespace std;

void printingtestthread(void const*)
{
    const char ID = 1;
    float buffer[3] = {ID};
    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
    while (true){
        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
            buffer[i] = ID ;
        }
        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
        Thread::wait(200);
    }
}

void printingtestthread2(void const*)
{
    const char ID = 2;
    float buffer[5] = {ID};
    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
    while (true){
        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
            buffer[i] = ID;
        }
        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
        Thread::wait(500);
    }
}


/*
void feedbacktest(){
    //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);

    Kalman::State state;

    float Pgain = -0.01;
    float fwdspeed = -400/3.0f;
    Timer timer;
    timer.start();

    while(true) {
        float expecdist = fwdspeed * timer.read();
        state = Kalman::getState();
        float errleft = left_encoder.getTicks() - (expecdist);
        float errright = right_encoder.getTicks() - expecdist;

        mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
        mright(max(min(errright*Pgain, 0.4f), -0.4f));
    }
}
*/

void cakesensortest()
{
    wait(1);
    printf("cakesensortest");

    CakeSensor cs(P_DISTANCE_SENSOR);
    while(true) {
        wait(0.1);
        printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
    }
}

void colourtest()
{
    //Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN,UPPER);
    Colour c(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);

    while(true) {
        wait(0.1);

        ColourEnum ce = c.getColour();
        switch(ce) {
            case BLUE :
                printf("BLUE\n");
                break;
            case RED:
                printf("RED\n");
                break;
            case WHITE:
                printf("WHITE\n");
                break;
            case BLACK:
                printf("BLACK\n");
                break;
            default:
                printf("BUG\n");
        }

    }

}

void pt_test()
{
    DigitalOut _blue_led(p10);
    DigitalOut _red_led(p11);
    AnalogIn _pt(p19);
    
    bytepack_t datapackage;
    // first 3 bytes of header is used for alignment
    datapackage.data.header[0] = 0xFF;
    datapackage.data.header[1] = 0xFF;
    datapackage.data.header[2] = 0xFF;
    while(true) {
        //toggles leds for the next state
        _blue_led = 1;
        _red_led = 0;
        wait(0.01);
        volatile float blue_temp = _pt.read();


        datapackage.data.ID = (unsigned char)0;
        datapackage.data.value = blue_temp;
        datapackage.data.aux = 0;
        for (int i = 0; i < sizeof(datapackage); i++) {
            //mbed_serial.putc(datapackage.type_char[i]);
            pc.putc(datapackage.type_char[i]);
        }

        _blue_led = 0;
        _red_led = 1;
        wait(0.01);
        volatile float red_temp = _pt.read();


        datapackage.data.ID = (unsigned char)1;
        datapackage.data.value = red_temp;
        datapackage.data.aux = 0;
        for (int i = 0; i < sizeof(datapackage); i++) {
            //mbed_serial.putc(datapackage.type_char[i]);
            pc.putc(datapackage.type_char[i]);
        }

        _blue_led = 0;
        _red_led = 0;
        wait(0.01);
        volatile float noise_temp = _pt.read();


        datapackage.data.ID = (unsigned char)2;
        datapackage.data.value = noise_temp;
        datapackage.data.aux = 0;
        for (int i = 0; i < sizeof(datapackage); i++) {
            //mbed_serial.putc(datapackage.type_char[i]);
            pc.putc(datapackage.type_char[i]);
        }

    }
}



void armtest()
{
    Arm white(p26), black(p25, false, 0.0005, 180);
    while(true) {
        white(0);
        black(0);
        wait(1);
        white(1);
        black(1);
        wait(1);
    }
}


void motorsandservostest()
{
    Encoder Eleft(p27, p28), Eright(p30, p29);
    MainMotor mleft(p24,p23), mright(p21,p22);
    Arm sTop(p25), sBottom(p26);
    //Serial pc(USBTX, USBRX);
    const float speed = 0.0;
    const float dspeed = 0.0;

    Timer servoTimer;
    mleft(speed);
    mright(speed);
    servoTimer.start();
    while (true) {
        printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
        if (Eleft.getTicks() < Eright.getTicks()) {
            mleft(speed);
            mright(speed - dspeed);
        } else {
            mright(speed);
            mleft(speed - dspeed);
        }
        if (servoTimer.read() < 1) {
            sTop.clockwise();
        } else if (servoTimer.read() < 4) {
            sTop.halt();
        } else if (servoTimer.read() < 5) {
            sBottom.anticlockwise();
            //Led=1;
        } else if (servoTimer.read() < 6) {
            sBottom.clockwise();
            //Led=0;
        } else if (servoTimer.read() < 7) {
            sBottom.halt();
        } else {
            sTop.anticlockwise();
        }
        if (servoTimer.read() >= 9) servoTimer.reset();
    }
}

void motortestline()
{
    MainMotor mleft(p24,p23), mright(p21,p22);
    const float speed = 0.2;
    mleft(speed);
    mright(speed);
    while(true) wait(1);
}

void motorencodetestline()
{
    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
    //Serial pc(USBTX, USBRX);
    const float speed = 0.2;
    const float dspeed = 0.1;

    mleft(speed);
    mright(speed);
    while (true) {
        //left 27 cm = 113 -> 0.239 cm/pulse
        //right 27 cm = 72 -> 0.375 cm/pulse
        printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
            mright(speed - dspeed);
        } else {
            mright(speed + dspeed);
        }
    }

}

void motorencodetest()
{
    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
    Serial pc(USBTX, USBRX);

    const float speed = -0.3;
    const int enc = -38;
    while(true) {
        mleft(speed);
        mright(0);
        while(Eleft.getTicks()>enc) {
            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
        }
        Eleft.reset();
        Eright.reset();
        mleft(0);
        mright(speed);
        while(Eright.getTicks()>enc) {
            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
        }
        Eleft.reset();
        Eright.reset();
    }
}

void encodertest()
{
    Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
    //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
    Serial pc(USBTX, USBRX);
    while(true) {
        wait(0.1);
        printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
    }

}
void motortest()
{
    MainMotor mright(p22,p21), mleft(p23,p24);
    while(true) {
        wait(1);
        mleft(0.8);
        mright(0.8);
        wait(1);
        mleft(-0.2);
        mright(0.2);
        wait(1);
        mleft(0);
        mright(0);
    }
}