Parallax Activity Bot code
Dependencies: ContinuousServo Tach mbed
main.cpp
- Committer:
- lddevrie
- Date:
- 2018-03-27
- Revision:
- 3:e33676098294
- Parent:
- 2:f72b9dd19159
File content as of revision 3:e33676098294:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" // Test change Serial pc(USBTX,USBRX); //servos ContinuousServo left(p23); ContinuousServo right(p26); //encoders Tach tLeft(p17,64); Tach tRight(p13,64); // speed controller Ticker spdCtrl; DigitalOut myled(LED1); // Function definition Prototypes (declarations) void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists float vl; float vr; float Ts = 0.01; // Control update period (seconds) (100 Hz equivalent) float speedL,speedR; float dcL; float dcR; float dcpL; float dcpR; float errpL; float errpR; float spdErrL; float spdErrR; float kp = 0.1; float ki = 0.01; float whl_rad = 3.1; // wheel radius in centimeters float baseL = 11.0; // wheelbase in centimeters float om = 0.0; // angular velocity float sp = 0.0; // forward speed in cm/s int main() { //wait for MATLAB command left.speed(0.0); right.speed(0.0); while(1) { spdErrL = 0.0; spdErrR = 0.0; dcL = 0.0; dcR = 0.0; errpL = 0.0; dcpL = 0.0; errpR = 0.0; dcpR = 0.0; pc.scanf("%f,%f,%f,%f",&sp,&om,&kp,&ki); spdCtrl.attach(&ctrCode,Ts); for(int i=1; i<=200; i++) { vl = (2*sp-om*baseL)/(2*whl_rad); // desired left wheel speed in cm/s vr = (2*sp+om*baseL)/(2*whl_rad); // desired right wheel speed in cm/s wait_ms(100); pc.printf("%f,%f,%f,%f\n", speedL,speedR,dcL,dcR); } spdCtrl.detach(); left.stop(); right.stop(); }// end while(1) } // Additional function definitions void ctrCode() // function to attach to ticker { myled = !myled; // toggle LED 2 to indicate control update speedL = tLeft.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second speedR = tRight.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second // compute error spdErrL = vl-speedL; spdErrR = vr-speedR; // PI controller dcL = dcpL+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrL+(Ts/2.0*(ki/kp)-1.0)*errpL); // compute duty cycle for motor using PI control scheme dcR = dcpR+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrR+(Ts/2.0*(ki/kp)-1.0)*errpR); // compute duty cycle for motor using PI control scheme // enforce duty cycle saturation if(dcL>1.0) { dcL = 1.0; } else if(dcL<0.0) { dcL = 0.0; } if(dcR>1.0) { dcR = 1.0; } else if(dcR<0.0) { dcR = 0.0; } // motor control left.speed(dcL); // first input is the motor channel, second is duty cycle right.speed(-dcR); // first input is the motor channel, second is duty cycle // Age variables errpL = spdErrL; // age speed error variable dcpL = dcL; // age duty cycle variable errpR = spdErrR; // age speed error variable dcpR = dcR; // age duty cycle variable } // end ctrCode()