Diff: main.cpp

Revision:

0:3c7038dd80b3

diff -r 000000000000 -r 3c7038dd80b3 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 26 21:03:49 2019 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "TCS3472_I2C.h"
+#include "ContinuousServo.h"
+#include "Tach.h"
+
+
+Serial pc(USBTX,USBRX);
+PwmOut LB(p22);
+TCS3472_I2C rgb_sensor(p9, p10);
+ContinuousServo left(p23);
+ContinuousServo right(p26);
+float turn, pt;
+float r,l;
+//ecnoders
+Tach tLeft(p17,64);
+Tach tRight(p13,64);
+int main()
+{
+    rgb_sensor.enablePowerAndRGBC();
+    rgb_sensor.setIntegrationTime(100);
+
+    pc.baud(9600);
+    int rgb_data[4];
+    float PWMbrightness=1.0;
+    right.speed(-0.1);
+        left.speed(0.1);
+    while (1) {
+        LB=PWMbrightness;
+        rgb_sensor.getAllColors(rgb_data);
+        pc.printf("unfiltered: %d, red: %d, green: %d, blue: %d \n\r", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+        turn=(750-rgb_data[0]);
+        pt=0.00038*(turn);
+        if (pt>0.25) {
+            pt=0.25;
+        }
+        if (pt<-0.25) {
+            pt=-0.25;
+        }
+        r=-0.1-(pt/2);
+        l=0.1-(pt/2);
+        right.speed(r);
+        left.speed(l);
+        pc.printf("right %f, left %f\n\r",r,l);
+    }
+}
+/* if ((rgb_data[0]<=800)&&(rgb_data[0]>=740)) {
+     pc.printf("Go straight, %d \n\r",rgb_data[0]);
+ } else if (rgb_data[0]<740) {
+     pc.printf("Turn left, %d\n\r",rgb_data[0]);
+ } else if (rgb_data[0]>800) {
+     pc.printf("Turn right, %d \n\r",rgb_data[0]);
+ }
+ wait (0.5);
+}
+}*/
\ No newline at end of file