Revision 13:57ea4e520dbd, committed 2012-04-28

Comitter:

narshu

Date:

Sat Apr 28 19:39:08 2012 +0000

Parent:

12:2981367c63a0

Parent:

11:ea2112ae3c4a

Child:

14:24f994dc2770

Commit message:

PID tuned. Modified ai signalling logic at target location.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eurobot_shared/Motion/motion.cpp	Sat Apr 28 19:39:08 2012 +0000
@@ -0,0 +1,133 @@
+#include "motion.h"
+#include "geometryfuncs.h"
+
+
+Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
+        thr_motion(mtwrapper,this,osPriorityNormal,1024),
+        motors(motorsin),
+        ai(aiin),
+        kalman(kalmanin) { }
+
+// motion control thread ------------------------
+void Motion::motion_thread() {
+    motors.resetEncoders();
+    motors.stop();
+    Thread::wait(1500);
+    ai.thr_AI.signal_set(0x01);
+
+    //PID declare
+    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
+    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
+
+    //PID Initialisation
+    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
+    PIDControllerMotorTheta.setMode(MANUAL_MODE);
+
+    PIDControllerMotorTheta2.setBias(0);
+    PIDControllerMotorTheta.setBias(0);
+
+    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
+    PIDControllerMotorTheta.setOutputLimits(-1, 1);
+
+    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
+    PIDControllerMotorTheta.setInputLimits(-PI, PI);
+
+    PIDControllerMotorTheta.setSetPoint(0);
+    PIDControllerMotorTheta2.setSetPoint(0);
+
+    float currX, currY,currTheta;
+    float speedL,speedR;
+    float diffDir;
+
+    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();
+
+        /*
+        //PID Tuning Code
+        if (pc.readable() == 1) {
+            float cmd;
+            pc.scanf("%f", &cmd);
+            //Tune PID referece
+            PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
+        }
+        */
+
+        // 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;
+
+            PIDControllerMotorTheta.setProcessValue(diffDir);
+            float tempPidVar = PIDControllerMotorTheta.compute();
+            motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
+
+            if (abs(diffDir) < ANGLE_TOR) {
+
+                static unsigned int lasttargetnumber = 0;
+                if (ai.targetnumber > lasttargetnumber) {
+                    ai.thr_AI.signal_set(0x01);
+                    lasttargetnumber++;
+                }
+            }
+        }
+
+        // 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 =  targetDir - PI;
+
+            //Angle differene in -PI to PI
+            diffDir = rectifyAng(currTheta - targetDir);
+
+            //Set PID process variable
+            PIDControllerMotorTheta.setProcessValue(diffDir);
+            PIDControllerMotorTheta2.setProcessValue(diffDir);
+
+            //if diffDIr is neg, spin right
+            //if diffDir is pos, spin left
+
+            if (abs(diffDir) > ANGLE_TOR*4) {   //roughly 32 degrees
+                //ANGLE_TOR*4
+                float tempPidVar = PIDControllerMotorTheta.compute();
+                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
+                //pc.printf("spin,%f\n",diffDir);
+
+            } else {
+
+                float tempPidVar = PIDControllerMotorTheta2.compute();
+                //pc.printf("turn,%f\n",diffDir);
+                // calculte the motor speeds
+                if (tempPidVar >= 0) {
+                    //turn left
+                    speedL = (1-abs(tempPidVar))*MOVE_SPEED;
+                    speedR = MOVE_SPEED;
+
+                } else {
+                    //turn right
+                    speedR = (1-abs(tempPidVar))*MOVE_SPEED;
+                    speedL = MOVE_SPEED;
+                }
+                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
+                else motors.setSpeed( -int(speedR),  -int(speedL));
+
+            }
+        }
+
+        // wait
+        Thread::wait(MOTION_UPDATE_PERIOD);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eurobot_shared/Motion/motion.h	Sat Apr 28 19:39:08 2012 +0000
@@ -0,0 +1,18 @@
+#include "motors.h"
+#include "ai.h"
+#include "Kalman.h"
+
+class Motion {
+public:
+    Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin);
+    Thread thr_motion;
+
+private:
+    Motors& motors;
+    AI& ai;
+    Kalman& kalman;
+    
+    void motion_thread();
+    static void mtwrapper(void const *arg){ ((Motion*)arg)->motion_thread(); }
+
+};
\ No newline at end of file

