Diff: main.cpp

Revision:

0:3e1d75bd313d

Child:

1:d9fb4f6b30f7

diff -r 000000000000 -r 3e1d75bd313d main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 12 17:52:51 2019 +0000
@@ -0,0 +1,81 @@
+/*
+TODO:
+Line following, gaussian filter, 
+Color det integration 
+PID algorithm integration 
+Line following integration
+Calibration????
+
+*/
+
+
+
+#include "mbed.h"
+
+DigitalOut solenoid(D7);//mosfet gate controlling solenoid
+PwmOut speed(PTC1);//speed for motors
+PwmOut speedb(PTA4);
+DigitalOut fwd1(PTA1);//directions for motors
+DigitalOut back1(PTA2);
+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;
+float spd1,spd2;//speeds of left and right motors
+
+
+void solenoid_engage()
+{
+    solenoid=1;
+}
+
+void solenoid_off()
+{
+    solenoid=0;
+}
+
+/*inline void move(spd1,spd2)
+{
+    {speed1>0 ? fwd1=1;back1=0 : fwd1=0;back1=1};
+    {speed2>0 ? fwd2=1;back2=0 : fwd2=0;back2=1};
+}
+*/
+inline bool detect_color(int stage)
+{
+    leds=1;//turn on the 4 red leds
+    if(stage==0)
+    {
+        white_reference=colorsensor.read();//global variable no prob
+    }
+    else
+    {
+        color=colorsensor.read();
+        return (color> white_reference (+/-) threshold ? true:false); //true means red, false means blue
+    }
+}
+
+int main() 
+{
+    //pwm driver code will go into a function asap
+    speed.period(0.2f);
+    speedb.period(0.2f);
+
+    speed.write(0.0);speedb.write(0.0);//do not move and scan color
+    float i=0.5,j=0.5;
+
+    
+           fwd1=0;back1=1;//fwd
+           fwd2=1;back2=0;
+           speed.write(i);
+           speedb.write(j);
+           wait(2);
+        
+           
+          fwd1=0;back1=1;//back
+           fwd2=0;back2=1;
+            speed.write(0);
+            speedb.write(0);
+           wait(2);
+
+}