File content as of revision 19:06610e1c0895:

#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

Motors motors;
UI ui;
Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
AI ai;
Motion motion(motors, ai, kalman);

//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) {
        
        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 () {
    /*
    //printf("aithreadstart\r\n");
    Thread::signal_wait(0x01);
    settarget(660, 400, PI/2, true);

    Thread::signal_wait(0x01);
    settarget(660, 570, PI, true);

    Thread::signal_wait(0x01);
    settarget(400, 870, PI, true);

    Thread::signal_wait(0x01);
    settarget(660, 870, PI, false);

    flag_terminate = true;
    */
    
    // strat 1 ==================================
    // goto middle x
    settarget(1500, 250, PI/2, true);
    Thread::signal_wait(0x01);
    Thread::wait(2000);
    
    // to palm tree
    settarget(1500, 1000, PI, true);
    Thread::signal_wait(0x01);
    Thread::wait(2000);
    
    // run over totem
    settarget(640,1000,PI, true);
    Thread::signal_wait(0x01);
    Thread::wait(2000);
    
    // back to ship
    settarget(220,780,PI,true);
    Thread::signal_wait(0x01);
    Thread::wait(2000);
    
    flag_terminate = true;
    while(true){
        Thread::wait(osWaitForever);
    }
    
    
    // end of strat 1 ===========================
    
    
   
    
    while (1) {
    
    
            // goes to the mid
        settarget(500, 1000, 0, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        settarget(2500, 1000, PI, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        }


        
                    // goes to the mid
        settarget(700, 1500, 0, false);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
//////////////////////////////////////////////////////
        // goes to the mid
        settarget(1500, 1000, PI/2, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);

        // left roll
        settarget(500, 1500, PI/2, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        // mid
        settarget(1500, 1000, PI/2, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);

        // map
        settarget(1500, 1500, PI/2, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        

        // mid
        settarget(1500, 1000, -PI/2, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        

        // home
        settarget(500, 500, 0, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        // oponents base
        settarget(2500, 500, 0, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        // oponents ship
        settarget(2500, 1500, 0, true);
        Thread::signal_wait(0x01);
        Thread::wait(2000);
        
        
    //}

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

    while (true) {
        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);
        Thread::wait(100);
    }
}