--- a/Eurobot_shared/ai/ai.cpp	Sat Apr 28 18:26:26 2012 +0000
+++ b/Eurobot_shared/ai/ai.cpp	Sat Apr 28 19:39:08 2012 +0000
@@ -5,6 +5,7 @@
 
 AI::AI() :
         thr_AI(aithreadwrapper,this,osPriorityNormal,1024) {
+        targetnumber = 0;
     flag_terminate = false;
     //printf("aistart\r\n");
 }
@@ -15,12 +16,14 @@
     target.y = targetY;
     target.theta = targetTheta;
     target.facing = targetfacing;
+    targetnumber++;
     targetlock.unlock();
 }
 
 void AI::settarget(Target targetin) {
     targetlock.lock();
     target = targetin;
+    targetnumber++;
     targetlock.unlock();
 }
 

--- a/Eurobot_shared/ai/ai.h	Sat Apr 28 18:26:26 2012 +0000
+++ b/Eurobot_shared/ai/ai.h	Sat Apr 28 19:39:08 2012 +0000
@@ -10,6 +10,8 @@
 Mutex targetlock;
 Thread thr_AI;
 
+unsigned int targetnumber;
+
 struct Target {
     float x;
     float y;

--- a/Eurobot_shared/ui/ui.cpp	Sat Apr 28 18:26:26 2012 +0000
+++ b/Eurobot_shared/ui/ui.cpp	Sat Apr 28 19:39:08 2012 +0000
@@ -61,7 +61,8 @@
 
 void UI::printloop() {
 
-    Thread::wait(1500);
+    //Thread::wait(1500);
+    Thread::wait(osWaitForever);
 
     char* sync = "ABCD";
     std::cout.write(sync, 4);

--- a/globals.h	Sat Apr 28 18:26:26 2012 +0000
+++ b/globals.h	Sat Apr 28 19:39:08 2012 +0000
@@ -71,8 +71,8 @@
 //#define TRACK_RATE              10       // +- rate for each wheel when tracking
 
 #ifdef ROBOT_PRIMARY
-#define FWD_MOVE_P 1
-#define SPIN_MOVE_P 1.3
+#define FWD_MOVE_P 16
+#define SPIN_MOVE_P 5.8
 #else
 #define FWD_MOVE_P 3.2
 #define SPIN_MOVE_P 4

--- a/main.cpp	Sat Apr 28 18:26:26 2012 +0000
+++ b/main.cpp	Sat Apr 28 19:39:08 2012 +0000
@@ -89,33 +89,52 @@
 
     flag_terminate = true;
     */
-
+    
     while (1) {
 
         // goes to the mid
+        settarget(1500, 1000, PI/2, true);
         Thread::signal_wait(0x01);
-        settarget(1500, 1000, PI/2, true);
+        Thread::wait(2000);
 
         // left roll
+        settarget(500, 1500, PI/2, true);
         Thread::signal_wait(0x01);
-        settarget(500, 1500, PI/2, true);
-
+        Thread::wait(2000);
+        
         // mid
+        settarget(1500, 1000, PI/2, true);
         Thread::signal_wait(0x01);
-        settarget(1500, 1000, PI/2, true);
+        Thread::wait(2000);
 
         // map
+        settarget(1500, 1500, PI/2, true);
         Thread::signal_wait(0x01);
-        settarget(1500, 1500, PI/2, true);
+        Thread::wait(2000);
+        
 
         // mid
+        settarget(1500, 1000, -PI/2, true);
         Thread::signal_wait(0x01);
-        settarget(1500, 1000, -PI/2, true);
+        Thread::wait(2000);
+        
 
         // home
+        settarget(500, 500, 0, true);
         Thread::signal_wait(0x01);
-        settarget(500, 500, 0, true);
-
+        Thread::wait(2000);
+        
+        // oponents base
+        settarget(2500, 500, 0, true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+        
+        // oponents ship
+        settarget(2500, 1500, 0, true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+        
+        
     }
 
     Thread::signal_wait(0x01);

--- a/motion.cpp	Sat Apr 28 18:26:26 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,130 +0,0 @@
-#include "motion.h"
-#include "geometryfuncs.h"
-
-Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
-    thr_motion(mtwrapper,this,osPriorityNormal,1024),
-    motors(motorsin),
-    ai(aiin),
-    kalman(kalmanin) { }
-
-// motion control thread ------------------------
-void Motion::motion_thread() {
-    motors.resetEncoders();
-    motors.stop();
-    ai.thr_AI.signal_set(0x01);
-
-    //PID declare
-    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
-    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
-
-    //PID Initialisation
-    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
-    PIDControllerMotorTheta.setMode(MANUAL_MODE);
-
-    PIDControllerMotorTheta2.setBias(0);
-    PIDControllerMotorTheta.setBias(0);
-
-    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
-    PIDControllerMotorTheta.setOutputLimits(-1, 1);
-
-    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
-    PIDControllerMotorTheta.setInputLimits(-PI, PI);
-
-    PIDControllerMotorTheta.setSetPoint(0);
-    PIDControllerMotorTheta2.setSetPoint(0);
-
-    float currX, currY,currTheta;
-    float speedL,speedR;
-    float diffDir;
-    
-    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();
-        
-       
-       /* 
-        if (pc.readable() == 1) {
-            pc.scanf("%f", &cmd);
-            //Tune PID referece
-               PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
-        }
-        */  
-
-        // 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;
-
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-
-            if (abs(diffDir) > ANGLE_TOR) {
-            
-                float tempPidVar = PIDControllerMotorTheta.compute();
-                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*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 =  targetDir - PI;
-
-            //Angle differene in -PI to PI
-            diffDir = rectifyAng(currTheta - targetDir);
-
-            //Set PID process variable
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-            PIDControllerMotorTheta2.setProcessValue(diffDir);
-
-            //if diffDIr is neg, spin right
-            //if diffDir is pos, spin left
-
-            if (abs(diffDir) > ANGLE_TOR*4) {   //roughly 32 degrees
-                //ANGLE_TOR*4
-                float tempPidVar = PIDControllerMotorTheta.compute();
-                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-                // pc.printf("spin,%f\n",diffDir);
-
-            } else {
-             
-             float tempPidVar = PIDControllerMotorTheta2.compute();
-             //pc.printf("turn,%f\n",diffDir);
-                // calculte the motor speeds
-                if (tempPidVar >= 0) {
-                    //turn left
-                    speedL = (1-abs(tempPidVar))*MOVE_SPEED;
-                    speedR = MOVE_SPEED;
-
-                } else {
-                    //turn right
-                    speedR = (1-abs(tempPidVar))*MOVE_SPEED;
-                    speedL = MOVE_SPEED;
-                }
-                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
-                else motors.setSpeed( -int(speedR),  -int(speedL));
-                
-            }
-        }
-
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file

--- a/motion.h	Sat Apr 28 18:26:26 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,18 +0,0 @@
-#include "motors.h"
-#include "ai.h"
-#include "Kalman.h"
-
-class Motion {
-public:
-    Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin);
-    Thread thr_motion;
-
-private:
-    Motors& motors;
-    AI& ai;
-    Kalman& kalman;
-    
-    void motion_thread();
-    static void mtwrapper(void const *arg){ ((Motion*)arg)->motion_thread(); }
-
-};
\ No newline at end of file