![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Open Loop drive forward
Dependencies: mbed Tach ContinuousServo
main.cpp
- Committer:
- m211482
- Date:
- 2019-04-26
- Revision:
- 3:af4ffcb8459d
- Parent:
- 2:f1ccaf498a0c
File content as of revision 3:af4ffcb8459d:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" //servos ContinuousServo left(p23); ContinuousServo right(p26); //ecnoders Tach tLeft(p17,64); Tach tRight(p13,64); float proportionl(float); float proportionr(float); float wL; float wR; float ref=1.45; float err, errl; float lspeed=0.3; float rspeed=-0.3; float newspeedl,newspeedr; float Kpl=0.028; float Kpr=-0.09; int main() { while(1) { wait(1); left.speed(lspeed); wL=tLeft.getSpeed(); 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 proportionl(float errl) { return newspeedl=Kpl*errl+lspeed; } float proportionr(float err) { return newspeedr=Kpr*err+rspeed; } // 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(); //} //}