Big Dog / Mbed 2 deprecated MECH_BigDog_Driving

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?

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