USNA ES202 / Mbed 2 deprecated ES202_FinalProject_workingExample

working example of final project code (in development)

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

Fork of ES202FinalProject by John Donnal

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?

UserRevisionLine numberNew 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()