Big Dog / Mbed 2 deprecated MECH_BigDog_Driving

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?

UserRevisionLine numberNew 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 //}