Diff: main.cpp

Revision:

2:cffa347bb943

Parent:

1:bbabbd997d21

Child:

4:7b7334441da9

--- a/main.cpp	Fri Apr 20 21:56:15 2012 +0000
+++ b/main.cpp	Thu Apr 26 21:02:12 2012 +0000
@@ -7,41 +7,20 @@
 #include "math.h"
 #include "system.h"
 #include "geometryfuncs.h"
+#include "ai.h"
+#include "ui.h"
 
 //#include <iostream>
 
 //Interface declaration
-//I2C i2c(p28, p27);        // sda, scl
-TSI2C i2c(p28, p27);
 Serial pc(USBTX, USBRX); // tx, rx
-Serial  IRturret(p9, p10);
-
-DigitalOut     OLED1(LED1);
-DigitalOut     OLED2(LED2);
-DigitalOut     OLED3(LED3);
-DigitalOut     OLED4(LED4);
 
 Motors motors;
-Kalman kalman(motors);
+UI ui;
 
-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);
+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!
@@ -49,42 +28,30 @@
 
 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;
+//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();
+    //Init kalman
+    kalman.KalmanInit();
 
     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!!!!!!!!!!!!!!
@@ -141,80 +108,18 @@
         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);
+        pc.printf("Angle_Offset: %0.4f \r\n",kalman.ir.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.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
+    //Thread::wait(1000);
     motors.stop();
-    (*AI_Thread_Ptr).signal_set(0x01);
-
-
+    ai.thr_AI.signal_set(0x01);
 
     float currX, currY,currTheta;
     float speedL,speedR;
@@ -222,7 +127,7 @@
     float lastdiffSpeed = 0;
 
     while (1) {
-        if (flag_terminate) {
+        if (ai.flag_terminate) {
             terminate();
         }
 
@@ -235,11 +140,11 @@
 
 
         // check if target reached ----------------------------------
-        if (  ( abs(currX - targetX) < POSITION_TOR )
-                &&( abs(currY - targetY) < POSITION_TOR )
+        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
+                &&( abs(currY - ai.target.y) < POSITION_TOR )
            ) {
 
-            diffDir = rectifyAng(currTheta - targetTheta);
+            diffDir = rectifyAng(currTheta - ai.target.theta);
             diffSpeed = diffDir / PI;
 
             if (abs(diffDir) > ANGLE_TOR) {
@@ -252,7 +157,7 @@
             } else {
                 motors.stop();
                 Thread::wait(4000);
-                (*AI_Thread_Ptr).signal_set(0x01);
+                ai.thr_AI.signal_set(0x01);
             }
         }
 
@@ -260,7 +165,9 @@
         else {
 
             // calc direction to target
-            float targetDir = atan2(targetY - currY, targetX - currX);
+            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
+            if (!ai.target.facing)
+                targetDir = PI - targetDir;
 
 
             diffDir = rectifyAng(currTheta - targetDir);
@@ -292,88 +199,14 @@
                     speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
                     speedL = MOVE_SPEED;
                 }
-
-                motors.setSpeed( int(speedL),  int(speedR));
+                if (ai.target.facing)
+                    motors.setSpeed( int(speedL),  int(speedR));
+                else
+                    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