Diff: color.cpp

Revision:

0:7d319151aaa0

Child:

1:1dd468e2ad40

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/color.cpp	Wed Apr 25 13:33:13 2018 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "ContinuousServo.h"
+#include "Tach.h"
+#include "TCS3472_I2C.h"
+
+DigitalIn hall(p21);
+InterruptIn hall(p21);
+BusOut flash(LED1, LED2, LED3, LED4);
+TCS3472_I2C rgb_sensor(p9, p10);
+ContinuousServo left(p23);                  //Set up left wheel driver
+ContinuousServo right(p26);                 //Set up right wheel driver
+Tach tleft(p17,64);                         //Set up left tachometer
+Tach tright(p13, 64);                       //Set up right tachometer
+
+float wl;                                   //Left wheel velocity
+float wr;                                   //Right wheel velocity
+float e;                                    //Error
+
+float r = 1.3125;                           //radius of wheels
+float l = 4.0625;                           //width of wheel base
+float omega = 0.0;                          //angular velocity of wheels
+float v = 0.5;                              //translational velocity of wheels
+float kp = 1.0;                             //Proportional control gain
+
+float vl = ((omega*l)+(2*v))/(2*r);         //Calculates left wheel velocity
+float vr = ((omega*l)-(2*v))/(2*r);         //Calculates right wheel velocity
+
+int counter = 0.0;
+ 
+void mine() {
+    flash = 16.0;
+}
+ 
+int main() {
+    
+    hall.rise(&mine);
+    hall.fall(&mine);
+    
+    while(1) {           // wait around, interrupts will interrupt this!
+        left.speed(vl);                     //Move the left wheel
+        right.speed(vr);                    //Move the right wheel
+        wl = tleft.getSpeed();              //Read the left tachometer
+        wr = tright.getSpeed();             //Read the right tachometer
+    
+        e = wl - (-wr);                     //Calculate the error
+        vr = vr - (kp*e);                   //Readjust the right wheel velocity
+        
+        if(rgb_sensor){
+        flash = counter;
+        }
+    }
+}
\ No newline at end of file