File content as of revision 0:fbfafa6bf5f9:

#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 <iostream>

//Interface declaration
//I2C i2c(p28, p27);        // sda, scl
TSI2C i2c(p28, p27);
Serial pc(USBTX, USBRX); // tx, rx
Serial  IRturret(p13, p14);

DigitalOut     OLED1(LED1);
DigitalOut     OLED2(LED2);
DigitalOut     OLED3(LED3);
DigitalOut     OLED4(LED4);

Motors motors(i2c);
Kalman kalman(motors);

float targetX = 1000, targetY = 1000, targetTheta = 0;

// bytes packing/unpacking for IR turret serial comm
union IRValue_t {
    float IR_floats[3];
    int IR_ints[3];
    unsigned char IR_chars[12];
} IRValues;

char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
int Alignment_ptr = 0;
bool data_flag = false;
int buff_pointer = 0;
bool angleInit = false;
float angleOffset = 0;

void vIRValueISR (void);
void vKalmanInit(void);

//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
//NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!


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


float getAngle (float x, float y);
void getIRValue(void const *argument);

// Thread pointers
Thread *AI_Thread_Ptr;
Thread *Motion_Thread_Ptr;

Mutex targetlock;
bool flag_terminate = false;

float temp = 0;

//Main loop
int main() {
    pc.baud(115200);
    IRturret.baud(115200);
    IRturret.format(8,Serial::Odd,1);
    IRturret.attach(&vIRValueISR,Serial::RxIrq);
    vKalmanInit();

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

    Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024);
    Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
    AI_Thread_Ptr = &thr_AI;
    Motion_Thread_Ptr = &thr_motion;

    //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");

    //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
    while (1) {
        // do nothing
        Thread::wait(osWaitForever);
    }
}


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]);
        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);
        pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI);
        Thread::wait(100);
    }
}


// AI thread ------------------------------------
void ai_thread (void const *argument) {
    // goes to the mid
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 1500;
    targetY     = 1000;
    targetTheta = PI/2;
    targetlock.unlock();

    // left roll
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 500;
    targetY     = 1700;
    targetTheta = PI/2;
    targetlock.unlock();

    // mid
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 1500;
    targetY     = 1000;
    targetTheta = PI/2;
    targetlock.unlock();

    // map
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 1500;
    targetY     = 1700;
    targetTheta = PI/2;
    targetlock.unlock();

    // mid
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 1500;
    targetY     = 1000;
    targetTheta = -PI/2;
    targetlock.unlock();

    // home
    Thread::signal_wait(0x01);
    targetlock.lock();
    targetX     = 500;
    targetY     = 500;
    targetTheta = 0;
    targetlock.unlock();

    Thread::signal_wait(0x01);
    flag_terminate = true;
    //OLED3 = true;

    while (true) {
        Thread::wait(osWaitForever);
    }
}

