![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
First run
Dependencies: ContinuousServo Tach mbed
main.cpp
- Committer:
- nbchaskin
- Date:
- 2018-04-30
- Revision:
- 2:b8e0c824a562
- Parent:
- 1:be893bda5f00
File content as of revision 2:b8e0c824a562:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" Tach tLeft(p17,64); Tach tRight(p13,64); ContinuousServo left(p23); ContinuousServo right(p26); AnalogIn sonar(p19); float distance; float l; float r; float speedL; float speedR; float errorL; float errorR; float sampling_per; Serial pc(USBTX,USBRX); float PIpwmL(float desired_speed,float speed); float PIpwmR(float desired_speed,float speed); int main() { while(1) { 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); 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); wait(0.05); } else { left.stop(); right.stop(); break; } } } float PIpwmL(float desired_speed,float speed) { float integral_errorL = 0.0; 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.48; return left; } float PIpwmR(float desired_speed,float speed) { float integral_errorR = 0.0; float sampling_per = 0.05; float errorR = desired_speed - speed; integral_errorR += (errorR*sampling_per); float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25; return right; }