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