Diff: main.cpp

Revision:

1:d9fb4f6b30f7

Parent:

0:3e1d75bd313d

--- a/main.cpp	Tue Feb 12 17:52:51 2019 +0000
+++ b/main.cpp	Wed Mar 06 16:56:44 2019 +0000
@@ -12,6 +12,13 @@
 
 #include "mbed.h"
 
+InterruptIn sw1(D10);
+
+Ticker cmd;
+char command[256],c;
+int num;
+
+
 DigitalOut solenoid(D7);//mosfet gate controlling solenoid
 PwmOut speed(PTC1);//speed for motors
 PwmOut speedb(PTA4);
@@ -20,8 +27,12 @@
 DigitalOut fwd2(PTA12);
 DigitalOut back2(PTD4);
 DigitalOut leds(PTC8);// the 4 red leds
-bool c; // memorise color so it knows which way to go at junction {true for red}
-float white_reference,color;
+int col; // memorise color so it knows which way to go at junction {true for red}
+PwmOut red(PTC9);
+PwmOut blue(PTC8);
+AnalogIn sense(A0);
+Serial pc(USBTX,USBRX);
+float color;
 float spd1,spd2;//speeds of left and right motors
 
 
@@ -41,24 +52,100 @@
     {speed2>0 ? fwd2=1;back2=0 : fwd2=0;back2=1};
 }
 */
-inline bool detect_color(int stage)
+inline int detect_color()
+{
+       red=0.75;blue=0;
+       wait(0.3);
+       
+       color= sense.read();
+        red=0;blue=0.75;wait(0.3);
+        color=color - sense.read();
+        red=0;blue=0;
+        if(color >= 0.025 && color < 0.055)
+        {
+            //pc.printf("RED  %.3f\n\n\n\n",color);
+            return 0;
+        }
+        else if( color < 0.025 && color >= 0)
+        {
+            return 1;
+            //pc.printf("BLUE %.3f\n\n\n\n",color);
+            }
+        else{
+            //pc.printf("Unknown Color \n\n\n\n");
+            return 2;
+            }
+        wait(1);
+}
+
+
+
+void command_mode()
 {
-    leds=1;//turn on the 4 red leds
-    if(stage==0)
+    c=pc.getc();
+    if(c=='p')//codename for PID indices
     {
-        white_reference=colorsensor.read();//global variable no prob
+        pc.printf("PID Coefficients Configuration\n");
+        c=pc.getc();
+        //c=pc.scanf("%s\n",&command);
+        num=(int)c-48;//get numerical value;
+        /*switch(num)
+        {
+            case 1:
+            myled=1;
+            break;
+            case 2:
+            myled2=1;
+            break;
+            case 3: 
+            myled3=1;
+            break;
+            default:
+            myled=0;myled2=0;myled3=0;break;
+        }*/
+            pc.printf("Done!\n");
+        memset(command,NULL, sizeof(command));
+    
+        
+    }
+    else if(c=='c')//codename for color detection
+    {
+        col=detect_color();
+        pc.printf("%d",col);
+    }
+    else if(c=='l')//codename for line detection 
+    {
+        //detect line and send stuff over serial -> hex number? could be too big to handle on script side 
+        pc.printf("112\n");//placeholder code 
     }
     else
     {
-        color=colorsensor.read();
-        return (color> white_reference (+/-) threshold ? true:false); //true means red, false means blue
+        pc.printf("Command Unknown.\n");
     }
 }
 
+        
+
+int x=0;
+void data(){
+    if(x>20) x=0;
+    pc.printf("%d\n",x);
+    x++;
+    if(sw1==1) data();
+    else 
+    {
+        pc.printf("%d\n",-1);
+        return;
+        }
+}
+
+
+
 int main() 
 {
+    sw1.rise(&command_mode);
     //pwm driver code will go into a function asap
-    speed.period(0.2f);
+    /*speed.period(0.2f);
     speedb.period(0.2f);
 
     speed.write(0.0);speedb.write(0.0);//do not move and scan color
@@ -76,6 +163,6 @@
            fwd2=0;back2=1;
             speed.write(0);
             speedb.write(0);
-           wait(2);
+           wait(2);*/
 
 }