Open Loop drive forward
Dependencies: mbed Tach ContinuousServo
Diff: main.cpp
- Revision:
- 2:f1ccaf498a0c
- Parent:
- 1:da5831bec373
- Child:
- 3:af4ffcb8459d
diff -r da5831bec373 -r f1ccaf498a0c main.cpp --- 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();