ES202_Final / Mbed 2 deprecated ES202_Stop

First run

Dependencies:   ContinuousServo Tach mbed

main.cpp@2:b8e0c824a562, 2018-04-30 (annotated)

Committer:
nbchaskin
Date:
Mon Apr 30 23:42:07 2018 +0000
Revision:
2:b8e0c824a562
Parent:
1:be893bda5f00 
final

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 2:b8e0c824a562 28 float son[6];
nbchaskin 2:b8e0c824a562 29 for (int count = 0;count<6;count++){
nbchaskin 2:b8e0c824a562 30 son[count] = sonar;
nbchaskin 2:b8e0c824a562 31 wait(0.01);
nbchaskin 2:b8e0c824a562 32 //pc.printf("%f\n",son[count]);
nbchaskin 2:b8e0c824a562 33 }
nbchaskin 2:b8e0c824a562 34
nbchaskin 2:b8e0c824a562 35 float a;
nbchaskin 2:b8e0c824a562 36 int i;
nbchaskin 2:b8e0c824a562 37 int j;
nbchaskin 2:b8e0c824a562 38 int n = 6;
nbchaskin 2:b8e0c824a562 39 for (i = 0; i < n; i++)
nbchaskin 2:b8e0c824a562 40 {
nbchaskin 2:b8e0c824a562 41 for (j = i + 1; j < n; j++)
nbchaskin 2:b8e0c824a562 42 {
nbchaskin 2:b8e0c824a562 43 if (son[i] > son[j])
nbchaskin 2:b8e0c824a562 44 {
nbchaskin 2:b8e0c824a562 45 a = son[i];
nbchaskin 2:b8e0c824a562 46 son[i] = son[j];
nbchaskin 2:b8e0c824a562 47 son[j] = a;
nbchaskin 2:b8e0c824a562 48 }
nbchaskin 2:b8e0c824a562 49 }
nbchaskin 2:b8e0c824a562 50 }
nbchaskin 2:b8e0c824a562 51 pc.printf("%f\n",son[2]);
nbchaskin 2:b8e0c824a562 52 distance = 10;//inches;
nbchaskin 2:b8e0c824a562 53 distance = 0.003*distance - 0.0057;
nbchaskin 1:be893bda5f00 54 wait(0.05);
nbchaskin 2:b8e0c824a562 55 pc.printf("%f\n",son);
nbchaskin 2:b8e0c824a562 56 if (son[2] > 0.038){
nbchaskin 2:b8e0c824a562 57 speedL = tLeft.getSpeed();
nbchaskin 2:b8e0c824a562 58 speedR = tRight.getSpeed();
nbchaskin 2:b8e0c824a562 59 l = PIpwmL(0.3, speedL);
nbchaskin 2:b8e0c824a562 60 r = PIpwmR(0.3, speedR);
nbchaskin 1:be893bda5f00 61
nbchaskin 2:b8e0c824a562 62 left.speed(l);
nbchaskin 2:b8e0c824a562 63 right.speed(-r);
nbchaskin 2:b8e0c824a562 64
nbchaskin 2:b8e0c824a562 65 wait(0.05);
nbchaskin 2:b8e0c824a562 66
nbchaskin 1:be893bda5f00 67 }
nbchaskin 0:a0013f51f232 68 else {
nbchaskin 1:be893bda5f00 69 left.stop();
nbchaskin 1:be893bda5f00 70 right.stop();
nbchaskin 0:a0013f51f232 71 break;
nbchaskin 0:a0013f51f232 72 }
nbchaskin 0:a0013f51f232 73 }
nbchaskin 0:a0013f51f232 74 }
nbchaskin 1:be893bda5f00 75 float PIpwmL(float desired_speed,float speed)
nbchaskin 1:be893bda5f00 76 {
nbchaskin 1:be893bda5f00 77 float integral_errorL = 0.0;
nbchaskin 1:be893bda5f00 78 float sampling_per = 0.05;
nbchaskin 1:be893bda5f00 79 float errorL = desired_speed - speed;
nbchaskin 1:be893bda5f00 80 integral_errorL += (errorL*sampling_per);
nbchaskin 2:b8e0c824a562 81 float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;
nbchaskin 1:be893bda5f00 82 return left;
nbchaskin 1:be893bda5f00 83 }
nbchaskin 1:be893bda5f00 84 float PIpwmR(float desired_speed,float speed)
nbchaskin 1:be893bda5f00 85 {
nbchaskin 1:be893bda5f00 86 float integral_errorR = 0.0;
nbchaskin 1:be893bda5f00 87 float sampling_per = 0.05;
nbchaskin 1:be893bda5f00 88 float errorR = desired_speed - speed;
nbchaskin 1:be893bda5f00 89 integral_errorR += (errorR*sampling_per);
nbchaskin 1:be893bda5f00 90 float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
nbchaskin 1:be893bda5f00 91 return right;
nbchaskin 1:be893bda5f00 92 }