Diff: main.cpp

Revision:

0:f3bf6c7e2283

Child:

1:bbabbd997d21

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,374 @@
+#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 <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);
+
+int16_t face;
+
+
+Motors motors;
+Kalman kalman(motors);
+
+float targetX = 1000, targetY = 1000, targetTheta = 0;
+
+// bytes packing for IR turret serial comm
+union IRValue_t {
+    float IR_floats[6];
+    int   IR_ints[6];
+    unsigned char IR_chars[24];
+} IRValues;
+
+//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);
+
+Mutex targetlock;
+bool flag_terminate = false;
+
+
+float temp = 0;
+
+//Main loop
+int main() {
+motors.stop();
+    pc.baud(115200);
+    IRturret.baud(9600);
+    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);
+    Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
+
+    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 motorSpeedThread(void const *argument) {
+    while (1) {
+        motors.speedRegulatorTask();
+        Thread::wait(10);
+    }
+}
+
+
+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);
+    }
+}
+
+
+
+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;
+
+    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;
+        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);
+    }
+}
+
+
+// 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;
+    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(20,20);
+    Thread::wait(2000);
+    motors.stop();
+    //thr_AI.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();
+
+
+
+        // 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 )
+           ) {
+
+            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);
+                }
+                motors.setSpeed( int(diffSpeed*MOVE_SPEED),  -int(diffSpeed*MOVE_SPEED));
+
+
+            } else {
+                motors.stop();
+                Thread::wait(4000);
+                //thr_AI.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 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();
+        }
+    }
+}
+
+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);
+    }
+
+    // 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) );
+    }
+
+}
+
+