worasuchad haomachai
/
positionControl
DC motor PID position control
Fork of QEI_HelloWorld by
main.cpp@2:916e20f507f2, 2018-11-07 (annotated)
- Committer:
- worasuchad
- Date:
- Wed Nov 07 17:05:37 2018 +0000
- Revision:
- 2:916e20f507f2
- Parent:
- 1:30696e4d196b
DC motor PID position control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
worasuchad | 2:916e20f507f2 | 1 | |
worasuchad | 2:916e20f507f2 | 2 | /***************************< File comment >***************************/ |
worasuchad | 2:916e20f507f2 | 3 | /* project: Lamborghini48 */ |
worasuchad | 2:916e20f507f2 | 4 | /* project description : mobile robot class */ |
worasuchad | 2:916e20f507f2 | 5 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 6 | /* version : 0.1 */ |
worasuchad | 2:916e20f507f2 | 7 | /* board : NUCLEO-F411RE */ |
worasuchad | 2:916e20f507f2 | 8 | /* detail : DC motor control */ |
worasuchad | 2:916e20f507f2 | 9 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 10 | /* create : 26/10/2018 */ |
worasuchad | 2:916e20f507f2 | 11 | /* programmer : Worasuchad Haomachai */ |
worasuchad | 2:916e20f507f2 | 12 | /**********************************************************************/ |
worasuchad | 2:916e20f507f2 | 13 | |
worasuchad | 2:916e20f507f2 | 14 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 15 | /* Include file */ |
worasuchad | 2:916e20f507f2 | 16 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 17 | #include "math.h" |
worasuchad | 2:916e20f507f2 | 18 | #include "QEI.h" // https://os.mbed.com/cookbook/QEI |
aberk | 0:bcff39fac858 | 19 | |
aberk | 0:bcff39fac858 | 20 | Serial pc(USBTX, USBRX); |
worasuchad | 2:916e20f507f2 | 21 | |
aberk | 1:30696e4d196b | 22 | //Use X4 encoding. |
aberk | 1:30696e4d196b | 23 | //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); |
aberk | 1:30696e4d196b | 24 | //Use X2 encoding by default. |
worasuchad | 2:916e20f507f2 | 25 | QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING); |
worasuchad | 2:916e20f507f2 | 26 | |
worasuchad | 2:916e20f507f2 | 27 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 28 | /* Global variable */ |
worasuchad | 2:916e20f507f2 | 29 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 30 | PwmOut pwm(D5); // this is the PWM pin for the motor for how much we move it to correct for its error |
worasuchad | 2:916e20f507f2 | 31 | DigitalOut dir1(D6); //these pins are to control the direction of the motor (clockwise/counter-clockwise) |
worasuchad | 2:916e20f507f2 | 32 | DigitalOut dir2(D7); |
worasuchad | 2:916e20f507f2 | 33 | |
worasuchad | 2:916e20f507f2 | 34 | double angle = 0; //set the angles |
aberk | 0:bcff39fac858 | 35 | |
worasuchad | 2:916e20f507f2 | 36 | double setpoint = 360; // 3072; // I am setting it to move through 100 degrees |
worasuchad | 2:916e20f507f2 | 37 | double Kp = 0.05; // 0.015; // you can set these constants however you like depending on trial & error |
worasuchad | 2:916e20f507f2 | 38 | double Ki = 0.001; // 0.000071; // 0.000109; |
worasuchad | 2:916e20f507f2 | 39 | double Kd = 0.0; // 0.93866; // 0.1009; |
worasuchad | 2:916e20f507f2 | 40 | |
worasuchad | 2:916e20f507f2 | 41 | float last_error = 0; |
worasuchad | 2:916e20f507f2 | 42 | float _error = 0; |
worasuchad | 2:916e20f507f2 | 43 | float changeError = 0; |
worasuchad | 2:916e20f507f2 | 44 | float totalError = 0; |
worasuchad | 2:916e20f507f2 | 45 | float pidTerm = 0; |
worasuchad | 2:916e20f507f2 | 46 | float pidTerm_scaled = 0; // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255| |
aberk | 0:bcff39fac858 | 47 | |
worasuchad | 2:916e20f507f2 | 48 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 49 | /* prototype fun */ |
worasuchad | 2:916e20f507f2 | 50 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 51 | void PIDcalculation(); // find PID value |
worasuchad | 2:916e20f507f2 | 52 | |
worasuchad | 2:916e20f507f2 | 53 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 54 | /* main */ |
worasuchad | 2:916e20f507f2 | 55 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 56 | int main() { |
worasuchad | 2:916e20f507f2 | 57 | pc.baud(115200); |
worasuchad | 2:916e20f507f2 | 58 | while(1) |
worasuchad | 2:916e20f507f2 | 59 | { |
worasuchad | 2:916e20f507f2 | 60 | PIDcalculation(); // find PID value |
worasuchad | 2:916e20f507f2 | 61 | |
worasuchad | 2:916e20f507f2 | 62 | if (angle < setpoint) |
worasuchad | 2:916e20f507f2 | 63 | { |
worasuchad | 2:916e20f507f2 | 64 | dir1 = 1; // Forward motion |
worasuchad | 2:916e20f507f2 | 65 | dir2 = 0; |
worasuchad | 2:916e20f507f2 | 66 | } |
worasuchad | 2:916e20f507f2 | 67 | else |
worasuchad | 2:916e20f507f2 | 68 | { |
worasuchad | 2:916e20f507f2 | 69 | dir1 = 0; // Reverse motion |
worasuchad | 2:916e20f507f2 | 70 | dir2 = 1; |
worasuchad | 2:916e20f507f2 | 71 | } |
worasuchad | 2:916e20f507f2 | 72 | |
worasuchad | 2:916e20f507f2 | 73 | pwm.write(pidTerm_scaled / 255.00f); |
worasuchad | 2:916e20f507f2 | 74 | |
worasuchad | 2:916e20f507f2 | 75 | // Serial.println("WHEEL ANGLE:"); |
worasuchad | 2:916e20f507f2 | 76 | pc.printf("%.3f\t\t", setpoint); |
worasuchad | 2:916e20f507f2 | 77 | pc.printf("%.3f\n\r", angle); |
worasuchad | 2:916e20f507f2 | 78 | //pc.printf("%d\n\r", wheel.getPulses()); |
aberk | 0:bcff39fac858 | 79 | } |
aberk | 0:bcff39fac858 | 80 | |
aberk | 0:bcff39fac858 | 81 | } |
worasuchad | 2:916e20f507f2 | 82 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 83 | /* PID */ |
worasuchad | 2:916e20f507f2 | 84 | /*--------------------------------------------------------------------*/ |
worasuchad | 2:916e20f507f2 | 85 | void PIDcalculation(){ |
worasuchad | 2:916e20f507f2 | 86 | /* |
worasuchad | 2:916e20f507f2 | 87 | * angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4) |
worasuchad | 2:916e20f507f2 | 88 | * reduction ratio: 64:1 , pulses per revolution: 12CPR |
worasuchad | 2:916e20f507f2 | 89 | */ |
worasuchad | 2:916e20f507f2 | 90 | |
worasuchad | 2:916e20f507f2 | 91 | angle = ( 0.004591 * wheel.getPulses()); // 360/(98*200*4) |
worasuchad | 2:916e20f507f2 | 92 | _error = setpoint - angle; |
worasuchad | 2:916e20f507f2 | 93 | |
worasuchad | 2:916e20f507f2 | 94 | changeError = _error - last_error; //derivative term |
worasuchad | 2:916e20f507f2 | 95 | totalError += _error; //accumalate errors to find integral term |
worasuchad | 2:916e20f507f2 | 96 | pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain |
worasuchad | 2:916e20f507f2 | 97 | |
worasuchad | 2:916e20f507f2 | 98 | // pidTerm = constrain(pidTerm, -255, 255); //constraining to appropriate value |
worasuchad | 2:916e20f507f2 | 99 | if(pidTerm > 255) |
worasuchad | 2:916e20f507f2 | 100 | { |
worasuchad | 2:916e20f507f2 | 101 | pidTerm = 255; |
worasuchad | 2:916e20f507f2 | 102 | } |
worasuchad | 2:916e20f507f2 | 103 | else if(pidTerm < -255) |
worasuchad | 2:916e20f507f2 | 104 | { |
worasuchad | 2:916e20f507f2 | 105 | pidTerm = -255; |
worasuchad | 2:916e20f507f2 | 106 | } |
worasuchad | 2:916e20f507f2 | 107 | |
worasuchad | 2:916e20f507f2 | 108 | pidTerm_scaled = abs(pidTerm); //make sure it's a positive value |
worasuchad | 2:916e20f507f2 | 109 | |
worasuchad | 2:916e20f507f2 | 110 | last_error = _error; |
worasuchad | 2:916e20f507f2 | 111 | } |