Thijs de Kleijn / Mbed 2 deprecated 2MotorPID

mbed motor control with emg

Dependencies:   Encoder HIDScope MODSERIAL QEI TextLCD biquadFilter mbed

Fork of 2MotorPID by Adam Bako

main.cpp@2:69bfc537508f, 2017-11-02 (annotated)

Committer:
Frimzenner
Date:
Thu Nov 02 18:37:18 2017 +0000
Revision:
2:69bfc537508f
Parent:
1:864a5f8bb886
Child:
3:c63c0a23ea21
Reach control adjusted for all quadrant of the (unit) circle

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"
Frimzenner 1:864a5f8bb886 8 #include "math.h"
Frimzenner 2:69bfc537508f 9 #include "iostream"
Frimzenner 2:69bfc537508f 10 #include "MODSERIAL.h"
bako 0:46cf63cba59a 11
bako 0:46cf63cba59a 12 //intialize all pins
bako 0:46cf63cba59a 13 PwmOut motor1(D5);
bako 0:46cf63cba59a 14 PwmOut motor2(D6);
bako 0:46cf63cba59a 15 DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 16 DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 17 QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 18 QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 19 DigitalIn button1(D8); //button to move cw
bako 0:46cf63cba59a 20 DigitalIn button2(D9); //button to move ccw
Frimzenner 2:69bfc537508f 21 DigitalIn button3(D2);
Frimzenner 2:69bfc537508f 22 DigitalIn button4(D3);
Frimzenner 2:69bfc537508f 23 MODSERIAL pc(USBTX, USBRX);
bako 0:46cf63cba59a 24
bako 0:46cf63cba59a 25 //initialize variables
Frimzenner 1:864a5f8bb886 26 const float pi = 3.14159265358979323846; //value for pi
Frimzenner 2:69bfc537508f 27 double positionIncrement = 20; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
bako 0:46cf63cba59a 28
Frimzenner 2:69bfc537508f 29 const double motor1KP=7.0; //Proportional gain of motor1 PI control
Frimzenner 2:69bfc537508f 30 const double motor1KI=3.0; //Integral gain of motor1 PI control
Frimzenner 2:69bfc537508f 31 const double motor1KD=3.0; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 32
bako 0:46cf63cba59a 33 const double motor2KP=1.3; //Proportional gain of motor1 PI control
bako 0:46cf63cba59a 34 const double motor2KI=0.5; //Integral gain of motor1 PI control
Frimzenner 2:69bfc537508f 35 const double motor2KD=1.0; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 36
bako 0:46cf63cba59a 37 const double N=100; //LP filter coefficient
bako 0:46cf63cba59a 38 const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
bako 0:46cf63cba59a 39 const double controllerTickerTime=0.01; //ticker frequency
bako 0:46cf63cba59a 40
bako 0:46cf63cba59a 41 double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 42 double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
Frimzenner 1:864a5f8bb886 43 double desiredAngle1 =0; //desired position of motor1
bako 0:46cf63cba59a 44
bako 0:46cf63cba59a 45 double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 46 double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
Frimzenner 1:864a5f8bb886 47 double desiredAngle2 =0; //desired position of motor1
bako 0:46cf63cba59a 48 //initialize ticker for checking and correcting the angle
bako 0:46cf63cba59a 49 Ticker myControllerTicker;
bako 0:46cf63cba59a 50
Frimzenner 1:864a5f8bb886 51 const float l1 = 460; //Length of the arm from base to joint 1 (arm1) ENTER MANUALLY [mm]
Frimzenner 1:864a5f8bb886 52 const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2) ENTER MANUALLY [mm]
Frimzenner 2:69bfc537508f 53 float x_des = l1+l2; //(initial)desired x location of the end-effector (ee)
Frimzenner 2:69bfc537508f 54 float y_des = 0; //(initial) desired y location of the end-effector (ee)
Frimzenner 1:864a5f8bb886 55 float xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors
Frimzenner 2:69bfc537508f 56 float radDeg, rad2rot;
Frimzenner 1:864a5f8bb886 57 //q1 is the angle for the base motor, q2 is the angle for the elbow motor, both in [rad]
bako 0:46cf63cba59a 58
Frimzenner 2:69bfc537508f 59
Frimzenner 2:69bfc537508f 60 double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError)
Frimzenner 2:69bfc537508f 61 {
bako 0:46cf63cba59a 62 const double a1 = -4/(N*Ts+2);
bako 0:46cf63cba59a 63 const double a2 = -(N*Ts-2)/(N*Ts+2);
bako 0:46cf63cba59a 64 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 65 const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2);
bako 0:46cf63cba59a 66 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 67
bako 0:46cf63cba59a 68 double v = error - a1*intError - a2*DifError;
bako 0:46cf63cba59a 69 double u = b0*v + b1*intError + b2*DifError;
Frimzenner 1:864a5f8bb886 70 DifError = intError;
Frimzenner 1:864a5f8bb886 71 intError = v;
bako 0:46cf63cba59a 72 return u;
bako 0:46cf63cba59a 73 }
bako 0:46cf63cba59a 74
Frimzenner 1:864a5f8bb886 75 //Code for motor angles as a function of the length and positions of the arms, for a double revolutional joint arm in 2D plane
Frimzenner 2:69bfc537508f 76 void motorAngle()
Frimzenner 2:69bfc537508f 77 {
Frimzenner 1:864a5f8bb886 78 //Function for making sure the arm does not exceed its maximum reach
Frimzenner 1:864a5f8bb886 79 //if it tries to go beyond its max. reach
Frimzenner 1:864a5f8bb886 80 //it will try to reach a point within reach in the same direction as desired.
Frimzenner 2:69bfc537508f 81 //Works for all 4 quadrants of the (unit) circle
Frimzenner 1:864a5f8bb886 82 xe = x_des;
Frimzenner 1:864a5f8bb886 83 ye = y_des;
Frimzenner 2:69bfc537508f 84
Frimzenner 2:69bfc537508f 85 while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
Frimzenner 2:69bfc537508f 86 if (xe > 0) { //right hand plane
Frimzenner 2:69bfc537508f 87 if (ye > 0) { //positive x & y QUADRANT 1
Frimzenner 2:69bfc537508f 88 xe = x_des - (x_des/y_des); //go to a smaller xe point on the same line
Frimzenner 2:69bfc537508f 89 ye = y_des - (y_des/x_des); //go to a smaller ye point on the same line
Frimzenner 2:69bfc537508f 90 } else if (ye < 0) { //positive x, negative y QUADRANT 2
Frimzenner 2:69bfc537508f 91 xe = x_des - (x_des/y_des); //smaller xe
Frimzenner 2:69bfc537508f 92 ye = y_des + (y_des/x_des); //greater ye
Frimzenner 2:69bfc537508f 93 } else if (ye == 0) { //positive x, y == 0
Frimzenner 2:69bfc537508f 94 xe = x_des - 1; //stay on the x-axis but at a smaller value (within reach)
Frimzenner 2:69bfc537508f 95 }
Frimzenner 2:69bfc537508f 96 } else if (xe < 0) { //left hand plane
Frimzenner 2:69bfc537508f 97 if (ye > 0) { //negative x, positive y QUADRANT 4
Frimzenner 2:69bfc537508f 98 xe = x_des + (x_des/y_des); //greater xe
Frimzenner 2:69bfc537508f 99 ye = y_des - (y_des/x_des); //smaller ye
Frimzenner 2:69bfc537508f 100 } else if (ye < 0) { //negative x & y QUADRANT 3
Frimzenner 2:69bfc537508f 101 xe = x_des + (x_des/y_des); //greater xe
Frimzenner 2:69bfc537508f 102 ye = y_des + (y_des/x_des); //greater ye
Frimzenner 2:69bfc537508f 103 } else if (ye ==0) { //negative x, y == 0
Frimzenner 2:69bfc537508f 104 xe = x_des + 1; //stay on the x-axis but at a greater value (within reach)
Frimzenner 2:69bfc537508f 105 }
Frimzenner 2:69bfc537508f 106
Frimzenner 2:69bfc537508f 107 } else if (xe == 0) { //on the y-axis
Frimzenner 2:69bfc537508f 108 if (ye > 0) { //x == 0, positive y
Frimzenner 2:69bfc537508f 109 ye = y_des - 1; //stay on the y-axis but at a smaller value (within reach)
Frimzenner 2:69bfc537508f 110 } else if (ye < 0) { //x == 0, negative y
Frimzenner 2:69bfc537508f 111 ye = y_des + 1; //stay on the y-axis but at a greater value (within reach)
Frimzenner 2:69bfc537508f 112 //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case
Frimzenner 2:69bfc537508f 113 }
Frimzenner 2:69bfc537508f 114 x_des = xe;
Frimzenner 2:69bfc537508f 115 y_des = ye;
Frimzenner 1:864a5f8bb886 116 }
Frimzenner 1:864a5f8bb886 117 }
Frimzenner 1:864a5f8bb886 118
Frimzenner 2:69bfc537508f 119
Frimzenner 1:864a5f8bb886 120 //from here on is the code for setting the angles for the motors
Frimzenner 1:864a5f8bb886 121 D = ((pow(l1,2)+pow(l2,2))-pow(xe,2)-pow(ye,2))/(2*l1*l2); //D = cos(phi)
Frimzenner 1:864a5f8bb886 122 phi = atan2(sqrt(1 - pow(D, 2)), D); //angle between arm1 and arm2, from arm1 to arm2 [rad]
Frimzenner 1:864a5f8bb886 123 //Use atan2(sqrt(1 - pow(D, 2)),D) for "elbow down" position (like your right arm)
Frimzenner 1:864a5f8bb886 124 q2 = pi - phi; //angle of arm2 with respect to the orientation of arm1, motor2 [rad]
Frimzenner 1:864a5f8bb886 125 if (-pi/2 > q2) { //Make sure the angle of motor2 doesn’t wreck our setup (max -90 or 90 degrees w.r.t. arm1)
Frimzenner 2:69bfc537508f 126 q2 = 0;
Frimzenner 1:864a5f8bb886 127 } else if ( q2 > pi/2) {
Frimzenner 1:864a5f8bb886 128 q2 = pi/2;
Frimzenner 1:864a5f8bb886 129 }
Frimzenner 1:864a5f8bb886 130 beta = atan2(ye, xe); //angle between "line from origin to ee" and x-axis [rad]
Frimzenner 1:864a5f8bb886 131 alpha = atan2(l2*sin(q2), l1+l2*cos(q2)); //angle between "line from origin to ee" and arm1 [rad]
Frimzenner 1:864a5f8bb886 132 q1 = beta - alpha; //angle of arm 1 with respect to the x-axis, motor1 [rad]
Frimzenner 2:69bfc537508f 133 radDeg = 180/pi;
Frimzenner 2:69bfc537508f 134 rad2rot = radDeg/360;
Frimzenner 1:864a5f8bb886 135 desiredAngle1 = q1 * rad2rot;
Frimzenner 1:864a5f8bb886 136 desiredAngle2 = q2 * rad2rot;
Frimzenner 2:69bfc537508f 137 pc.printf("\n New values \n\r");
Frimzenner 2:69bfc537508f 138 pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
Frimzenner 2:69bfc537508f 139 pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n\r", q1, desiredAngle1, q2, desiredAngle2);
Frimzenner 1:864a5f8bb886 140 }
bako 0:46cf63cba59a 141
Frimzenner 2:69bfc537508f 142 void motorButtonController()
Frimzenner 2:69bfc537508f 143 {
Frimzenner 2:69bfc537508f 144 double angle1= encoderToMotor*motor1Encoder.getPulses();
Frimzenner 2:69bfc537508f 145 double angleError1 = (desiredAngle1 - angle1)*1.25;
Frimzenner 1:864a5f8bb886 146
bako 0:46cf63cba59a 147 //change direction based on error sign
Frimzenner 2:69bfc537508f 148 if(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
bako 0:46cf63cba59a 149 motor1Dir=0;
bako 0:46cf63cba59a 150 } else {
bako 0:46cf63cba59a 151 motor1Dir =1;
bako 0:46cf63cba59a 152 }
bako 0:46cf63cba59a 153 //set motor speed based on PI controller error
Frimzenner 2:69bfc537508f 154 motor1 = fabs(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
Frimzenner 1:864a5f8bb886 155
Frimzenner 2:69bfc537508f 156 double angle2= encoderToMotor*motor2Encoder.getPulses();
Frimzenner 2:69bfc537508f 157 double angleError2 = desiredAngle2 - angle2;
Frimzenner 1:864a5f8bb886 158
bako 0:46cf63cba59a 159 //change direction based on error sign
Frimzenner 2:69bfc537508f 160 if(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
bako 0:46cf63cba59a 161 motor2Dir=0;
bako 0:46cf63cba59a 162 } else {
bako 0:46cf63cba59a 163 motor2Dir =1;
bako 0:46cf63cba59a 164 }
bako 0:46cf63cba59a 165 //set motor speed based on PI controller error
Frimzenner 2:69bfc537508f 166 motor2 = fabs(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
bako 0:46cf63cba59a 167
bako 0:46cf63cba59a 168 }
bako 0:46cf63cba59a 169
Frimzenner 1:864a5f8bb886 170 int main()
Frimzenner 1:864a5f8bb886 171 {
bako 0:46cf63cba59a 172 wait(2);
Frimzenner 2:69bfc537508f 173 xe = x_des;
Frimzenner 2:69bfc537508f 174 ye = y_des;
Frimzenner 2:69bfc537508f 175 pc.baud(115200);
Frimzenner 2:69bfc537508f 176 pc.printf("\n Start up complete \n \r");
Frimzenner 2:69bfc537508f 177 pc.printf("initial values: \n \r");
Frimzenner 2:69bfc537508f 178 pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
Frimzenner 2:69bfc537508f 179 pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n \r", q1, desiredAngle1, q2, desiredAngle2);
bako 0:46cf63cba59a 180 myControllerTicker.attach(&motorButtonController, controllerTickerTime);
bako 0:46cf63cba59a 181 while(1) {
bako 0:46cf63cba59a 182 if(!button1) {
Frimzenner 2:69bfc537508f 183 y_des += positionIncrement;
Frimzenner 2:69bfc537508f 184 motorAngle();
Frimzenner 2:69bfc537508f 185 wait(0.3f);
bako 0:46cf63cba59a 186 }
bako 0:46cf63cba59a 187
bako 0:46cf63cba59a 188 if(!button2) {
Frimzenner 2:69bfc537508f 189 y_des -= positionIncrement;
Frimzenner 2:69bfc537508f 190 motorAngle();
Frimzenner 2:69bfc537508f 191 wait(0.3f);
Frimzenner 2:69bfc537508f 192 }
Frimzenner 2:69bfc537508f 193
Frimzenner 2:69bfc537508f 194 if(button3) {
Frimzenner 2:69bfc537508f 195 x_des += positionIncrement;
Frimzenner 2:69bfc537508f 196 motorAngle();
Frimzenner 2:69bfc537508f 197 wait(0.3f);
Frimzenner 2:69bfc537508f 198 }
Frimzenner 2:69bfc537508f 199 if(button4) {
Frimzenner 2:69bfc537508f 200 x_des -= positionIncrement;
Frimzenner 2:69bfc537508f 201 motorAngle();
Frimzenner 2:69bfc537508f 202 wait(0.3f);
bako 0:46cf63cba59a 203 }
bako 0:46cf63cba59a 204 }
bako 0:46cf63cba59a 205 }