// motion control thread ------------------------
void motion_thread(void const *argument) {
    motors.resetEncoders();
    motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
    Thread::wait(1000);
    motors.stop();
    (*AI_Thread_Ptr).signal_set(0x01);



    float currX, currY,currTheta;
    float speedL,speedR;
    float diffDir,diffSpeed;
    float lastdiffSpeed = 0;

    while (1) {
        if (flag_terminate) {
            terminate();
        }

        // get kalman localization estimate ------------------------
        kalman.statelock.lock();
        currX = kalman.X(0)*1000.0f;
        currY = kalman.X(1)*1000.0f;
        currTheta = kalman.X(2);
        kalman.statelock.unlock();


        // check if target reached ----------------------------------
        if (  ( abs(currX - targetX) < POSITION_TOR )
                &&( abs(currY - targetY) < POSITION_TOR )
           ) {

            diffDir = rectifyAng(currTheta - targetTheta);
            diffSpeed = diffDir / PI;

            if (abs(diffDir) > ANGLE_TOR) {
                if (abs(diffSpeed) < 0.5) {
                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
                }
                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));


            } else {
                motors.stop();
                Thread::wait(4000);
                (*AI_Thread_Ptr).signal_set(0x01);
            }
        }

        // adjust motion to reach target ----------------------------
        else {

            // calc direction to target
            float targetDir = atan2(targetY - currY, targetX - currX);


            diffDir = rectifyAng(currTheta - targetDir);
            diffSpeed = diffDir / PI;

            if (abs(diffDir) > ANGLE_TOR*2) {
                if (abs(diffSpeed) < 0.5) {
                    diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
                }
                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
            } else {


                if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO  ) {
                    if (diffSpeed-lastdiffSpeed >= 0) {
                        diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
                    } else {
                        diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
                    }
                }
                lastdiffSpeed = diffSpeed;

                // calculte the motor speeds
                if (diffSpeed <= 0) {
                    speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
                    speedR = MOVE_SPEED;

                } else {
                    speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
                    speedL = MOVE_SPEED;
                }

                motors.setSpeed( int(speedL),  int(speedR));
            }
        }

        // wait
        Thread::wait(MOTION_UPDATE_PERIOD);
    }
}

void vIRValueISR (void) {

    OLED3 = !OLED3;
    // A workaround for mbed UART ISR bug
    // Clear the RBR flag to make sure the interrupt doesn't loop
    // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
    // UART0 for USB UART
    unsigned char RBR = LPC_UART1->RBR;


    if (!data_flag) { // look for alignment bytes
        if (RBR == Alignment_char[Alignment_ptr]) {
            Alignment_ptr ++;
        }
        if (Alignment_ptr >= 4) {
            Alignment_ptr = 0;
            data_flag = true; // set the dataflag
        }
    } else { // fetch data bytes
        IRValues.IR_chars[buff_pointer] = RBR;
        buff_pointer ++;
        if (buff_pointer >= 12) {
            buff_pointer = 0;
            data_flag = false; // dessert the dataflag
            if (angleInit) {
                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]);
            } else {
                kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
            }
        }

    }
}

void vKalmanInit(void) {
    float SonarMeasures[3];
    float IRMeasures[3];
    int beacon_cnt = 0;
    wait(1);
    IRturret.attach(NULL,Serial::RxIrq);
    kalman.statelock.lock();
    SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f;
    SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f;
    SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f;
    IRMeasures[0] = kalman.IRMeasures[0];
    IRMeasures[1] = kalman.IRMeasures[1];
    IRMeasures[2] = kalman.IRMeasures[2];
    kalman.statelock.unlock();
    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]*180/PI, IRMeasures[1]*180/PI, IRMeasures[2]*180/PI);
    float d = beaconpos[2].y - beaconpos[1].y;
    float i = beaconpos[0].y - beaconpos[1].y;
    float j = beaconpos[0].x - beaconpos[1].x;
    float y_coor = (SonarMeasures[1]*SonarMeasures[1]- SonarMeasures[2]*SonarMeasures[2] + d*d) / (2*d);
    float x_coor = (SonarMeasures[1]*SonarMeasures[1] - SonarMeasures[0]*SonarMeasures[0] + i*i + j*j)/(2*j) - i*y_coor/j;
    angleOffset = 0;
    for (int i = 0; i < 3; i++) {
        float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
        if (IRMeasures[i] != 0){
           beacon_cnt ++;
           float angle_temp = angle_est - IRMeasures[i];
           angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; 
           angleOffset += angle_temp;
        }
    }
    angleOffset = angleOffset/float(beacon_cnt);
    //printf("\n\r");
    angleInit = true;
    kalman.statelock.lock();
    kalman.X(0) = x_coor/1000.0f;
    kalman.X(1) = y_coor/1000.0f;
    kalman.X(2) = 0;
    kalman.statelock.unlock();
    //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
    IRturret.attach(&vIRValueISR,Serial::RxIrq);
}