Adam Bako / Mbed 2 deprecated 2MotorPID

2 motor control

Dependencies:   Encoder QEI mbed

main.cpp@0:46cf63cba59a, 2017-11-01 (annotated)

Committer:
bako
Date:
Wed Nov 01 09:47:36 2017 +0000
Revision:
0:46cf63cba59a
it works for 2 motors, angle increments go in different directions for the arms

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bako 0:46cf63cba59a 1 /*
bako 0:46cf63cba59a 2 * Parts of the code copied from PES lecture slides
bako 0:46cf63cba59a 3 */
bako 0:46cf63cba59a 4
bako 0:46cf63cba59a 5 // include all necessary libraries
bako 0:46cf63cba59a 6 #include "mbed.h"
bako 0:46cf63cba59a 7 #include "QEI.h"
bako 0:46cf63cba59a 8
bako 0:46cf63cba59a 9 //intialize all pins
bako 0:46cf63cba59a 10 PwmOut motor1(D5);
bako 0:46cf63cba59a 11 PwmOut motor2(D6);
bako 0:46cf63cba59a 12 DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 13 DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 14 QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 15 QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 16 DigitalIn button1(D8); //button to move cw
bako 0:46cf63cba59a 17 DigitalIn button2(D9); //button to move ccw
bako 0:46cf63cba59a 18
bako 0:46cf63cba59a 19 //initialize variables
bako 0:46cf63cba59a 20 double angleIncrement = 0.036; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
bako 0:46cf63cba59a 21
bako 0:46cf63cba59a 22 const double motor1KP=1.3; //Proportional gain of motor1 PI control
bako 0:46cf63cba59a 23 const double motor1KI=0.5; //Integral gain of motor1 PI control
bako 0:46cf63cba59a 24 const double motor1KD=0.5; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 25
bako 0:46cf63cba59a 26 const double motor2KP=1.3; //Proportional gain of motor1 PI control
bako 0:46cf63cba59a 27 const double motor2KI=0.5; //Integral gain of motor1 PI control
bako 0:46cf63cba59a 28 const double motor2KD=0.5; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 29
bako 0:46cf63cba59a 30 const double N=100; //LP filter coefficient
bako 0:46cf63cba59a 31 const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
bako 0:46cf63cba59a 32 const double controllerTickerTime=0.01; //ticker frequency
bako 0:46cf63cba59a 33
bako 0:46cf63cba59a 34 double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 35 double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 36 double desiredPos1 =0; //desired position of motor1
bako 0:46cf63cba59a 37
bako 0:46cf63cba59a 38 double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 39 double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 40 double desiredPos2 =0; //desired position of motor1
bako 0:46cf63cba59a 41 //initialize ticker for checking and correcting the angle
bako 0:46cf63cba59a 42 Ticker myControllerTicker;
bako 0:46cf63cba59a 43
bako 0:46cf63cba59a 44
bako 0:46cf63cba59a 45 double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError){
bako 0:46cf63cba59a 46 const double a1 = -4/(N*Ts+2);
bako 0:46cf63cba59a 47 const double a2 = -(N*Ts-2)/(N*Ts+2);
bako 0:46cf63cba59a 48 const double b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
bako 0:46cf63cba59a 49 const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2);
bako 0:46cf63cba59a 50 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
bako 0:46cf63cba59a 51
bako 0:46cf63cba59a 52 double v = error - a1*intError - a2*DifError;
bako 0:46cf63cba59a 53 double u = b0*v + b1*intError + b2*DifError;
bako 0:46cf63cba59a 54 DifError = intError; intError = v;
bako 0:46cf63cba59a 55 return u;
bako 0:46cf63cba59a 56 }
bako 0:46cf63cba59a 57
bako 0:46cf63cba59a 58
bako 0:46cf63cba59a 59 void motorButtonController(){
bako 0:46cf63cba59a 60 double position1= encoderToMotor*motor1Encoder.getPulses();
bako 0:46cf63cba59a 61 double posError1 = desiredPos1 - position1;
bako 0:46cf63cba59a 62
bako 0:46cf63cba59a 63 //change direction based on error sign
bako 0:46cf63cba59a 64 if(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
bako 0:46cf63cba59a 65 motor1Dir=0;
bako 0:46cf63cba59a 66 } else {
bako 0:46cf63cba59a 67 motor1Dir =1;
bako 0:46cf63cba59a 68 }
bako 0:46cf63cba59a 69 //set motor speed based on PI controller error
bako 0:46cf63cba59a 70 motor1 = fabs(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
bako 0:46cf63cba59a 71
bako 0:46cf63cba59a 72 double position2= encoderToMotor*motor2Encoder.getPulses();
bako 0:46cf63cba59a 73 double posError2 = desiredPos2 - position2;
bako 0:46cf63cba59a 74
bako 0:46cf63cba59a 75 //change direction based on error sign
bako 0:46cf63cba59a 76 if(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
bako 0:46cf63cba59a 77 motor2Dir=0;
bako 0:46cf63cba59a 78 } else {
bako 0:46cf63cba59a 79 motor2Dir =1;
bako 0:46cf63cba59a 80 }
bako 0:46cf63cba59a 81 //set motor speed based on PI controller error
bako 0:46cf63cba59a 82 motor2 = fabs(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
bako 0:46cf63cba59a 83
bako 0:46cf63cba59a 84 }
bako 0:46cf63cba59a 85
bako 0:46cf63cba59a 86 int main(){
bako 0:46cf63cba59a 87 wait(2);
bako 0:46cf63cba59a 88 myControllerTicker.attach(&motorButtonController, controllerTickerTime);
bako 0:46cf63cba59a 89 while(1) {
bako 0:46cf63cba59a 90 if(!button1) {
bako 0:46cf63cba59a 91 desiredPos1+=angleIncrement;
bako 0:46cf63cba59a 92 desiredPos2-=angleIncrement;
bako 0:46cf63cba59a 93 wait(0.5f);
bako 0:46cf63cba59a 94 }
bako 0:46cf63cba59a 95
bako 0:46cf63cba59a 96 if(!button2) {
bako 0:46cf63cba59a 97 desiredPos1-=angleIncrement;
bako 0:46cf63cba59a 98 desiredPos2+=angleIncrement;
bako 0:46cf63cba59a 99 wait(0.5f);
bako 0:46cf63cba59a 100 }
bako 0:46cf63cba59a 101 }
bako 0:46cf63cba59a 102 }