File content as of revision 2:cffa347bb943:

#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 "ai.h"
#include "ui.h"

//#include <iostream>

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

Motors motors;
UI ui;


Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11,p9,p10);
AI ai;

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

//bool flag_terminate = false;

float temp = 0;

//Main loop
int main() {
    pc.baud(115200);
    //Init kalman
    kalman.KalmanInit();

    Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
    Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
    
    
    
    
    //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
    //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",kalman.ir.angleOffset*180/PI);
        Thread::wait(100);
    }
}

// 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.thr_AI.signal_set(0x01);

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

    while (1) {
        if (ai.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 - ai.target.x) < POSITION_TOR )
                &&( abs(currY - ai.target.y) < POSITION_TOR )
           ) {

            diffDir = rectifyAng(currTheta - ai.target.theta);
            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.thr_AI.signal_set(0x01);
            }
        }

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

            // calc direction to target
            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
            if (!ai.target.facing)
                targetDir = PI - targetDir;


            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;
                }
                if (ai.target.facing)
                    motors.setSpeed( int(speedL),  int(speedR));
                else
                    motors.setSpeed( -int(speedL),  -int(speedR));
            }
        }

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