ES202_Final / Mbed 2 deprecated ES202_Turn

Turn

Dependencies:   ContinuousServo Tach mbed

main.cpp@0:29e8568563e4, 2018-04-26 (annotated)

Committer:
nbchaskin
Date:
Thu Apr 26 14:07:38 2018 +0000
Revision:
0:29e8568563e4
Rough try

Who changed what in which revision?

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