take 1
Dependencies: ContinuousServo Tach mbed
main.cpp
- Committer:
- nbchaskin
- Date:
- 2018-04-23
- Revision:
- 2:bf1da9059e82
- Parent:
- 1:ed49c46d8998
File content as of revision 2:bf1da9059e82:
#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() { wait(5); //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.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; }