ES202_Final / Mbed 2 deprecated ES202_straight1

take 1

Dependencies:   ContinuousServo Tach mbed

main.cpp@1:ed49c46d8998, 2018-04-19 (annotated)

Committer:
nbchaskin
Date:
Thu Apr 19 15:36:11 2018 +0000
Revision:
1:ed49c46d8998
Parent:
0:b059e65bf362
Child:
2:bf1da9059e82
added wait to start

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nbchaskin 0:b059e65bf362 1 #include "mbed.h"
nbchaskin 0:b059e65bf362 2 #include "ContinuousServo.h"
nbchaskin 0:b059e65bf362 3 #include "Tach.h"
nbchaskin 0:b059e65bf362 4
nbchaskin 0:b059e65bf362 5 Tach tLeft(p17,64);
nbchaskin 0:b059e65bf362 6 Tach tRight(p13,64);
nbchaskin 0:b059e65bf362 7
nbchaskin 0:b059e65bf362 8 ContinuousServo left(p23);
nbchaskin 0:b059e65bf362 9 ContinuousServo right(p26);
nbchaskin 0:b059e65bf362 10
nbchaskin 0:b059e65bf362 11 float l;
nbchaskin 0:b059e65bf362 12 float r;
nbchaskin 0:b059e65bf362 13 float speedL;
nbchaskin 0:b059e65bf362 14 float speedR;
nbchaskin 0:b059e65bf362 15 float errorL;
nbchaskin 0:b059e65bf362 16 float errorR;
nbchaskin 0:b059e65bf362 17 float sampling_per;
nbchaskin 0:b059e65bf362 18
nbchaskin 0:b059e65bf362 19 Serial pc(USBTX,USBRX);
nbchaskin 0:b059e65bf362 20
nbchaskin 0:b059e65bf362 21 float PIpwmL(float desired_speed,float speed);
nbchaskin 0:b059e65bf362 22 float PIpwmR(float desired_speed,float speed);
nbchaskin 0:b059e65bf362 23
nbchaskin 0:b059e65bf362 24 int main() {
nbchaskin 1:ed49c46d8998 25 wait(5);
nbchaskin 0:b059e65bf362 26 //go straight
nbchaskin 0:b059e65bf362 27 while(1){
nbchaskin 1:ed49c46d8998 28
nbchaskin 0:b059e65bf362 29 speedL = tLeft.getSpeed();
nbchaskin 0:b059e65bf362 30 speedR = tRight.getSpeed();
nbchaskin 0:b059e65bf362 31 l = PIpwmL(0.5, speedL);
nbchaskin 0:b059e65bf362 32 r = PIpwmR(0.5, speedR);
nbchaskin 0:b059e65bf362 33
nbchaskin 0:b059e65bf362 34 left.speed(l);
nbchaskin 0:b059e65bf362 35 right.speed(-r);
nbchaskin 0:b059e65bf362 36
nbchaskin 0:b059e65bf362 37 pc.printf("%f,%f\n",speedL,speedR);
nbchaskin 0:b059e65bf362 38 wait(0.05);
nbchaskin 0:b059e65bf362 39 }
nbchaskin 0:b059e65bf362 40 }
nbchaskin 0:b059e65bf362 41
nbchaskin 0:b059e65bf362 42 float PIpwmL(float desired_speed,float speed)
nbchaskin 0:b059e65bf362 43 {
nbchaskin 0:b059e65bf362 44 float integral_errorL = 0.0;
nbchaskin 0:b059e65bf362 45 float sampling_per = 0.05;
nbchaskin 0:b059e65bf362 46 float errorL = desired_speed - speed;
nbchaskin 0:b059e65bf362 47 integral_errorL += (errorL*sampling_per);
nbchaskin 0:b059e65bf362 48 float left = (0.07*integral_errorL) + (0.08*errorL) + 0.49;
nbchaskin 0:b059e65bf362 49 return left;
nbchaskin 0:b059e65bf362 50 }
nbchaskin 0:b059e65bf362 51 float PIpwmR(float desired_speed,float speed)
nbchaskin 0:b059e65bf362 52 {
nbchaskin 0:b059e65bf362 53 float integral_errorR = 0.0;
nbchaskin 0:b059e65bf362 54 float sampling_per = 0.05;
nbchaskin 0:b059e65bf362 55 float errorR = desired_speed - speed;
nbchaskin 0:b059e65bf362 56 integral_errorR += (errorR*sampling_per);
nbchaskin 1:ed49c46d8998 57 float right = (0.07*integral_errorR) + (0.06*errorR) + 0.3;
nbchaskin 0:b059e65bf362 58 return right;
nbchaskin 0:b059e65bf362 59 }