Revision 15:acae5c0e9ca8, committed 2012-04-28

Comitter:

narshu

Date:

Sat Apr 28 22:21:03 2012 +0000

Parent:

14:24f994dc2770

Child:

16:b3dd4e0b3100

Commit message:

functioning motion control code

Changed in this revision

--- a/Eurobot_shared/Kalman/Sonar/RFSRF05.cpp	Sat Apr 28 20:44:09 2012 +0000
+++ b/Eurobot_shared/Kalman/Sonar/RFSRF05.cpp	Sat Apr 28 22:21:03 2012 +0000
@@ -141,8 +141,8 @@
         ValidPulse = false;
 
         //Calucate distance
-        //true offset is about 330, we put 400 so circles overlap
-        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 400;
+        //true offset is about 330, we put 450 so circles overlap
+        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 450;
 
         if (callbackfunc)
             (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);

--- a/Eurobot_shared/Motion/motion.cpp	Sat Apr 28 20:44:09 2012 +0000
+++ b/Eurobot_shared/Motion/motion.cpp	Sat Apr 28 22:21:03 2012 +0000
@@ -1,5 +1,6 @@
 #include "motion.h"
 #include "geometryfuncs.h"
+#include "system.h"
 
 
 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
@@ -38,18 +39,22 @@
     float currX, currY,currTheta;
     float speedL,speedR;
     float diffDir;
+    int atTargetFlag = 0;
 
     while (1) {
+        kalman.statelock.lock();
         if (ai.flag_terminate) {
             terminate();
         }
 
         // get kalman localization estimate ------------------------
-        kalman.statelock.lock();
+        //kalman.statelock.lock();
         currX = kalman.X(0)*1000.0f;
         currY = kalman.X(1)*1000.0f;
         currTheta = kalman.X(2);
-        kalman.statelock.unlock();
+        //kalman.statelock.unlock();
+        
+        AI::Target loctarget = ai.gettarget();
 
         /*
         //PID Tuning Code
@@ -60,14 +65,14 @@
             PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
         }
         */
-
+        
         // check if target reached ----------------------------------
-        int localtargetnumber = ai.targetnumber;
-        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
-                &&( abs(currY - ai.target.y) < POSITION_TOR )
-           ) {
-
-            diffDir = rectifyAng(currTheta - ai.target.theta);
+        if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
+                
+            atTargetFlag = loctarget.reached;
+            OLED4 = 1;
+            
+            diffDir = rectifyAng(currTheta - loctarget.theta);
             //diffSpeed = diffDir / PI;
 
             PIDControllerMotorTheta.setProcessValue(diffDir);
@@ -76,20 +81,26 @@
 
             if (abs(diffDir) < ANGLE_TOR) {
 
-                static unsigned int lasttargetnumber = 1;
-                if (localtargetnumber > lasttargetnumber) {
+                if (!loctarget.reached) {
+                    static int counter = 10;
+                    if (counter-- == 0){
+                    counter = 10;
+                    ai.target.reached = true;
                     ai.thr_AI.signal_set(0x01);
-                    lasttargetnumber++;
+                    
+                    }
                 }
             }
         }
 
         // adjust motion to reach target ----------------------------
         else {
+        
+            OLED4 = 0;
 
             // calc direction to target
-            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
-            if (!ai.target.facing) targetDir =  targetDir - PI;
+            float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
+            if (!loctarget.facing) targetDir =  targetDir + PI;
 
             //Angle differene in -PI to PI
             diffDir = rectifyAng(currTheta - targetDir);
@@ -112,16 +123,22 @@
                 float tempPidVar = PIDControllerMotorTheta2.compute();
                 float MoveSpeedLimiter = 1;
                 //pc.printf("turn,%f\n",diffDir);
-                
-                float distanceToX = (float)abs(currX - ai.target.x);
-                float distanceToY = (float)abs(currY - ai.target.y);
+
+                float distanceToX = (float)abs(currX - loctarget.x);
+                float distanceToY = (float)abs(currY - loctarget.y);
+
+                float distanceToTarget = hypot(distanceToX, distanceToY);
+
+                if ((distanceToTarget < 400) && (distanceToTarget > 100)) {
+                    MoveSpeedLimiter = (distanceToTarget)/400;
+                }
+                else if((distanceToTarget < 400) && (distanceToTarget > 100)) {
+                    MoveSpeedLimiter  = 0.25;
+                }
                 
-                float distanceToTarget = sqrt(distanceToX*distanceToX+distanceToY*distanceToY);
+                
                 
-                if (distanceToTarget < 500) {
-                    MoveSpeedLimiter = (distanceToTarget)/500;
-                }             
-                
+
                 // calculte the motor speeds
                 if (tempPidVar >= 0) {
                     //turn left
@@ -133,16 +150,16 @@
                     speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
                     speedL = MOVE_SPEED*MoveSpeedLimiter;
                 }
-                
+
+
 
-                
-                
-                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
+
+                if (loctarget.facing) motors.setSpeed( int(speedL),  int(speedR));
                 else motors.setSpeed( -int(speedR),  -int(speedL));
 
             }
         }
-
+        kalman.statelock.unlock();
         // wait
         Thread::wait(MOTION_UPDATE_PERIOD);
     }

--- a/Eurobot_shared/ai/ai.cpp	Sat Apr 28 20:44:09 2012 +0000
+++ b/Eurobot_shared/ai/ai.cpp	Sat Apr 28 22:21:03 2012 +0000
@@ -5,25 +5,23 @@
 
 AI::AI() :
         thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
-        targetnumber = 0;
     flag_terminate = false;
     //printf("aistart\r\n");
 }
 
 void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing) {
     targetlock.lock();
-    targetnumber++;
     target.x = targetX;
     target.y = targetY;
     target.theta = targetTheta;
     target.facing = targetfacing;
+    target.reached = false;
     targetlock.unlock();
 }
 
 void AI::settarget(Target targetin) {
     targetlock.lock();
     target = targetin;
-    targetnumber++;
     targetlock.unlock();
 }
 

--- a/Eurobot_shared/ai/ai.h	Sat Apr 28 20:44:09 2012 +0000
+++ b/Eurobot_shared/ai/ai.h	Sat Apr 28 22:21:03 2012 +0000
@@ -10,13 +10,12 @@
 Mutex targetlock;
 Thread thr_AI;
 
-unsigned int targetnumber;
-
 struct Target {
     float x;
     float y;
     float theta;
     bool facing;
+    bool reached;
 } target;
 
 void settarget(float targetX, float targetY, float targetTheta, bool targetfacing = true);

--- a/globals.h	Sat Apr 28 20:44:09 2012 +0000
+++ b/globals.h	Sat Apr 28 22:21:03 2012 +0000
@@ -74,7 +74,7 @@
 //#define TRACK_RATE              10       // +- rate for each wheel when tracking
 
 #ifdef ROBOT_PRIMARY
-#define FWD_MOVE_P 16
+#define FWD_MOVE_P 14
 #define SPIN_MOVE_P 5.8
 #else
 #define FWD_MOVE_P 3.2