Diff: main.cpp

Revision:

1:bbabbd997d21

Parent:

0:f3bf6c7e2283

Child:

2:cffa347bb943

--- a/main.cpp	Fri Apr 20 20:39:35 2012 +0000
+++ b/main.cpp	Fri Apr 20 21:56:15 2012 +0000
@@ -6,6 +6,7 @@
 #include "motors.h"
 #include "math.h"
 #include "system.h"
+#include "geometryfuncs.h"
 
 //#include <iostream>
 
@@ -13,63 +14,77 @@
 //I2C i2c(p28, p27);        // sda, scl
 TSI2C i2c(p28, p27);
 Serial pc(USBTX, USBRX); // tx, rx
-Serial  IRturret(p13, p14);
+Serial  IRturret(p9, p10);
 
 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
+// bytes packing/unpacking for IR turret serial comm
 union IRValue_t {
-    float IR_floats[6];
-    int   IR_ints[6];
-    unsigned char IR_chars[24];
+    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);
-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);
+// Thread pointers
+Thread *AI_Thread_Ptr;
+Thread *Motion_Thread_Ptr;
 
 Mutex targetlock;
 bool flag_terminate = false;
 
-
 float temp = 0;
 
 //Main loop
 int main() {
-motors.stop();
     pc.baud(115200);
-    IRturret.baud(9600);
+    IRturret.baud(115200);
     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);
+    //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!!!!!!!!!!!!!!
@@ -79,18 +94,15 @@
     }
 }
 
-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);
+        motors.setSpeed(20,20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
+        motors.setSpeed(-20,-20);
         Thread::wait(2000);
         motors.stop();
         Thread::wait(5000);
@@ -98,6 +110,10 @@
         Thread::wait(2000);
         motors.stop();
         Thread::wait(5000);
+        motors.setSpeed(20,-20);
+        Thread::wait(2000);
+        motors.stop();
+        Thread::wait(5000);
     }
 }
 
@@ -105,41 +121,28 @@
 
 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;
+    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);
-        sonardist[0] = kalman.SonarMeasures[0]*1000.0f;
-        sonardist[1] = kalman.SonarMeasures[1]*1000.0f;
-        sonardist[2] = kalman.SonarMeasures[2]*1000.0f;
+        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();
-
-        // 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);
+        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);
     }
 }
 
@@ -183,7 +186,7 @@
     targetlock.lock();
     targetX     = 1500;
     targetY     = 1000;
-    targetTheta = -PI;
+    targetTheta = -PI/2;
     targetlock.unlock();
 
     // home
@@ -206,10 +209,10 @@
 // motion control thread ------------------------
 void motion_thread(void const *argument) {
     motors.resetEncoders();
-    motors.setSpeed(20,20);
-    Thread::wait(2000);
+    motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
+    Thread::wait(1000);
     motors.stop();
-    //thr_AI.signal_set(0x01);
+    (*AI_Thread_Ptr).signal_set(0x01);
 
 
 
@@ -231,17 +234,6 @@
         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 )
@@ -250,7 +242,6 @@
             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);
@@ -261,7 +252,7 @@
             } else {
                 motors.stop();
                 Thread::wait(4000);
-                //thr_AI.signal_set(0x01);
+                (*AI_Thread_Ptr).signal_set(0x01);
             }
         }
 
@@ -311,64 +302,78 @@
     }
 }
 
+void vIRValueISR (void) {
 
-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();
+    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 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);
+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;
+        }
     }
-
-    // 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) );
-    }
-
-}
-
-
+    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