![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
take 1
Dependencies: ContinuousServo Tach mbed
Diff: main.cpp
- Revision:
- 0:b059e65bf362
- Child:
- 1:ed49c46d8998
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 19 15:22:15 2018 +0000 @@ -0,0 +1,57 @@ +#include "mbed.h" +#include "ContinuousServo.h" +#include "Tach.h" + +Tach tLeft(p17,64); +Tach tRight(p13,64); + +ContinuousServo left(p23); +ContinuousServo right(p26); + +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() { + //go straight + while(1){ + speedL = tLeft.getSpeed(); + speedR = tRight.getSpeed(); + l = PIpwmL(0.5, speedL); + r = PIpwmR(0.5, speedR); + + left.speed(l); + right.speed(-r); + + pc.printf("%f,%f\n",speedL,speedR); + wait(0.05); + } +} + +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.49; + 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.06*errorR) + 0.31; + return right; + } \ No newline at end of file