Open Loop drive forward
Dependencies: mbed Tach ContinuousServo
main.cpp@1:da5831bec373, 2019-04-16 (annotated)
- Committer:
- m211482
- Date:
- Tue Apr 16 15:42:03 2019 +0000
- Revision:
- 1:da5831bec373
- Parent:
- 0:072052527879
- Child:
- 2:f1ccaf498a0c
Working Proportional control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m211482 | 0:072052527879 | 1 | #include "mbed.h" |
m211482 | 0:072052527879 | 2 | #include "ContinuousServo.h" |
m211482 | 0:072052527879 | 3 | #include "Tach.h" |
m211482 | 0:072052527879 | 4 | |
m211482 | 0:072052527879 | 5 | //servos |
m211482 | 0:072052527879 | 6 | ContinuousServo left(p23); |
m211482 | 0:072052527879 | 7 | ContinuousServo right(p26); |
m211482 | 0:072052527879 | 8 | //ecnoders |
m211482 | 0:072052527879 | 9 | Tach tLeft(p17,64); |
m211482 | 0:072052527879 | 10 | Tach tRight(p13,64); |
m211482 | 1:da5831bec373 | 11 | float proportion(float); |
m211482 | 0:072052527879 | 12 | float wL; |
m211482 | 0:072052527879 | 13 | float wR; |
m211482 | 1:da5831bec373 | 14 | float ref=1.45; |
m211482 | 1:da5831bec373 | 15 | float err; |
m211482 | 1:da5831bec373 | 16 | float lspeed=1; |
m211482 | 1:da5831bec373 | 17 | float newspeed; |
m211482 | 1:da5831bec373 | 18 | float Kp=0.1; |
m211482 | 1:da5831bec373 | 19 | int main() |
m211482 | 1:da5831bec373 | 20 | { |
m211482 | 0:072052527879 | 21 | while(1) { |
m211482 | 1:da5831bec373 | 22 | left.speed(lspeed); |
m211482 | 0:072052527879 | 23 | wL=tLeft.getSpeed(); |
m211482 | 1:da5831bec373 | 24 | err=ref-wL; |
m211482 | 1:da5831bec373 | 25 | if (err!=0) { |
m211482 | 1:da5831bec373 | 26 | newspeed=proportion(err); |
m211482 | 1:da5831bec373 | 27 | left.speed(newspeed); |
m211482 | 1:da5831bec373 | 28 | } |
m211482 | 1:da5831bec373 | 29 | printf("wL=%f, err=%f, speed=%f\n\r",wL,err,newspeed); |
m211482 | 1:da5831bec373 | 30 | wait(0.2); |
m211482 | 0:072052527879 | 31 | } |
m211482 | 0:072052527879 | 32 | } |
m211482 | 1:da5831bec373 | 33 | float proportion(float err) |
m211482 | 1:da5831bec373 | 34 | { |
m211482 | 1:da5831bec373 | 35 | return newspeed=Kp*err; |
m211482 | 1:da5831bec373 | 36 | } |
m211482 | 1:da5831bec373 | 37 | // left.speed(0.49); |
m211482 | 1:da5831bec373 | 38 | //wL=tLeft.getSpeed(); |
m211482 | 1:da5831bec373 | 39 | //right.speed(-0.5); //the servo is inverted so much be negative |
m211482 | 1:da5831bec373 | 40 | //wR=tRight.getSpeed(); |
m211482 | 1:da5831bec373 | 41 | //printf("Both moving, l=%f, r=%f\n\r",wL,wR); |
m211482 | 1:da5831bec373 | 42 | //wait(0.5); |
m211482 | 1:da5831bec373 | 43 | //left.stop(); |
m211482 | 1:da5831bec373 | 44 | //right.stop(); |
m211482 | 1:da5831bec373 | 45 | |
m211482 | 1:da5831bec373 | 46 | //} |
m211482 | 1:da5831bec373 | 47 | //} |