Diff: main.cpp

Revision:

1:4a4e697b10d2

Parent:

0:5121458888b5

diff -r 5121458888b5 -r 4a4e697b10d2 main.cpp
--- a/main.cpp	Sun Mar 17 21:21:17 2019 +0000
+++ b/main.cpp	Mon Mar 18 16:55:28 2019 +0000
@@ -14,18 +14,22 @@
 DigitalOut test_blue(LED_BLUE);
 Serial pc(PTE0,PTE1); // tx, rx
 BusIn line(PTC3,PTC0,PTC7,PTC5,PTC4,PTC6,PTC10); //Line Board input
+Ticker timed;
 char command[256],c;
 int num;
+int col;
 float color;
+bool detected=false;
 // Minimum and maximum motor speeds
-#define MAX 0.9
-
-
+#define MAX 0.5
 // PID terms
-#define P_TERM 30 
-#define DD_TERM 0
+#define P_TERM 30
+#define DD_TERM 2
 #define D_TERM 76
 #define k 0.1
+
+inline void junction();
+inline void color_detect();
 inline float get_position()//-3 to 3 based on the sensor position 
 {
     switch(line)//rates are wrong CHANGE ASAP
@@ -44,6 +48,52 @@
         return 0.9;
         case 0x40:
         return 1.0;
+        case 0x14:
+            junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x1C:
+              junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x28:
+              junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x50:
+               junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x05:
+               junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x0A:
+              junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x24:
+              junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x12:
+              junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        case 0x41:
+               junction();
+        detected=false;
+        timed.attach(&color_detect,0.2);
+        break;
+        
     } 
 }
 
@@ -68,43 +118,100 @@
     speedb.write(right);
 
 }
- 
+
+inline void junction()//TO GET OUT OF JUNCTION
+{
+    if(col== 1)
+    {
+        //turn left
+        drive(0.5,0.1);wait(0.1);
+    }
+    else 
+    {
+        drive( 0.1,0.5);wait(0.1);
+        //turn right
+    }
+}
+inline void solenoid_actuate()
+{
+    solenoid=!solenoid;
+}
+inline void color_detect()
+{
+    //for timed functions
+    if(detected) 
+    {
+        timed.detach();
+        return;
+    }
+    else{
+    red=0.8;blue=0;
+
+       wait(0.1);
+
+       color= sense.read();
+
+        red=0;blue=0.8;wait(0.1);
+
+        color=color - sense.read();
+
+        red=0;blue=0;
+
+        if(color >= 0.033 && color < 0.045)
+
+        {
+            detected=true;
+            solenoid_actuate();
+            test_red=0;test_blue=1;
+            col =1;
+
+        }
+
+        else if( color <= 0.020 && color >= 0)
+
+        {
+            detected=true;
+            test_blue=0;test_red=1;
+            solenoid_actuate();
+            col=0;
+            
+            }
+      }      
+}
 
 inline int detect_color()
-
 {
-       solenoid=1;
        red=0.75;blue=0;
 
-       wait(0.01);
+       wait(0.08);
 
        
 
        color= sense.read();
 
-        red=0;blue=0.75;wait(0.01);
+        red=0;blue=0.75;wait(0.08);
 
         color=color - sense.read();
 
         red=0;blue=0;
 
-        if(color >= 0.025 && color < 0.055)
+        if(color >= 0.035 && color < 0.045)
 
         {
 
-            //pc.printf("RED  %.3f\n\n\n\n",color);
+            pc.printf("RED  %.3f\n\n\n\n",color);
             test_red=0;test_blue=1;
             return 0;
 
         }
 
-        else if( color < 0.025 && color >= 0)
+        else if( color < 0.02 && color >= 0)
 
         {
             test_blue=0;test_red=1;
             return 1;
 
-            //pc.printf("BLUE %.3f\n\n\n\n",color);
+            pc.printf("BLUE %.3f\n\n\n\n",color);
 
             }
 
@@ -210,17 +317,20 @@
 
 int main() 
 {
+    
     wait(2);
     if(pc.readable())
     {
-        if((char)pc.getc()=='z');
+        if((char)pc.getc()=='z'){
+            timed.detach();
         command_mode();
+        }
     }
     else
     {
-
+        
     wait(2.0);
-
+    solenoid=0;
     float right;
     float left;
     float current_pos_of_line = 0.0;
@@ -229,9 +339,8 @@
     float derivative,proportional,DoD = 0;
     float power;
     float speed = MAX;
-    solenoid=1;
+    timed.attach(&color_detect,0.25);
     while (1) {
-        detect_color();
         // Get the position of the line.
         current_pos_of_line = get_position();      
         proportional = current_pos_of_line;