worasuchad haomachai / Mbed 2 deprecated positionControl

DC motor PID position control

Dependencies:   QEI mbed

Fork of QEI_HelloWorld by Aaron Berk

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?

UserRevisionLine numberNew 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 }