Diff: main.cpp

Revision:

1:da5831bec373

Parent:

0:072052527879

Child:

2:f1ccaf498a0c

--- a/main.cpp	Tue Apr 16 14:54:18 2019 +0000
+++ b/main.cpp	Tue Apr 16 15:42:03 2019 +0000
@@ -8,18 +8,40 @@
 //ecnoders
 Tach tLeft(p17,64);
 Tach tRight(p13,64);
+float proportion(float);
 float wL;
 float wR;
-int main() {
+float ref=1.45;
+float err;
+float lspeed=1;
+float newspeed;
+float Kp=0.1;
+int main()
+{
     while(1) {
-        left.speed(0.49);
+        left.speed(lspeed);
         wL=tLeft.getSpeed();
-        right.speed(-0.5); //the servo is inverted so much be negative
-        wR=tRight.getSpeed();
-        printf("Both moving, l=%f, r=%f\n\r",wL,wR);
-        wait(0.5);
-        left.stop();
-        right.stop();
-        
+        err=ref-wL;
+        if (err!=0)  {
+            newspeed=proportion(err);
+            left.speed(newspeed);
+        }
+        printf("wL=%f, err=%f, speed=%f\n\r",wL,err,newspeed);
+        wait(0.2);
     }
 }
+float proportion(float err)
+{
+    return newspeed=Kp*err;
+}
+// left.speed(0.49);
+//wL=tLeft.getSpeed();
+//right.speed(-0.5); //the servo is inverted so much be negative
+//wR=tRight.getSpeed();
+//printf("Both moving, l=%f, r=%f\n\r",wL,wR);
+//wait(0.5);
+//left.stop();
+//right.stop();
+
+//}
+//}