File content as of revision 0:f3bf6c7e2283:

#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 <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);

int16_t face;


Motors motors;
Kalman kalman(motors);

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

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

//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);
void vIRValueISR (void);
void motorSpeedThread(void const *argument);

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


//Thread thr_AI(ai_thread);
//Thread thr_motion(motion_thread);
//Thread thr_getIRValue(getIRValue);

Mutex targetlock;
bool flag_terminate = false;


float temp = 0;

//Main loop
int main() {
motors.stop();
    pc.baud(115200);
    IRturret.baud(9600);
    IRturret.format(8,Serial::Odd,1);
    //Thread thread(motorSpeedThread, NULL, osPriorityAboveNormal); //PID controller task. run at 50ms
    //IRturret.attach(&vIRValueISR);
    //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
    Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);

    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 motorSpeedThread(void const *argument) {
    while (1) {
        motors.speedRegulatorTask();
        Thread::wait(10);
    }
}


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);
    }
}



void vPrintState(void const *argument) {
    float state[3];
    float sonardist[3];
    //float x,y;
    //float d = beaconpos[2].y - beaconpos[1].y;
    //float i = beaconpos[0].y;
    //float j = beaconpos[0].x;

    while (1) {
        kalman.statelock.lock();
        state[0] = kalman.X(0);
        state[1] = kalman.X(1);
        state[2] = kalman.X(2);
        sonardist[0] = kalman.SonarMeasures[0]*1000.0f;
        sonardist[1] = kalman.SonarMeasures[1]*1000.0f;
        sonardist[2] = kalman.SonarMeasures[2]*1000.0f;
        kalman.statelock.unlock();

        // trilateration algorithm
        //y = ((sonardist[1]*sonardist[1]) - (sonardist[2]*sonardist[2]) + d*d) / (2*d);
        //x = ((sonardist[1]*sonardist[1]) - (sonardist[0]*sonardist[0]) + i*i + j*j) / (2*j) - (i/j)*y;


        //kalman.statelock.lock();
        pc.printf("\r\n");
        //printf("Position X: %0.1f Y: %0.1f \r\n", x,y);
        pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
        //targetlock.lock();
        //pc.printf("target : %0.4f %0.4f %0.4f \r\n", targetX/1000, targetY/1000,targetTheta);
        //targetlock.unlock();
        printf("Sonar Dist: %0.4f, %0.4f, %0.4f \r\n", sonardist[0],sonardist[1],sonardist[2]);

        //printf("Distance is %0.1f, %0.1f \r\n", motors.encoderToDistance(motors.getEncoder1()), motors.encoderToDistance(motors.getEncoder2()));

        //pc.printf("IR ang : %f \n",temp * 180/3.14);
        //kalman.statelock.unlock();
        Thread::wait(200);
    }
}


// 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;
    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(20,20);
    Thread::wait(2000);
    motors.stop();
    //thr_AI.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();



        // get IR turret angle --------------------------------------
        /*        float angEst = getAngle(currX, currY);          // check this
                if (angEst != 1000){
                    // here the value should be a valid estimate
                    // so update kalman state

                }
        */
        //!!! add code here to update kalman angle state !!!

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

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

            //pc.printf("%f \n",diffDir);
            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);
                //thr_AI.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 getIRValue(void const *argument) {
    Thread::signal_wait(0x01);
    if (IRturret.readable() >= 24) {
        for (int i=0; i < 24; i++) {
            IRValues.IR_chars[i] = IRturret.getc();
        }
    }
}

void vIRValueISR (void) {
    //thr_getIRValue.signal_set(0x01);
}


float getAngle (float x, float y) {
    /*  function that returns current robot orientation
        input:  x, y coords in mm
        output: orientation in rad
        note: the angle readings from IR is read in here
            must be called faster than IR turret frequency (few Hz - currently 3Hz)
    */

    // variables
    float   ang0 = 0, ang1 = 0, ang2 = 0;
    int     sc0 = 0, sc1 = 0, sc2 = 0;



    // read serial port
    if (IRturret.readable()) {
        IRturret.scanf("%f;%f;%f;%d;%d;%d;",&ang0,&ang1,&ang2,&sc0,&sc1,sc2);
    }

    // if none sampled
    if (!sc0 && !sc1 && ! sc2) {
        return 1000;
    }

    //
    else {
        float est0 = 0, est1 = 0, est2 = 0;

        // calc
        if ((sc0 > RELI_BOUND_LOW)||(sc0 < RELI_BOUND_HIGH)) {
            est0 = atan2(1000 - y, 3000-x) - ang0;
        }
        if ((sc1 > RELI_BOUND_LOW)||(sc1 < RELI_BOUND_HIGH)) {
            est1 = atan2( -y, -x) - ang1;
        }
        if ((sc2 > RELI_BOUND_LOW)||(sc2 < RELI_BOUND_HIGH)) {
            est2 = atan2(2000 - y, -x) - ang2;
        }

        // weighted avg
        return rectifyAng( ((est0 * RELI_BOUND_HIGH-sc0) + (est1 * RELI_BOUND_HIGH-sc1) + (est2 * RELI_BOUND_HIGH-sc2)) / (3*RELI_BOUND_HIGH - sc0 - sc1 - sc2) );
    }

}