Revision 46:adcd57a5e402, committed 2013-04-12

Comitter:

xiaxia686

Date:

Fri Apr 12 20:59:18 2013 +0000

Parent:

45:77cf6375348a

Child:

50:937e860f4621

Commit message:

Colours Sensors fixed

Changed in this revision

--- a/Processes/AI/ai.cpp	Fri Apr 12 20:40:52 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 20:59:18 2013 +0000
@@ -23,18 +23,24 @@
 */
 
     current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 1.85;
+    current_waypoint[0].y = 1.65;
     current_waypoint[0].theta = 0.0;
     current_waypoint[0].pos_threshold = 0.05;
     current_waypoint[0].angle_threshold = 0.05*PI;
+    
+    current_waypoint[1].x = 2.5;
+    current_waypoint[1].y = 0.45;
+    current_waypoint[1].theta = 0.0;
+    current_waypoint[1].pos_threshold = 0.05;
+    current_waypoint[1].angle_threshold = 0.05*PI;    
 
-    current_waypoint[1].x = 1.2;
-    current_waypoint[1].y = 0.18;
-    current_waypoint[1].theta = 0;
-    current_waypoint[1].pos_threshold = 0.01;
-    current_waypoint[1].angle_threshold = 0.00001;
+    current_waypoint[2].x = 1.2;
+    current_waypoint[2].y = 0.18;
+    current_waypoint[2].theta = 0;
+    current_waypoint[2].pos_threshold = 0.01;
+    current_waypoint[2].angle_threshold = 0.00001;
 
-    current_waypoint[2].x = -999;
+    current_waypoint[3].x = -999;
 /*
     //TODO: temp current waypoint hack
     current_waypoint = new Waypoint;
@@ -52,15 +58,19 @@
     secondwp->angle_threshold = 0.00001;
 */    
     motion::setNewWaypoint(current_waypoint);
+        Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
+    Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
     while(1)
     {
+
         Thread::wait(50);
         
+        
         motion::waypoint_flag_mutex.lock();
         if (motion::checkWaypointStatus())
         {
             
-            if ((current_waypoint+1)->x != -999)
+            if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED)))
             { 
                 motion::clearWaypointReached();
                 current_waypoint++;

--- a/Processes/AI/ai.h	Fri Apr 12 20:40:52 2013 +0000
+++ b/Processes/AI/ai.h	Fri Apr 12 20:59:18 2013 +0000
@@ -4,6 +4,7 @@
 #include "rtos.h"
 #include "globals.h"
 #include "motion.h"
+#include "Colour.h"
 
 namespace AI
 {

--- a/main.cpp	Fri Apr 12 20:40:52 2013 +0000
+++ b/main.cpp	Fri Apr 12 20:59:18 2013 +0000
@@ -75,29 +75,29 @@
     
     
     pc.baud(115200);
-    
- //   InitSerial();
- //   wait(1);
- //   Kalman::KalmanInit();
+    wait(2);
+    InitSerial();
+    wait(3);
+    Kalman::KalmanInit();
     
- //   Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
- //   Kalman::start_predict_ticker(&predictthread);
+    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+    Kalman::start_predict_ticker(&predictthread);
     
- //   Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
+    Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
     
- //   Ticker motorcontroltestticker;
- //   motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+    Ticker motorcontroltestticker;
+    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
     // ai layer thread
- //   Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
+    Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
     // motion layer periodic callback
- //   RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
- //   motion_timer.start(50);
+    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
+    motion_timer.start(50);
 
- //   Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
- //   measureCPUidle(); //repurpose thread for idle measurement
-    colourtest();
+    measureCPUidle(); //repurpose thread for idle measurement
+    //colourtest();
     //pt_test();
     while(true) {
     Thread::wait(osWaitForever);