Diff: main.cpp

Revision:

2:f1ccaf498a0c

Parent:

1:da5831bec373

Child:

3:af4ffcb8459d

--- a/main.cpp	Tue Apr 16 15:42:03 2019 +0000
+++ b/main.cpp	Wed Apr 17 14:43:47 2019 +0000
@@ -8,31 +8,43 @@
 //ecnoders
 Tach tLeft(p17,64);
 Tach tRight(p13,64);
-float proportion(float);
+float proportionl(float);
+float proportionr(float);
 float wL;
 float wR;
 float ref=1.45;
-float err;
-float lspeed=1;
-float newspeed;
-float Kp=0.1;
+float err, errl;
+float lspeed=0.49;
+float rspeed=-0.49;
+float newspeedl,newspeedr;
+float Kpl=0.1;
+float Kpr=-0.07;
 int main()
 {
     while(1) {
+        wait(1);
         left.speed(lspeed);
         wL=tLeft.getSpeed();
-        err=ref-wL;
-        if (err!=0)  {
-            newspeed=proportion(err);
-            left.speed(newspeed);
-        }
-        printf("wL=%f, err=%f, speed=%f\n\r",wL,err,newspeed);
+        errl=ref-wL;
+        newspeedl=proportionl(errl);
+        left.speed(newspeedl);
+        right.speed(rspeed);
+        wR=tRight.getSpeed();
+        err=ref-wR;
+        newspeedr=proportionr(err);
+        right.speed(newspeedr);
+        printf("speed left=%f\n\r",newspeedl);
+        printf("speed right=%f\n\r",newspeedr);
         wait(0.2);
     }
 }
-float proportion(float err)
+float proportionl(float errl)
 {
-    return newspeed=Kp*err;
+    return newspeedl=Kpl*errl+lspeed;
+}
+float proportionr(float err)
+{
+    return newspeedr=Kpr*err+rspeed;
 }
 // left.speed(0.49);
 //wL=tLeft.getSpeed();