Revision 3:8d12cc699e4c, committed 2018-04-19

Comitter:

kingkang2

Date:

Thu Apr 19 15:29:36 2018 +0000

Parent:

2:9ab67bed1a34

Commit message:

straight line;

Changed in this revision

--- a/PROJECT.cpp	Thu Apr 19 14:31:10 2018 +0000
+++ b/PROJECT.cpp	Thu Apr 19 15:29:36 2018 +0000
@@ -3,21 +3,75 @@
 #include "Tach.h"
 #include "TCS3472_I2C.h"
 
-Serial pc(USBTX,USBRX);
-// color sensor
+Serial pc(USBTX,USBRX);// color sensor
 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
-// Hall effect sensor
-DigitalIn hall(p22);
-// sonar sensor
-AnalogIn sonar(p20); // range sensor 9.8 mV/inch
+DigitalIn hall(p21);// Hall effect sensor
+DigitalOut hallpwr(p22); 
+AnalogIn sonar(p19); // sonar sensor, range sensor 9.8 mV/inch
+
 //servos
 ContinuousServo left(p23);
 ContinuousServo right(p26);
+
 //encoders
 Tach tLeft(p17,64);
 Tach tRight(p13,64);
 
+// functions
+float calcPWM(float desired_velocity, float velocity);
+
+// initialize variables
+    w = 0;
+    v = 0.5;
+    r = 3.3;
+    phi = 90;
+    l = 4.1;
+    float left_rps;
+    float right_rps;
+    Vright = (2v+ wl)/2r;   // rad/s
+    Vleft = (2v - wl)/2r;   // rad/s
+
+void forward(){
+    
+}
+ 
+void stop(){
+    
+}
+
+int main()
+{
+
+
 //drive straight
 
-w = 0;
-v = 0.5;
+    while(1) {
+        
+    right = Vright; 
+    left = Vleft;
+    right_rps = tRight.read() * 2* pi; //rad/s
+    left_rps = tLeft.read() * 2 * pi;  //rad/s
+    sDC_right = calcPWM(Vright,right_rps); // SET DUTY CYCLE FOR RIGHT SERVO
+    sDC_left = calcPWM(Vleft,left_rps); // SET DUTY CYCLE FOR LEFT SERVO
+    right.speed(sDC_right);
+    left.speed(sDC_left);
+    wait(0.05);
+    
+
+
+
+    }
+}
+
+
+// calculates the PWM duty cycle given the desired and measured velocity
+float calcPWM(float desired_velocity, float velocity)
+{
+
+    float v_err;
+    // velocity error
+    v_err = (desired_velocity - velocity);
+
+    sDC = .039*v_err; // enter duty cycle calculation here!!
+    return sDC;
+}