Diff: main.cpp

Revision:

10:294b9adbc9d3

Parent:

9:377560539b74

Child:

13:57ea4e520dbd

--- a/main.cpp	Sat Apr 28 17:21:24 2012 +0000
+++ b/main.cpp	Sat Apr 28 18:10:55 2012 +0000
@@ -7,6 +7,7 @@
 #include "math.h"
 #include "system.h"
 #include "geometryfuncs.h"
+#include "motion.h"
 #include "ai.h"
 #include "ui.h"
 
@@ -17,10 +18,9 @@
 
 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!
@@ -43,7 +43,7 @@
     //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
     //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
     
-    Thread thr_motion(motion_thread,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
@@ -68,7 +68,7 @@
         }
         
         // do nothing
-        Thread::wait(osWaitForever);
+        //Thread::wait(osWaitForever);
     }
 }
 
@@ -151,7 +151,6 @@
 }
 
 
-
 void vPrintState(void const *argument) {
     float state[3];
     float SonarMeasures[3];
@@ -177,101 +176,3 @@
         Thread::wait(100);
     }
 }
-
-// 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.thr_AI.signal_set(0x01);
-
-    float currX, currY,currTheta;
-    float speedL,speedR;
-    float diffDir,diffSpeed;
-    float lastdiffSpeed = 0;
-
-    while (1) {
-        if (ai.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 - ai.target.x) < POSITION_TOR )
-                &&( abs(currY - ai.target.y) < POSITION_TOR )
-           ) {
-
-            diffDir = rectifyAng(currTheta - ai.target.theta);
-            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.thr_AI.signal_set(0x01);
-            }
-        }
-
-        // adjust motion to reach target ----------------------------
-        else {
-
-            // calc direction to target
-            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
-            if (!ai.target.facing)
-                targetDir = PI - targetDir;
-
-
-            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;
-                }
-                if (ai.target.facing)
-                    motors.setSpeed( int(speedL),  int(speedR));
-                else
-                    motors.setSpeed( -int(speedL),  -int(speedR));
-            }
-        }
-
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file