working example of final project code (in development)
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Fork of ES202FinalProject by
main.cpp@3:e33676098294, 2018-03-27 (annotated)
- Committer:
- lddevrie
- Date:
- Tue Mar 27 13:22:52 2018 +0000
- Revision:
- 3:e33676098294
- Parent:
- 2:f72b9dd19159
- Child:
- 4:ceae6ab77baf
update to command speed and angular velocity
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdonnal | 0:ac6a1723d393 | 1 | #include "mbed.h" |
jdonnal | 0:ac6a1723d393 | 2 | #include "ContinuousServo.h" |
jdonnal | 0:ac6a1723d393 | 3 | #include "Tach.h" |
jdonnal | 0:ac6a1723d393 | 4 | |
lddevrie | 2:f72b9dd19159 | 5 | // Test change |
lddevrie | 2:f72b9dd19159 | 6 | |
jdonnal | 0:ac6a1723d393 | 7 | Serial pc(USBTX,USBRX); |
jdonnal | 0:ac6a1723d393 | 8 | //servos |
jdonnal | 0:ac6a1723d393 | 9 | ContinuousServo left(p23); |
jdonnal | 0:ac6a1723d393 | 10 | ContinuousServo right(p26); |
jdonnal | 0:ac6a1723d393 | 11 | //encoders |
jdonnal | 0:ac6a1723d393 | 12 | Tach tLeft(p17,64); |
jdonnal | 0:ac6a1723d393 | 13 | Tach tRight(p13,64); |
jdonnal | 0:ac6a1723d393 | 14 | |
lddevrie | 3:e33676098294 | 15 | // speed controller |
lddevrie | 3:e33676098294 | 16 | Ticker spdCtrl; |
lddevrie | 3:e33676098294 | 17 | DigitalOut myled(LED1); |
lddevrie | 3:e33676098294 | 18 | |
lddevrie | 3:e33676098294 | 19 | // Function definition Prototypes (declarations) |
lddevrie | 3:e33676098294 | 20 | void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists |
lddevrie | 3:e33676098294 | 21 | |
lddevrie | 3:e33676098294 | 22 | |
lddevrie | 3:e33676098294 | 23 | float vl; |
lddevrie | 3:e33676098294 | 24 | float vr; |
lddevrie | 3:e33676098294 | 25 | float Ts = 0.01; // Control update period (seconds) (100 Hz equivalent) |
lddevrie | 3:e33676098294 | 26 | float speedL,speedR; |
lddevrie | 3:e33676098294 | 27 | float dcL; |
lddevrie | 3:e33676098294 | 28 | float dcR; |
lddevrie | 3:e33676098294 | 29 | float dcpL; |
lddevrie | 3:e33676098294 | 30 | float dcpR; |
lddevrie | 3:e33676098294 | 31 | float errpL; |
lddevrie | 3:e33676098294 | 32 | float errpR; |
lddevrie | 3:e33676098294 | 33 | float spdErrL; |
lddevrie | 3:e33676098294 | 34 | float spdErrR; |
lddevrie | 3:e33676098294 | 35 | float kp = 0.1; |
lddevrie | 3:e33676098294 | 36 | float ki = 0.01; |
lddevrie | 3:e33676098294 | 37 | float whl_rad = 3.1; // wheel radius in centimeters |
lddevrie | 3:e33676098294 | 38 | float baseL = 11.0; // wheelbase in centimeters |
lddevrie | 3:e33676098294 | 39 | float om = 0.0; // angular velocity |
lddevrie | 3:e33676098294 | 40 | float sp = 0.0; // forward speed in cm/s |
lddevrie | 3:e33676098294 | 41 | |
lddevrie | 3:e33676098294 | 42 | int main() |
lddevrie | 3:e33676098294 | 43 | { |
lddevrie | 3:e33676098294 | 44 | //wait for MATLAB command |
jdonnal | 0:ac6a1723d393 | 45 | left.speed(0.0); |
jdonnal | 0:ac6a1723d393 | 46 | right.speed(0.0); |
lddevrie | 3:e33676098294 | 47 | while(1) { |
lddevrie | 3:e33676098294 | 48 | spdErrL = 0.0; |
lddevrie | 3:e33676098294 | 49 | spdErrR = 0.0; |
lddevrie | 3:e33676098294 | 50 | dcL = 0.0; |
lddevrie | 3:e33676098294 | 51 | dcR = 0.0; |
lddevrie | 3:e33676098294 | 52 | errpL = 0.0; |
lddevrie | 3:e33676098294 | 53 | dcpL = 0.0; |
lddevrie | 3:e33676098294 | 54 | errpR = 0.0; |
lddevrie | 3:e33676098294 | 55 | dcpR = 0.0; |
lddevrie | 3:e33676098294 | 56 | pc.scanf("%f,%f,%f,%f",&sp,&om,&kp,&ki); |
lddevrie | 3:e33676098294 | 57 | |
lddevrie | 3:e33676098294 | 58 | spdCtrl.attach(&ctrCode,Ts); |
lddevrie | 3:e33676098294 | 59 | |
lddevrie | 3:e33676098294 | 60 | for(int i=1; i<=200; i++) { |
lddevrie | 3:e33676098294 | 61 | |
lddevrie | 3:e33676098294 | 62 | vl = (2*sp-om*baseL)/(2*whl_rad); // desired left wheel speed in cm/s |
lddevrie | 3:e33676098294 | 63 | vr = (2*sp+om*baseL)/(2*whl_rad); // desired right wheel speed in cm/s |
lddevrie | 3:e33676098294 | 64 | |
lddevrie | 3:e33676098294 | 65 | |
lddevrie | 3:e33676098294 | 66 | |
lddevrie | 3:e33676098294 | 67 | wait_ms(100); |
lddevrie | 3:e33676098294 | 68 | pc.printf("%f,%f,%f,%f\n", speedL,speedR,dcL,dcR); |
lddevrie | 3:e33676098294 | 69 | } |
lddevrie | 3:e33676098294 | 70 | |
lddevrie | 3:e33676098294 | 71 | |
lddevrie | 3:e33676098294 | 72 | |
lddevrie | 3:e33676098294 | 73 | spdCtrl.detach(); |
lddevrie | 3:e33676098294 | 74 | left.stop(); |
lddevrie | 3:e33676098294 | 75 | right.stop(); |
lddevrie | 3:e33676098294 | 76 | }// end while(1) |
lddevrie | 3:e33676098294 | 77 | } |
lddevrie | 3:e33676098294 | 78 | |
lddevrie | 3:e33676098294 | 79 | |
lddevrie | 3:e33676098294 | 80 | // Additional function definitions |
lddevrie | 3:e33676098294 | 81 | void ctrCode() // function to attach to ticker |
lddevrie | 3:e33676098294 | 82 | { |
lddevrie | 3:e33676098294 | 83 | myled = !myled; // toggle LED 2 to indicate control update |
lddevrie | 3:e33676098294 | 84 | |
lddevrie | 3:e33676098294 | 85 | speedL = tLeft.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second |
lddevrie | 3:e33676098294 | 86 | speedR = tRight.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second |
lddevrie | 3:e33676098294 | 87 | |
lddevrie | 3:e33676098294 | 88 | // compute error |
lddevrie | 3:e33676098294 | 89 | spdErrL = vl-speedL; |
lddevrie | 3:e33676098294 | 90 | spdErrR = vr-speedR; |
lddevrie | 3:e33676098294 | 91 | |
lddevrie | 3:e33676098294 | 92 | // PI controller |
lddevrie | 3:e33676098294 | 93 | 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 |
lddevrie | 3:e33676098294 | 94 | 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 |
lddevrie | 3:e33676098294 | 95 | |
lddevrie | 3:e33676098294 | 96 | // enforce duty cycle saturation |
lddevrie | 3:e33676098294 | 97 | if(dcL>1.0) { |
lddevrie | 3:e33676098294 | 98 | dcL = 1.0; |
lddevrie | 3:e33676098294 | 99 | } else if(dcL<0.0) { |
lddevrie | 3:e33676098294 | 100 | dcL = 0.0; |
jdonnal | 0:ac6a1723d393 | 101 | } |
lddevrie | 3:e33676098294 | 102 | if(dcR>1.0) { |
lddevrie | 3:e33676098294 | 103 | dcR = 1.0; |
lddevrie | 3:e33676098294 | 104 | } else if(dcR<0.0) { |
lddevrie | 3:e33676098294 | 105 | dcR = 0.0; |
lddevrie | 3:e33676098294 | 106 | } |
lddevrie | 3:e33676098294 | 107 | |
lddevrie | 3:e33676098294 | 108 | // motor control |
lddevrie | 3:e33676098294 | 109 | left.speed(dcL); // first input is the motor channel, second is duty cycle |
lddevrie | 3:e33676098294 | 110 | right.speed(-dcR); // first input is the motor channel, second is duty cycle |
lddevrie | 3:e33676098294 | 111 | |
lddevrie | 3:e33676098294 | 112 | // Age variables |
lddevrie | 3:e33676098294 | 113 | errpL = spdErrL; // age speed error variable |
lddevrie | 3:e33676098294 | 114 | dcpL = dcL; // age duty cycle variable |
lddevrie | 3:e33676098294 | 115 | |
lddevrie | 3:e33676098294 | 116 | errpR = spdErrR; // age speed error variable |
lddevrie | 3:e33676098294 | 117 | dcpR = dcR; // age duty cycle variable |
lddevrie | 3:e33676098294 | 118 | |
lddevrie | 3:e33676098294 | 119 | |
lddevrie | 3:e33676098294 | 120 | } // end ctrCode() |