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@1:864a5f8bb886, 2017-11-02 (annotated)

Committer:
Frimzenner
Date:
Thu Nov 02 11:00:44 2017 +0000
Revision:
1:864a5f8bb886
Parent:
0:46cf63cba59a
Child:
2:69bfc537508f
Now including the inverse kinematics and foolproof location desirements

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"
bako 0:46cf63cba59a 9
bako 0:46cf63cba59a 10 //intialize all pins
bako 0:46cf63cba59a 11 PwmOut motor1(D5);
bako 0:46cf63cba59a 12 PwmOut motor2(D6);
bako 0:46cf63cba59a 13 DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 14 DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front))
bako 0:46cf63cba59a 15 QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 16 QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
bako 0:46cf63cba59a 17 DigitalIn button1(D8); //button to move cw
bako 0:46cf63cba59a 18 DigitalIn button2(D9); //button to move ccw
bako 0:46cf63cba59a 19
bako 0:46cf63cba59a 20 //initialize variables
Frimzenner 1:864a5f8bb886 21 const float pi = 3.14159265358979323846; //value for pi
Frimzenner 1:864a5f8bb886 22 double positionIncrement = 30; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
bako 0:46cf63cba59a 23
bako 0:46cf63cba59a 24 const double motor1KP=1.3; //Proportional gain of motor1 PI control
bako 0:46cf63cba59a 25 const double motor1KI=0.5; //Integral gain of motor1 PI control
bako 0:46cf63cba59a 26 const double motor1KD=0.5; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 27
bako 0:46cf63cba59a 28 const double motor2KP=1.3; //Proportional gain of motor1 PI control
bako 0:46cf63cba59a 29 const double motor2KI=0.5; //Integral gain of motor1 PI control
bako 0:46cf63cba59a 30 const double motor2KD=0.5; // Differential gain of motor1 PID control
bako 0:46cf63cba59a 31
bako 0:46cf63cba59a 32 const double N=100; //LP filter coefficient
bako 0:46cf63cba59a 33 const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
bako 0:46cf63cba59a 34 const double controllerTickerTime=0.01; //ticker frequency
bako 0:46cf63cba59a 35
bako 0:46cf63cba59a 36 double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 37 double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
Frimzenner 1:864a5f8bb886 38 double desiredAngle1 =0; //desired position of motor1
bako 0:46cf63cba59a 39
bako 0:46cf63cba59a 40 double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
bako 0:46cf63cba59a 41 double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
Frimzenner 1:864a5f8bb886 42 double desiredAngle2 =0; //desired position of motor1
bako 0:46cf63cba59a 43 //initialize ticker for checking and correcting the angle
bako 0:46cf63cba59a 44 Ticker myControllerTicker;
bako 0:46cf63cba59a 45
Frimzenner 1:864a5f8bb886 46 const float l1 = 460; //Length of the arm from base to joint 1 (arm1) ENTER MANUALLY [mm]
Frimzenner 1:864a5f8bb886 47 const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2) ENTER MANUALLY [mm]
Frimzenner 1:864a5f8bb886 48 float x_des = 0; //(initial)desired x location of the end-effector (ee)
Frimzenner 1:864a5f8bb886 49 float y_des = l1+l2; //(initial) desired y location of the end-effector (ee)
Frimzenner 1:864a5f8bb886 50 float xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors
Frimzenner 1:864a5f8bb886 51 //q1 is the angle for the base motor, q2 is the angle for the elbow motor, both in [rad]
bako 0:46cf63cba59a 52
bako 0:46cf63cba59a 53 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 54 const double a1 = -4/(N*Ts+2);
bako 0:46cf63cba59a 55 const double a2 = -(N*Ts-2)/(N*Ts+2);
bako 0:46cf63cba59a 56 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 57 const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2);
bako 0:46cf63cba59a 58 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 59
bako 0:46cf63cba59a 60 double v = error - a1*intError - a2*DifError;
bako 0:46cf63cba59a 61 double u = b0*v + b1*intError + b2*DifError;
Frimzenner 1:864a5f8bb886 62 DifError = intError;
Frimzenner 1:864a5f8bb886 63 intError = v;
bako 0:46cf63cba59a 64 return u;
bako 0:46cf63cba59a 65 }
bako 0:46cf63cba59a 66
Frimzenner 1:864a5f8bb886 67 //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 1:864a5f8bb886 68 void motorAngle(){
Frimzenner 1:864a5f8bb886 69 //Function for making sure the arm does not exceed its maximum reach
Frimzenner 1:864a5f8bb886 70 //if it tries to go beyond its max. reach
Frimzenner 1:864a5f8bb886 71 //it will try to reach a point within reach in the same direction as desired.
Frimzenner 1:864a5f8bb886 72 xe = x_des;
Frimzenner 1:864a5f8bb886 73 ye = y_des;
Frimzenner 1:864a5f8bb886 74 while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)) {
Frimzenner 1:864a5f8bb886 75 if (y_des == 0) { //make sure you do not divide by 0 if ye == 0
Frimzenner 1:864a5f8bb886 76 xe = x_des - 1;
Frimzenner 1:864a5f8bb886 77 } else {
Frimzenner 1:864a5f8bb886 78 xe = x_des - (x_des/y_des)/10; //go to a smaller xe point on the same line
Frimzenner 1:864a5f8bb886 79 }
Frimzenner 1:864a5f8bb886 80 if (x_des == 0) { //make sure you do not divide by 0 if xe == 0
Frimzenner 1:864a5f8bb886 81 ye = y_des - 1;
Frimzenner 1:864a5f8bb886 82 } else {
Frimzenner 1:864a5f8bb886 83 ye = y_des - (y_des/x_des)/10; //go to a smaller ye point on the same line
Frimzenner 1:864a5f8bb886 84 }
Frimzenner 1:864a5f8bb886 85 x_des = xe;
Frimzenner 1:864a5f8bb886 86 y_des = ye;
Frimzenner 1:864a5f8bb886 87 }
Frimzenner 1:864a5f8bb886 88
Frimzenner 1:864a5f8bb886 89 //from here on is the code for setting the angles for the motors
Frimzenner 1:864a5f8bb886 90 D = ((pow(l1,2)+pow(l2,2))-pow(xe,2)-pow(ye,2))/(2*l1*l2); //D = cos(phi)
Frimzenner 1:864a5f8bb886 91 phi = atan2(sqrt(1 - pow(D, 2)), D); //angle between arm1 and arm2, from arm1 to arm2 [rad]
Frimzenner 1:864a5f8bb886 92 //Use atan2(sqrt(1 - pow(D, 2)),D) for "elbow down" position (like your right arm)
Frimzenner 1:864a5f8bb886 93 q2 = pi - phi; //angle of arm2 with respect to the orientation of arm1, motor2 [rad]
Frimzenner 1:864a5f8bb886 94 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 1:864a5f8bb886 95 q2 = -pi/2;
Frimzenner 1:864a5f8bb886 96 } else if ( q2 > pi/2) {
Frimzenner 1:864a5f8bb886 97 q2 = pi/2;
Frimzenner 1:864a5f8bb886 98 }
Frimzenner 1:864a5f8bb886 99 beta = atan2(ye, xe); //angle between "line from origin to ee" and x-axis [rad]
Frimzenner 1:864a5f8bb886 100 alpha = atan2(l2*sin(q2), l1+l2*cos(q2)); //angle between "line from origin to ee" and arm1 [rad]
Frimzenner 1:864a5f8bb886 101 q1 = beta - alpha; //angle of arm 1 with respect to the x-axis, motor1 [rad]
Frimzenner 1:864a5f8bb886 102 float 1radDeg = 180/pi; //amount of degrees in 1 radian
Frimzenner 1:864a5f8bb886 103 float rad2rot = 1radDeg/360;
Frimzenner 1:864a5f8bb886 104 desiredAngle1 = q1 * rad2rot;
Frimzenner 1:864a5f8bb886 105 desiredAngle2 = q2 * rad2rot;
Frimzenner 1:864a5f8bb886 106 }
bako 0:46cf63cba59a 107
bako 0:46cf63cba59a 108 void motorButtonController(){
bako 0:46cf63cba59a 109 double position1= encoderToMotor*motor1Encoder.getPulses();
Frimzenner 1:864a5f8bb886 110 double posError1 = desiredAngle1 - position1;
Frimzenner 1:864a5f8bb886 111
bako 0:46cf63cba59a 112 //change direction based on error sign
bako 0:46cf63cba59a 113 if(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
bako 0:46cf63cba59a 114 motor1Dir=0;
bako 0:46cf63cba59a 115 } else {
bako 0:46cf63cba59a 116 motor1Dir =1;
bako 0:46cf63cba59a 117 }
bako 0:46cf63cba59a 118 //set motor speed based on PI controller error
bako 0:46cf63cba59a 119 motor1 = fabs(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
Frimzenner 1:864a5f8bb886 120
bako 0:46cf63cba59a 121 double position2= encoderToMotor*motor2Encoder.getPulses();
Frimzenner 1:864a5f8bb886 122 double posError2 = desiredAngle2 - position2;
Frimzenner 1:864a5f8bb886 123
bako 0:46cf63cba59a 124 //change direction based on error sign
bako 0:46cf63cba59a 125 if(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
bako 0:46cf63cba59a 126 motor2Dir=0;
bako 0:46cf63cba59a 127 } else {
bako 0:46cf63cba59a 128 motor2Dir =1;
bako 0:46cf63cba59a 129 }
bako 0:46cf63cba59a 130 //set motor speed based on PI controller error
bako 0:46cf63cba59a 131 motor2 = fabs(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
bako 0:46cf63cba59a 132
bako 0:46cf63cba59a 133 }
bako 0:46cf63cba59a 134
Frimzenner 1:864a5f8bb886 135 int main()
Frimzenner 1:864a5f8bb886 136 {
bako 0:46cf63cba59a 137 wait(2);
bako 0:46cf63cba59a 138 myControllerTicker.attach(&motorButtonController, controllerTickerTime);
bako 0:46cf63cba59a 139 while(1) {
bako 0:46cf63cba59a 140 if(!button1) {
Frimzenner 1:864a5f8bb886 141 y_des+=positionIncrement;
bako 0:46cf63cba59a 142 wait(0.5f);
bako 0:46cf63cba59a 143 }
bako 0:46cf63cba59a 144
bako 0:46cf63cba59a 145 if(!button2) {
Frimzenner 1:864a5f8bb886 146 y_des-=positionIncrement;
bako 0:46cf63cba59a 147 wait(0.5f);
bako 0:46cf63cba59a 148 }
bako 0:46cf63cba59a 149 }
bako 0:46cf63cba59a 150 }