Diff: AIControl.h

Revision:

6:3fb9f96765f6

Parent:

5:210cd333f770

--- a/AIControl.h	Wed Apr 30 05:53:51 2014 +0000
+++ b/AIControl.h	Wed Apr 30 12:23:04 2014 +0000
@@ -76,11 +76,12 @@
 }
 
 //Turn left about the center
-void centerTurnLeft(int delta_degrees)
+void centerTurnLeft(bool gameRun)
 {
     //Reset the filter and re-init the IMU
     imuFilter.reset();
     rpy_init();
+    int delta_degrees = 0;
 
     //So there's this weird thing where the gyro treats left turns as POSITIVE degrees...
     float initial = toDegrees(imuFilter.getYaw());
@@ -91,20 +92,25 @@
     //Continue to turn to target
     while (!gameOver) {
         setTurnLeft(0.6);
+        if(gameRun) {
+            receivePosition(NULL);
+        }
         sample = toDegrees(imuFilter.getYaw());
         wait(0.02);
-        if(sample > target - 5)
+        if((sonar3.read()*100) > (SONAR_STOP+6))
             break;
     }
     stop();
 }
 
 //Turn right about the center
-void centerTurnRight(int delta_degrees)
+void centerTurnRight(bool gameRun)
 {
     //Reset the filter and re-init the IMU
     imuFilter.reset();
     rpy_init();
+    
+    int delta_degrees = 0;
 
     //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees...
     float initial = toDegrees(imuFilter.getYaw());
@@ -116,15 +122,18 @@
     //Continue to turn to target
     while (!gameOver) {
         setTurnRight(0.6);
+        if(gameRun) {
+            receivePosition(NULL);
+        }
         sample = toDegrees(imuFilter.getYaw());
         wait(0.02);
-        if(sample < target)
+        if((sonar1.read()*100) > (SONAR_STOP+6))
             break;
     }
     stop();
 }
 
-void checkSonars()
+void checkSonars(bool gameRun)
 {
     //Collision Detection
     // read all sonar sensors
@@ -133,16 +142,16 @@
     float s3 = sonar3.read() * 100;
 
     // check if obstacle is near and adjust
-    pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP);
+    // pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP);
     if(s2 < SONAR_STOP) {
 
         if(s1 <= s3) {
-            centerTurnRight(90);
+            centerTurnRight(gameRun);
         } else {
-            centerTurnLeft(90);
+            centerTurnLeft(gameRun);
         }
-        imuFilter.reset();
-        rpy_init();
+        //imuFilter.reset();
+        //rpy_init();
     }
 }
 
@@ -162,7 +171,7 @@
     while (timer.read_ms() < ms) {
 
         timer.stop();
-        //checkSonars();
+        checkSonars(gameRun);
         if(gameRun) {
             receivePosition(NULL);
         }
@@ -170,14 +179,14 @@
 
         imuFilter.computeEuler();
         sample = toDegrees(imuFilter.getYaw());
-/*
+
         //Drift Correction
         sample = sample + float(timer.read_ms() / 800);
         // pc.printf("%f :::", sample);
 
         if (sample < 3) {
             // pc.printf("Steer Left\r\n");
-            leftMotorPWM = speed - 0.2;
+            leftMotorPWM = speed;
             rightMotorPWM = speed;
             leftMotor1 = 0;
             leftMotor2 = 1;
@@ -186,7 +195,7 @@
         } else if (sample > -3) {
             // pc.printf("Steer Right \r\n");
             leftMotorPWM = speed;
-            rightMotorPWM = speed - 0.18;
+            rightMotorPWM = speed - 0.2;
             leftMotor1 = 0;
             leftMotor2 = 1;
             rightMotor1 = 0;
@@ -194,7 +203,7 @@
         } else {
             // pc.printf("Straight\r\n");
             setMove(speed, false);
-        }*/
+        }
     }
     stop();
 }