Diff: main.cpp

Revision:

0:fbfafa6bf5f9

Child:

1:cc2a9eb0bd55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,379 @@
+#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);
+}
\ No newline at end of file