Scott N / Mbed 2 deprecated Basic_SingleMotor_PID

Simple PID controller for a DC motor with an attached QEI and potentiometer.

Dependencies:   QEI mbed

Basic_SM_PID.cpp@0:31ccfbaf5bff, 2018-08-02 (annotated)

Committer:
snaranjo
Date:
Thu Aug 02 14:46:49 2018 +0000
Revision:
0:31ccfbaf5bff
Simple PID control with a DC motor, attached QEI, and potentiometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
snaranjo 0:31ccfbaf5bff 1 #include "mbed.h"
snaranjo 0:31ccfbaf5bff 2 #include "QEI.h"
snaranjo 0:31ccfbaf5bff 3
snaranjo 0:31ccfbaf5bff 4 #define pi 3.14159265358979323846
snaranjo 0:31ccfbaf5bff 5
snaranjo 0:31ccfbaf5bff 6
snaranjo 0:31ccfbaf5bff 7 // typedef struct
snaranjo 0:31ccfbaf5bff 8 struct sPID
snaranjo 0:31ccfbaf5bff 9 {
snaranjo 0:31ccfbaf5bff 10 double dState; // Last position input
snaranjo 0:31ccfbaf5bff 11 double iState; // Integrator state
snaranjo 0:31ccfbaf5bff 12 double iMax, iMin; // Maximum and minimum allowable integrator state
snaranjo 0:31ccfbaf5bff 13 double iGain, // integral gain
snaranjo 0:31ccfbaf5bff 14 pGain, // proportional gain
snaranjo 0:31ccfbaf5bff 15 dGain; // derivative gain
snaranjo 0:31ccfbaf5bff 16 };
snaranjo 0:31ccfbaf5bff 17
snaranjo 0:31ccfbaf5bff 18
snaranjo 0:31ccfbaf5bff 19
snaranjo 0:31ccfbaf5bff 20 double UpdatePID(sPID * pid, double error, double position)
snaranjo 0:31ccfbaf5bff 21 {
snaranjo 0:31ccfbaf5bff 22 double pTerm, dTerm, iTerm;
snaranjo 0:31ccfbaf5bff 23
snaranjo 0:31ccfbaf5bff 24 pTerm = pid->pGain * error; // calculate the proportional term
snaranjo 0:31ccfbaf5bff 25
snaranjo 0:31ccfbaf5bff 26
snaranjo 0:31ccfbaf5bff 27 pid->iState += error; // calculate the integral state with appropriate limiting
snaranjo 0:31ccfbaf5bff 28
snaranjo 0:31ccfbaf5bff 29 if (pid->iState > pid->iMax) pid->iState = pid->iMax;
snaranjo 0:31ccfbaf5bff 30 else if (pid->iState < pid->iMin) pid->iState = pid->iMin;
snaranjo 0:31ccfbaf5bff 31
snaranjo 0:31ccfbaf5bff 32 iTerm = pid->iGain * pid->iState; // calculate the integral term
snaranjo 0:31ccfbaf5bff 33 dTerm = pid->dGain * (pid->dState - position);
snaranjo 0:31ccfbaf5bff 34
snaranjo 0:31ccfbaf5bff 35 pid->dState = position;
snaranjo 0:31ccfbaf5bff 36
snaranjo 0:31ccfbaf5bff 37 return pTerm + dTerm + iTerm;
snaranjo 0:31ccfbaf5bff 38 }
snaranjo 0:31ccfbaf5bff 39
snaranjo 0:31ccfbaf5bff 40 void setPID(sPID *pid, double P, double I, double D,
snaranjo 0:31ccfbaf5bff 41 double iMx, double iMn, double dS, double iS){
snaranjo 0:31ccfbaf5bff 42
snaranjo 0:31ccfbaf5bff 43 pid->pGain = P;
snaranjo 0:31ccfbaf5bff 44 pid->iGain = I;
snaranjo 0:31ccfbaf5bff 45 pid->dGain = D;
snaranjo 0:31ccfbaf5bff 46 pid->iMax = iMx;
snaranjo 0:31ccfbaf5bff 47 pid->iMin = iMn;
snaranjo 0:31ccfbaf5bff 48 pid->dState = dS;
snaranjo 0:31ccfbaf5bff 49 pid->iState = iS;
snaranjo 0:31ccfbaf5bff 50
snaranjo 0:31ccfbaf5bff 51 }
snaranjo 0:31ccfbaf5bff 52
snaranjo 0:31ccfbaf5bff 53 Serial pc(USBTX, USBRX);
snaranjo 0:31ccfbaf5bff 54
snaranjo 0:31ccfbaf5bff 55 int baudRate = 460800;
snaranjo 0:31ccfbaf5bff 56
snaranjo 0:31ccfbaf5bff 57
snaranjo 0:31ccfbaf5bff 58
snaranjo 0:31ccfbaf5bff 59 int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
snaranjo 0:31ccfbaf5bff 60 int pulseRead = 0;
snaranjo 0:31ccfbaf5bff 61
snaranjo 0:31ccfbaf5bff 62 float ctrlDeg = 0.0;
snaranjo 0:31ccfbaf5bff 63 float shaftDeg = 0.0;
snaranjo 0:31ccfbaf5bff 64 float cmdSig = 0.0;
snaranjo 0:31ccfbaf5bff 65 float degErr = 0.0;
snaranjo 0:31ccfbaf5bff 66
snaranjo 0:31ccfbaf5bff 67 Timer timer;
snaranjo 0:31ccfbaf5bff 68
snaranjo 0:31ccfbaf5bff 69 //Use X4 encoding.
snaranjo 0:31ccfbaf5bff 70 QEI wheel(p11, p12, NC, steps_per_rev, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
snaranjo 0:31ccfbaf5bff 71
snaranjo 0:31ccfbaf5bff 72 //Output to motors. PWM signal is sent to either one, depending on desired rotational direction. CW or CCW.
snaranjo 0:31ccfbaf5bff 73 DigitalOut cwCH(p22); //Ouput pin to h-bridge 1
snaranjo 0:31ccfbaf5bff 74 DigitalOut ccwCH(p26);
snaranjo 0:31ccfbaf5bff 75 PwmOut motor(p24); //Output pin to h-bridge 2
snaranjo 0:31ccfbaf5bff 76 AnalogIn Current (p19); //Current sense pin
snaranjo 0:31ccfbaf5bff 77 AnalogIn ctrlPot (p16);
snaranjo 0:31ccfbaf5bff 78 AnalogIn currPot (p18);
snaranjo 0:31ccfbaf5bff 79
snaranjo 0:31ccfbaf5bff 80
snaranjo 0:31ccfbaf5bff 81 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
snaranjo 0:31ccfbaf5bff 82 {
snaranjo 0:31ccfbaf5bff 83 return ((pulses/steps_per_rev)*(2*pi));
snaranjo 0:31ccfbaf5bff 84 }
snaranjo 0:31ccfbaf5bff 85
snaranjo 0:31ccfbaf5bff 86 float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
snaranjo 0:31ccfbaf5bff 87 {
snaranjo 0:31ccfbaf5bff 88
snaranjo 0:31ccfbaf5bff 89 return ((pulses/steps_per_rev)*360);
snaranjo 0:31ccfbaf5bff 90 }
snaranjo 0:31ccfbaf5bff 91
snaranjo 0:31ccfbaf5bff 92 void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward
snaranjo 0:31ccfbaf5bff 93 {
snaranjo 0:31ccfbaf5bff 94 //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
snaranjo 0:31ccfbaf5bff 95 if (signal < 0) {
snaranjo 0:31ccfbaf5bff 96 cwCH = 0;
snaranjo 0:31ccfbaf5bff 97 ccwCH = 1;
snaranjo 0:31ccfbaf5bff 98 motor.write(signal * -1);
snaranjo 0:31ccfbaf5bff 99 // Motor1.write(0);
snaranjo 0:31ccfbaf5bff 100 // Motor2.write(signal * -1);
snaranjo 0:31ccfbaf5bff 101 } else if (signal > 0) {
snaranjo 0:31ccfbaf5bff 102 ccwCH = 0;
snaranjo 0:31ccfbaf5bff 103 cwCH = 1;
snaranjo 0:31ccfbaf5bff 104 motor.write(signal);
snaranjo 0:31ccfbaf5bff 105 // Motor1.write(signal);
snaranjo 0:31ccfbaf5bff 106 // Motor2.write(0);
snaranjo 0:31ccfbaf5bff 107 } else {
snaranjo 0:31ccfbaf5bff 108 cwCH = 0;
snaranjo 0:31ccfbaf5bff 109 ccwCH = 0;
snaranjo 0:31ccfbaf5bff 110 // Motor1.write(0);
snaranjo 0:31ccfbaf5bff 111 // Motor2.write(0);
snaranjo 0:31ccfbaf5bff 112 }
snaranjo 0:31ccfbaf5bff 113 }
snaranjo 0:31ccfbaf5bff 114
snaranjo 0:31ccfbaf5bff 115
snaranjo 0:31ccfbaf5bff 116 int main() {
snaranjo 0:31ccfbaf5bff 117
snaranjo 0:31ccfbaf5bff 118 pc.baud(baudRate);
snaranjo 0:31ccfbaf5bff 119 //will run a loop to apply the PID controller. I will command a movement with the
snaranjo 0:31ccfbaf5bff 120 //pot, then will run the updatePID function to find the error difference and
snaranjo 0:31ccfbaf5bff 121 //learn how to get good feedback with the PID controller
snaranjo 0:31ccfbaf5bff 122 sPID mPid;
snaranjo 0:31ccfbaf5bff 123 sPID* pmPid;
snaranjo 0:31ccfbaf5bff 124 float currVal, prev_curr;
snaranjo 0:31ccfbaf5bff 125 // float currFactor;
snaranjo 0:31ccfbaf5bff 126 pmPid = &mPid; // .1
snaranjo 0:31ccfbaf5bff 127 // P , I, D, Imax, Imin, Dstate, Istate
snaranjo 0:31ccfbaf5bff 128 // setPID(pmPid,0.07, 0, .6, 0.45, -0.1, 0, 0); //gains for when working with Degs
snaranjo 0:31ccfbaf5bff 129 setPID(pmPid,0.9, 0, 7, 0.45, -0.1, 0, 0); //gains when working in radians
snaranjo 0:31ccfbaf5bff 130
snaranjo 0:31ccfbaf5bff 131 float t, t_ref;
snaranjo 0:31ccfbaf5bff 132
snaranjo 0:31ccfbaf5bff 133 //initialize PID controller
snaranjo 0:31ccfbaf5bff 134 motor.period(0.00005f); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds)
snaranjo 0:31ccfbaf5bff 135
snaranjo 0:31ccfbaf5bff 136 timer.start();
snaranjo 0:31ccfbaf5bff 137 t_ref = 0;
snaranjo 0:31ccfbaf5bff 138 while (1) {
snaranjo 0:31ccfbaf5bff 139 //Ctrl pot range will be -30 to 30 degrees, just to have high point resolution
snaranjo 0:31ccfbaf5bff 140 //Gather inputs
snaranjo 0:31ccfbaf5bff 141 ctrlDeg = ctrlPot*(1.0472f)- 0.5236f;
snaranjo 0:31ccfbaf5bff 142 shaftDeg = pulsesToRadians(wheel.getPulses());
snaranjo 0:31ccfbaf5bff 143 degErr = ctrlDeg - shaftDeg;
snaranjo 0:31ccfbaf5bff 144
snaranjo 0:31ccfbaf5bff 145 // currFactor = currPot*3.3*(1/3.3);
snaranjo 0:31ccfbaf5bff 146 currVal = Current.read()*3.3/.140f;
snaranjo 0:31ccfbaf5bff 147
snaranjo 0:31ccfbaf5bff 148
snaranjo 0:31ccfbaf5bff 149 //Update PID vars and send CMD signal to motor
snaranjo 0:31ccfbaf5bff 150 cmdSig = UpdatePID(pmPid, degErr, shaftDeg);
snaranjo 0:31ccfbaf5bff 151 signal_to_hbridge(cmdSig);
snaranjo 0:31ccfbaf5bff 152 t = timer.read();
snaranjo 0:31ccfbaf5bff 153 //Print out result
snaranjo 0:31ccfbaf5bff 154 // pc.printf("t: %010lu\t SP: %3.3f\t PV: %3.3f\t CV: %3.3f\n",
snaranjo 0:31ccfbaf5bff 155 // t, ctrlDeg, shaftDeg, cmdSig);
snaranjo 0:31ccfbaf5bff 156 if (t - t_ref > 0.1) {
snaranjo 0:31ccfbaf5bff 157 pc.printf("%6.3f\t %5.2f\t %3.3f\t %3.3f\t %3.3f\n",
snaranjo 0:31ccfbaf5bff 158 //t, ctrlDeg, shaftDeg, cmdSig);
snaranjo 0:31ccfbaf5bff 159 t, ctrlDeg, shaftDeg, degErr, currVal);
snaranjo 0:31ccfbaf5bff 160 t_ref = t;
snaranjo 0:31ccfbaf5bff 161 //Current protection of motor
snaranjo 0:31ccfbaf5bff 162 if (((currVal > 6.0f) && (abs(currVal - prev_curr) <0.20))) break;
snaranjo 0:31ccfbaf5bff 163
snaranjo 0:31ccfbaf5bff 164 prev_curr = currVal;
snaranjo 0:31ccfbaf5bff 165 }//end if t_ref
snaranjo 0:31ccfbaf5bff 166 wait(0.0001f);
snaranjo 0:31ccfbaf5bff 167 }//end while loop
snaranjo 0:31ccfbaf5bff 168 //assuming motor needs braknig
snaranjo 0:31ccfbaf5bff 169 signal_to_hbridge(0);
snaranjo 0:31ccfbaf5bff 170 }