First run
Dependencies: ContinuousServo Tach mbed
Diff: main.cpp
- Revision:
- 2:b8e0c824a562
- Parent:
- 1:be893bda5f00
--- a/main.cpp Wed Apr 25 15:18:05 2018 +0000 +++ b/main.cpp Mon Apr 30 23:42:07 2018 +0000 @@ -25,34 +25,45 @@ int main() { while(1) { - float check; - check = sonar; - //son[0] = sonar; - //son[1] = sonar; - //son[2] = sonar; - //son[3] = sonar; - //son[4] = sonar; - //son[5] = sonar; - //float check; - //check = son[0] + son[1] + son[2] + son[3]+son[4]+son[5]; - //check = check/6; - check = check*3.3; - check = check/0.0098; - pc.printf("%f\n",check); - distance = 7;//inches; + float son[6]; + for (int count = 0;count<6;count++){ + son[count] = sonar; + wait(0.01); + //pc.printf("%f\n",son[count]); + } + + float a; + int i; + int j; + int n = 6; + for (i = 0; i < n; i++) + { + for (j = i + 1; j < n; j++) + { + if (son[i] > son[j]) + { + a = son[i]; + son[i] = son[j]; + son[j] = a; + } + } + } + pc.printf("%f\n",son[2]); + distance = 10;//inches; + distance = 0.003*distance - 0.0057; wait(0.05); - if (check > distance){ - - speedL = tLeft.getSpeed(); - speedR = tRight.getSpeed(); - l = PIpwmL(0.3, speedL); - r = PIpwmR(0.3, speedR); + pc.printf("%f\n",son); + if (son[2] > 0.038){ + speedL = tLeft.getSpeed(); + speedR = tRight.getSpeed(); + l = PIpwmL(0.3, speedL); + r = PIpwmR(0.3, speedR); - left.speed(l); - right.speed(-r); - - pc.printf("%f,%f\n",speedL,speedR); - wait(0.05); + left.speed(l); + right.speed(-r); + + wait(0.05); + } else { left.stop(); @@ -67,7 +78,7 @@ float sampling_per = 0.05; float errorL = desired_speed - speed; integral_errorL += (errorL*sampling_per); - float left = (0.07*integral_errorL) + (0.08*errorL) + 0.485; + float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48; return left; } float PIpwmR(float desired_speed,float speed)