![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Open Loop drive forward
Dependencies: mbed Tach ContinuousServo
main.cpp@3:af4ffcb8459d, 2019-04-26 (annotated)
- Committer:
- m211482
- Date:
- Fri Apr 26 20:14:34 2019 +0000
- Revision:
- 3:af4ffcb8459d
- Parent:
- 2:f1ccaf498a0c
Doesn't really work but oh well
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 | 2:f1ccaf498a0c | 11 | float proportionl(float); |
m211482 | 2:f1ccaf498a0c | 12 | float proportionr(float); |
m211482 | 0:072052527879 | 13 | float wL; |
m211482 | 0:072052527879 | 14 | float wR; |
m211482 | 1:da5831bec373 | 15 | float ref=1.45; |
m211482 | 2:f1ccaf498a0c | 16 | float err, errl; |
m211482 | 3:af4ffcb8459d | 17 | float lspeed=0.3; |
m211482 | 3:af4ffcb8459d | 18 | float rspeed=-0.3; |
m211482 | 2:f1ccaf498a0c | 19 | float newspeedl,newspeedr; |
m211482 | 3:af4ffcb8459d | 20 | float Kpl=0.028; |
m211482 | 3:af4ffcb8459d | 21 | float Kpr=-0.09; |
m211482 | 1:da5831bec373 | 22 | int main() |
m211482 | 1:da5831bec373 | 23 | { |
m211482 | 0:072052527879 | 24 | while(1) { |
m211482 | 2:f1ccaf498a0c | 25 | wait(1); |
m211482 | 1:da5831bec373 | 26 | left.speed(lspeed); |
m211482 | 0:072052527879 | 27 | wL=tLeft.getSpeed(); |
m211482 | 2:f1ccaf498a0c | 28 | errl=ref-wL; |
m211482 | 2:f1ccaf498a0c | 29 | newspeedl=proportionl(errl); |
m211482 | 2:f1ccaf498a0c | 30 | left.speed(newspeedl); |
m211482 | 2:f1ccaf498a0c | 31 | right.speed(rspeed); |
m211482 | 2:f1ccaf498a0c | 32 | wR=tRight.getSpeed(); |
m211482 | 2:f1ccaf498a0c | 33 | err=ref-wR; |
m211482 | 2:f1ccaf498a0c | 34 | newspeedr=proportionr(err); |
m211482 | 2:f1ccaf498a0c | 35 | right.speed(newspeedr); |
m211482 | 2:f1ccaf498a0c | 36 | printf("speed left=%f\n\r",newspeedl); |
m211482 | 2:f1ccaf498a0c | 37 | printf("speed right=%f\n\r",newspeedr); |
m211482 | 1:da5831bec373 | 38 | wait(0.2); |
m211482 | 0:072052527879 | 39 | } |
m211482 | 0:072052527879 | 40 | } |
m211482 | 2:f1ccaf498a0c | 41 | float proportionl(float errl) |
m211482 | 1:da5831bec373 | 42 | { |
m211482 | 2:f1ccaf498a0c | 43 | return newspeedl=Kpl*errl+lspeed; |
m211482 | 2:f1ccaf498a0c | 44 | } |
m211482 | 2:f1ccaf498a0c | 45 | float proportionr(float err) |
m211482 | 2:f1ccaf498a0c | 46 | { |
m211482 | 2:f1ccaf498a0c | 47 | return newspeedr=Kpr*err+rspeed; |
m211482 | 1:da5831bec373 | 48 | } |
m211482 | 1:da5831bec373 | 49 | // left.speed(0.49); |
m211482 | 1:da5831bec373 | 50 | //wL=tLeft.getSpeed(); |
m211482 | 1:da5831bec373 | 51 | //right.speed(-0.5); //the servo is inverted so much be negative |
m211482 | 1:da5831bec373 | 52 | //wR=tRight.getSpeed(); |
m211482 | 1:da5831bec373 | 53 | //printf("Both moving, l=%f, r=%f\n\r",wL,wR); |
m211482 | 1:da5831bec373 | 54 | //wait(0.5); |
m211482 | 1:da5831bec373 | 55 | //left.stop(); |
m211482 | 1:da5831bec373 | 56 | //right.stop(); |
m211482 | 1:da5831bec373 | 57 | |
m211482 | 1:da5831bec373 | 58 | //} |
m211482 | 1:da5831bec373 | 59 | //} |