ES202_Final / Mbed 2 deprecated ES202_Stop

First run

Dependencies:   ContinuousServo Tach mbed

main.cpp@1:be893bda5f00, 2018-04-25 (annotated)

Committer:
nbchaskin
Date:
Wed Apr 25 15:18:05 2018 +0000
Revision:
1:be893bda5f00
Parent:
0:a0013f51f232
Child:
2:b8e0c824a562
Sensor is not consistent. Will read in value less than distance randomly and then stop early. Added code (commented out now) to potentially average values to get rid of extremities.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nbchaskin 0:a0013f51f232 1 #include "mbed.h"
nbchaskin 0:a0013f51f232 2 #include "ContinuousServo.h"
nbchaskin 0:a0013f51f232 3 #include "Tach.h"
nbchaskin 0:a0013f51f232 4
nbchaskin 0:a0013f51f232 5 Tach tLeft(p17,64);
nbchaskin 0:a0013f51f232 6 Tach tRight(p13,64);
nbchaskin 0:a0013f51f232 7
nbchaskin 0:a0013f51f232 8 ContinuousServo left(p23);
nbchaskin 0:a0013f51f232 9 ContinuousServo right(p26);
nbchaskin 0:a0013f51f232 10 AnalogIn sonar(p19);
nbchaskin 0:a0013f51f232 11 float distance;
nbchaskin 0:a0013f51f232 12
nbchaskin 1:be893bda5f00 13 float l;
nbchaskin 1:be893bda5f00 14 float r;
nbchaskin 1:be893bda5f00 15 float speedL;
nbchaskin 1:be893bda5f00 16 float speedR;
nbchaskin 1:be893bda5f00 17 float errorL;
nbchaskin 1:be893bda5f00 18 float errorR;
nbchaskin 1:be893bda5f00 19 float sampling_per;
nbchaskin 1:be893bda5f00 20
nbchaskin 1:be893bda5f00 21 Serial pc(USBTX,USBRX);
nbchaskin 1:be893bda5f00 22
nbchaskin 1:be893bda5f00 23 float PIpwmL(float desired_speed,float speed);
nbchaskin 1:be893bda5f00 24 float PIpwmR(float desired_speed,float speed);
nbchaskin 1:be893bda5f00 25
nbchaskin 0:a0013f51f232 26 int main() {
nbchaskin 0:a0013f51f232 27 while(1) {
nbchaskin 1:be893bda5f00 28 float check;
nbchaskin 1:be893bda5f00 29 check = sonar;
nbchaskin 1:be893bda5f00 30 //son[0] = sonar;
nbchaskin 1:be893bda5f00 31 //son[1] = sonar;
nbchaskin 1:be893bda5f00 32 //son[2] = sonar;
nbchaskin 1:be893bda5f00 33 //son[3] = sonar;
nbchaskin 1:be893bda5f00 34 //son[4] = sonar;
nbchaskin 1:be893bda5f00 35 //son[5] = sonar;
nbchaskin 1:be893bda5f00 36 //float check;
nbchaskin 1:be893bda5f00 37 //check = son[0] + son[1] + son[2] + son[3]+son[4]+son[5];
nbchaskin 1:be893bda5f00 38 //check = check/6;
nbchaskin 1:be893bda5f00 39 check = check*3.3;
nbchaskin 1:be893bda5f00 40 check = check/0.0098;
nbchaskin 1:be893bda5f00 41 pc.printf("%f\n",check);
nbchaskin 1:be893bda5f00 42 distance = 7;//inches;
nbchaskin 1:be893bda5f00 43 wait(0.05);
nbchaskin 1:be893bda5f00 44 if (check > distance){
nbchaskin 1:be893bda5f00 45
nbchaskin 1:be893bda5f00 46 speedL = tLeft.getSpeed();
nbchaskin 1:be893bda5f00 47 speedR = tRight.getSpeed();
nbchaskin 1:be893bda5f00 48 l = PIpwmL(0.3, speedL);
nbchaskin 1:be893bda5f00 49 r = PIpwmR(0.3, speedR);
nbchaskin 1:be893bda5f00 50
nbchaskin 1:be893bda5f00 51 left.speed(l);
nbchaskin 1:be893bda5f00 52 right.speed(-r);
nbchaskin 1:be893bda5f00 53
nbchaskin 1:be893bda5f00 54 pc.printf("%f,%f\n",speedL,speedR);
nbchaskin 1:be893bda5f00 55 wait(0.05);
nbchaskin 1:be893bda5f00 56 }
nbchaskin 0:a0013f51f232 57 else {
nbchaskin 1:be893bda5f00 58 left.stop();
nbchaskin 1:be893bda5f00 59 right.stop();
nbchaskin 0:a0013f51f232 60 break;
nbchaskin 0:a0013f51f232 61 }
nbchaskin 0:a0013f51f232 62 }
nbchaskin 0:a0013f51f232 63 }
nbchaskin 1:be893bda5f00 64 float PIpwmL(float desired_speed,float speed)
nbchaskin 1:be893bda5f00 65 {
nbchaskin 1:be893bda5f00 66 float integral_errorL = 0.0;
nbchaskin 1:be893bda5f00 67 float sampling_per = 0.05;
nbchaskin 1:be893bda5f00 68 float errorL = desired_speed - speed;
nbchaskin 1:be893bda5f00 69 integral_errorL += (errorL*sampling_per);
nbchaskin 1:be893bda5f00 70 float left = (0.07*integral_errorL) + (0.08*errorL) + 0.485;
nbchaskin 1:be893bda5f00 71 return left;
nbchaskin 1:be893bda5f00 72 }
nbchaskin 1:be893bda5f00 73 float PIpwmR(float desired_speed,float speed)
nbchaskin 1:be893bda5f00 74 {
nbchaskin 1:be893bda5f00 75 float integral_errorR = 0.0;
nbchaskin 1:be893bda5f00 76 float sampling_per = 0.05;
nbchaskin 1:be893bda5f00 77 float errorR = desired_speed - speed;
nbchaskin 1:be893bda5f00 78 integral_errorR += (errorR*sampling_per);
nbchaskin 1:be893bda5f00 79 float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
nbchaskin 1:be893bda5f00 80 return right;
nbchaskin 1:be893bda5f00 81 }