Diff: main.cpp

Revision:

8:f3e0b4814888

Parent:

7:ea395616348c

Child:

10:de7a56fb94bc

Child:

13:f7ec3f026634

--- a/main.cpp	Mon Mar 02 22:11:35 2015 +0000
+++ b/main.cpp	Fri Mar 06 00:28:42 2015 +0000
@@ -24,6 +24,11 @@
 float tuning_val = 1;
 
 Timer t;
+Timer servoTimer;
+
+// Servo parameters
+float lastTurnTime = 0.0f;
+bool servoLeft = true;
 
 //Observed average speeds for each duty cycle
 const float TUNING_CONSTANT_20 = 3.00;
@@ -171,6 +176,17 @@
                 
                 while(1){
                     velocity_control(0.2f, TUNING_CONSTANT_20);
+                    if(servoLeft){
+                        servo.pulsewidth(.001);    
+                    }
+                    else{
+                        servo.pulsewidth(.002);
+                    }
+                    float turnTime = servoTimer.read();
+                    if(turnTime - lastTurnTime > 3.0){
+                        servoLeft = !servoLeft;
+                        lastTurnTime = turnTime;
+                    }
                 }
                 
                 //break;