![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Parallax Activity Bot code
Dependencies: ContinuousServo Tach mbed
Diff: main.cpp
- Revision:
- 3:e33676098294
- Parent:
- 2:f72b9dd19159
--- a/main.cpp Fri Mar 02 17:02:06 2018 +0000 +++ b/main.cpp Tue Mar 27 13:22:52 2018 +0000 @@ -12,19 +12,109 @@ Tach tLeft(p17,64); Tach tRight(p13,64); -int main() { - //wait for MATLAB command +// 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); - pc.getc(); - //spin servos from -1 to 1 and print speed - for(float i=-1.0;i<=1.0;i+=0.01){ - left.speed(-1.0*i); - right.speed(i); - wait_ms(100); - pc.printf("%f,%f\n", tLeft.getSpeed(),tRight.getSpeed()); + 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; } - left.stop(); - right.stop(); -} + 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()