BMT Module 9 Group 4 / Mbed 2 deprecated DemoState

DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2

Dependencies:   FastPWM MODSERIAL Matrix MatrixMath mbed QEI

main.cpp@1:2219a519e2bf, 2018-11-01 (annotated)

Committer:
1856413
Date:
Thu Nov 01 19:24:28 2018 +0000
Revision:
1:2219a519e2bf
Parent:
0:550f6e86da32
Child:
2:638c6155d0af
Geen aangegeven bugs, maar alleen M2 draait en wisselt continue van richting en reageert niet op POT1 of BUT2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
1856413 0:550f6e86da32 1 #include "mbed.h"
1856413 0:550f6e86da32 2 #include "FastPWM.h"
1856413 0:550f6e86da32 3 #include "MODSERIAL.h"
1856413 0:550f6e86da32 4 #include "Matrix.h"
1856413 0:550f6e86da32 5 #include "QEI.h"
1856413 0:550f6e86da32 6 #include <math.h>
1856413 0:550f6e86da32 7 MODSERIAL pc(USBTX, USBRX);
1856413 1:2219a519e2bf 8 DigitalOut motor1Direction(D7);
1856413 1:2219a519e2bf 9 DigitalOut motor2Direction(D4);
1856413 0:550f6e86da32 10 DigitalOut Led(LED_GREEN);
1856413 0:550f6e86da32 11 DigitalIn button2(SW3);
1856413 1:2219a519e2bf 12 FastPWM motor1PWM(D6);
1856413 1:2219a519e2bf 13 FastPWM motor2PWM(D5);
1856413 0:550f6e86da32 14 AnalogIn potMeter1(A4);
1856413 0:550f6e86da32 15 AnalogIn potMeter2(A5);
1856413 0:550f6e86da32 16 QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 17 QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 18
1856413 0:550f6e86da32 19 // Tickers
1856413 0:550f6e86da32 20 Ticker startMotor;
1856413 0:550f6e86da32 21 Ticker printTicker;
1856413 0:550f6e86da32 22
1856413 0:550f6e86da32 23 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 24 //Global Variables
1856413 1:2219a519e2bf 25
1856413 1:2219a519e2bf 26 double currentPosition1 = 0.0; // Starting position of motor 1 [rad]
1856413 1:2219a519e2bf 27 double currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad]
1856413 1:2219a519e2bf 28 int a = 0;
1856413 1:2219a519e2bf 29
1856413 0:550f6e86da32 30 //Inverse Kinematics
1856413 0:550f6e86da32 31 const double L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!!
1856413 0:550f6e86da32 32 const double L1 = 0.3; // Length link 1 [m] NAMETEN!!
1856413 0:550f6e86da32 33 const double L2 = 0.3; // Length link 2 [m] NAMETEN!!
1856413 0:550f6e86da32 34 const double L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN!
1856413 0:550f6e86da32 35 volatile double q1 = 0.0; // Starting reference angle of first link [rad]
1856413 0:550f6e86da32 36 volatile double q2 = -(0.5*3.14); // Starting reference angle of second link wrt first link [rad]
1856413 0:550f6e86da32 37 double t = 2.0; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!!
1856413 1:2219a519e2bf 38 volatile double vx_des = 0.0; // Starting velocity in x-direction [rad/s]
1856413 1:2219a519e2bf 39 volatile double vy_des = 0.0; // Starting velocity in y-direction [rad/s]
1856413 0:550f6e86da32 40 Matrix Q_set(2,1); // Setting a matrix for storing the angular velocities [rad/sec]
1856413 0:550f6e86da32 41 Matrix J(2,2); // Setting a matrix for the Jacobian
1856413 0:550f6e86da32 42 Matrix V(2,1); // Setting a matrix for storing the EMG-measured velocities
1856413 1:2219a519e2bf 43
1856413 0:550f6e86da32 44 // Motor Control
1856413 0:550f6e86da32 45 volatile double potMeterPosition1 = 0.0;
1856413 0:550f6e86da32 46 volatile double potMeterPosition2 = 0.0;
1856413 0:550f6e86da32 47 volatile double motorValue1 = 0.01;
1856413 0:550f6e86da32 48 volatile double motorValue2 = 0.01;
1856413 0:550f6e86da32 49 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
1856413 0:550f6e86da32 50 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
1856413 0:550f6e86da32 51 volatile double Kd = 0.0;
1856413 0:550f6e86da32 52 volatile double Ts = 0.01; // SAMPLING TIJD!!
1856413 1:2219a519e2bf 53
1856413 0:550f6e86da32 54
1856413 0:550f6e86da32 55 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 56 // Potmeter values TO DETERMINE VX_DES, VY_DES
1856413 1:2219a519e2bf 57 double GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION
1856413 0:550f6e86da32 58 {
1856413 0:550f6e86da32 59 double potMeter1In = potMeter1.read();
1856413 1:2219a519e2bf 60 vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions
1856413 1:2219a519e2bf 61 return vx_des;
1856413 1:2219a519e2bf 62 }
1856413 1:2219a519e2bf 63
1856413 1:2219a519e2bf 64 double SwitchPotmeterVelocity1() { //Button has been pressed,
1856413 1:2219a519e2bf 65 vy_des = vx_des;
1856413 1:2219a519e2bf 66 return vy_des;
1856413 1:2219a519e2bf 67 }
1856413 1:2219a519e2bf 68
1856413 1:2219a519e2bf 69 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 70 // KINEMATIC FUNCTIONS
1856413 1:2219a519e2bf 71
1856413 1:2219a519e2bf 72 Matrix ComputeJ(void)
1856413 1:2219a519e2bf 73 {
1856413 1:2219a519e2bf 74 double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 75 double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 76 double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 77 double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 78 J << a << b
1856413 1:2219a519e2bf 79 << c << d;
1856413 1:2219a519e2bf 80 return J;
1856413 1:2219a519e2bf 81 }
1856413 1:2219a519e2bf 82
1856413 1:2219a519e2bf 83 Matrix ComputeV(void)
1856413 1:2219a519e2bf 84 {
1856413 1:2219a519e2bf 85 V.add(1,1,vx_des); // Add desired x-velocity in V
1856413 1:2219a519e2bf 86 V.add(2,1,vy_des); // Add desired y-velocity in V
1856413 1:2219a519e2bf 87 return V;
1856413 1:2219a519e2bf 88 }
1856413 1:2219a519e2bf 89
1856413 1:2219a519e2bf 90 double IntegrateQ1()
1856413 1:2219a519e2bf 91 {
1856413 1:2219a519e2bf 92 q1 = q1 + (Q_set(1,1))*t; // new value for q1
1856413 1:2219a519e2bf 93 return q1;
1856413 1:2219a519e2bf 94 }
1856413 1:2219a519e2bf 95
1856413 1:2219a519e2bf 96 double IntegrateQ2()
1856413 1:2219a519e2bf 97 {
1856413 1:2219a519e2bf 98 q2 = q2 + (Q_set(2,1))*t; // new value for q2
1856413 1:2219a519e2bf 99 return q2;
1856413 1:2219a519e2bf 100 }
1856413 1:2219a519e2bf 101
1856413 1:2219a519e2bf 102 Matrix ComputeQ_set(void)
1856413 1:2219a519e2bf 103 {
1856413 1:2219a519e2bf 104 float a = J(1,1);
1856413 1:2219a519e2bf 105 float b = J(1,2);
1856413 1:2219a519e2bf 106 float c = J(2,1);
1856413 1:2219a519e2bf 107 float d = J(2,2);
1856413 1:2219a519e2bf 108 float e = V(1,1);
1856413 1:2219a519e2bf 109 float f = V(2,1);
1856413 1:2219a519e2bf 110 float first_row = a*e + b*f;
1856413 1:2219a519e2bf 111 float second_row = c*e + d*f;
1856413 1:2219a519e2bf 112 Q_set.add(1,1,first_row);
1856413 1:2219a519e2bf 113 Q_set.add(2,1,second_row);
1856413 1:2219a519e2bf 114 return Q_set;
1856413 1:2219a519e2bf 115 }
1856413 1:2219a519e2bf 116
1856413 1:2219a519e2bf 117 double ErrorInverseKinematics1()
1856413 1:2219a519e2bf 118 {
1856413 1:2219a519e2bf 119 double errorIK1 = q1 - currentPosition1;
1856413 1:2219a519e2bf 120 return errorIK1;
1856413 0:550f6e86da32 121 }
1856413 0:550f6e86da32 122
1856413 1:2219a519e2bf 123 double ErrorInverseKinematics2()
1856413 0:550f6e86da32 124 {
1856413 1:2219a519e2bf 125 double errorIK2 = q2 - currentPosition2;
1856413 1:2219a519e2bf 126 return errorIK2;
1856413 1:2219a519e2bf 127 }
1856413 1:2219a519e2bf 128 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 129 // MOTOR PART
1856413 1:2219a519e2bf 130 //Encoder Posities
1856413 1:2219a519e2bf 131 double MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad]
1856413 1:2219a519e2bf 132 {
1856413 1:2219a519e2bf 133 int counts1i = Encoder1.getPulses();
1856413 1:2219a519e2bf 134 double counts1 = counts1i*1.0f;
1856413 1:2219a519e2bf 135 double measuredPosition1 = (counts1/8400)*6.28; //Rotational position in radians
1856413 1:2219a519e2bf 136 return measuredPosition1;
1856413 0:550f6e86da32 137 }
1856413 0:550f6e86da32 138
1856413 1:2219a519e2bf 139 double MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad]
1856413 1:2219a519e2bf 140 {
1856413 1:2219a519e2bf 141 int counts2i = Encoder2.getPulses();
1856413 1:2219a519e2bf 142 double counts2 = counts2i*1.0f;
1856413 1:2219a519e2bf 143 double measuredPosition2 = (counts2/8400)*6.28; //Rotational position in radians
1856413 1:2219a519e2bf 144 return measuredPosition2;
1856413 1:2219a519e2bf 145 }
1856413 1:2219a519e2bf 146
1856413 1:2219a519e2bf 147 double FeedbackControl1(double Error1)
1856413 0:550f6e86da32 148 {
1856413 0:550f6e86da32 149 static double Error_integral1 = 0;
1856413 0:550f6e86da32 150 static double Error_prev1 = Error1;
1856413 0:550f6e86da32 151 // Proportional part:
1856413 0:550f6e86da32 152 double u_k1 = Kp * Error1;
1856413 0:550f6e86da32 153 // Integral part:
1856413 0:550f6e86da32 154 Error_integral1 = Error_integral1 + Error1 * Ts;
1856413 0:550f6e86da32 155 double u_i1 = Ki * Error_integral1;
1856413 1:2219a519e2bf 156 // Derivative part:
1856413 0:550f6e86da32 157 double Error_derivative1 = (Error1 - Error_prev1)/Ts;
1856413 0:550f6e86da32 158 double u_d1 = Kd * Error_derivative1;
1856413 0:550f6e86da32 159 Error_prev1 = Error1;
1856413 0:550f6e86da32 160 // Sum all parts and return it
1856413 0:550f6e86da32 161 return u_k1 + u_i1 + u_d1; //motorValue
1856413 0:550f6e86da32 162 }
1856413 1:2219a519e2bf 163
1856413 0:550f6e86da32 164 double FeedbackControl2(double Error2)
1856413 0:550f6e86da32 165 {
1856413 0:550f6e86da32 166 static double Error_integral2 = 0;
1856413 0:550f6e86da32 167 static double Error_prev2 = Error2;
1856413 0:550f6e86da32 168 // Proportional part:
1856413 0:550f6e86da32 169 double u_k2 = Kp * Error2;
1856413 0:550f6e86da32 170 // Integral part:
1856413 0:550f6e86da32 171 Error_integral2 = Error_integral2 + Error2 * Ts;
1856413 0:550f6e86da32 172 double u_i2 = Ki * Error_integral2;
1856413 1:2219a519e2bf 173 // Derivative part:
1856413 0:550f6e86da32 174 double Error_derivative2 = (Error2 - Error_prev2)/Ts;
1856413 0:550f6e86da32 175 double u_d2 = Kd * Error_derivative2;
1856413 0:550f6e86da32 176 Error_prev2 = Error2;
1856413 0:550f6e86da32 177 // Sum all parts and return it
1856413 0:550f6e86da32 178 return u_k2 + u_i2 + u_d2; //motorValue
1856413 0:550f6e86da32 179 }
1856413 1:2219a519e2bf 180
1856413 0:550f6e86da32 181 void SetMotor1(double motorValue1)
1856413 0:550f6e86da32 182 {
1856413 0:550f6e86da32 183 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 184 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 185 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 186 // within range
1856413 0:550f6e86da32 187 // 0 = clockwise motor direction
1856413 0:550f6e86da32 188 // 1 = counterclockwise motor direction
1856413 0:550f6e86da32 189 if (motorValue1 >=0) {
1856413 0:550f6e86da32 190 motor1Direction=0;
1856413 0:550f6e86da32 191 } else {
1856413 0:550f6e86da32 192 motor1Direction=1;
1856413 0:550f6e86da32 193 }
1856413 0:550f6e86da32 194 if (fabs(motorValue1)>1) {
1856413 0:550f6e86da32 195 motor1PWM = 1;
1856413 0:550f6e86da32 196 } else {
1856413 0:550f6e86da32 197 motor1PWM = fabs(motorValue1);
1856413 0:550f6e86da32 198 }
1856413 0:550f6e86da32 199 }
1856413 1:2219a519e2bf 200
1856413 0:550f6e86da32 201 void SetMotor2(double motorValue2)
1856413 0:550f6e86da32 202 {
1856413 0:550f6e86da32 203 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 204 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 205 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 206 // within range
1856413 0:550f6e86da32 207 // 0 = counterclockwise motor direction
1856413 0:550f6e86da32 208 // 1 = clockwise motor direction
1856413 0:550f6e86da32 209 if (motorValue2 >=0) {
1856413 0:550f6e86da32 210 motor2Direction=1;
1856413 0:550f6e86da32 211 } else {
1856413 0:550f6e86da32 212 motor2Direction=0;
1856413 0:550f6e86da32 213 }
1856413 0:550f6e86da32 214 if (fabs(motorValue2)>1) {
1856413 0:550f6e86da32 215 motor2PWM = 1;
1856413 0:550f6e86da32 216 } else {
1856413 0:550f6e86da32 217 motor2PWM = fabs(motorValue2);
1856413 0:550f6e86da32 218 }
1856413 0:550f6e86da32 219 }
1856413 1:2219a519e2bf 220
1856413 1:2219a519e2bf 221 /*void MeasureAndControl1(void)
1856413 0:550f6e86da32 222 {
1856413 0:550f6e86da32 223 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 224 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 225 // a simple Feedback controller. Call this from a Ticker.
1856413 0:550f6e86da32 226 potMeterPosition1 = q1;
1856413 0:550f6e86da32 227 currentPosition1 = MeasureEncoderPosition1();
1856413 0:550f6e86da32 228 motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
1856413 0:550f6e86da32 229 SetMotor1(motorValue1);
1856413 0:550f6e86da32 230 }
1856413 1:2219a519e2bf 231
1856413 0:550f6e86da32 232 void MeasureAndControl2(void)
1856413 0:550f6e86da32 233 {
1856413 0:550f6e86da32 234 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 235 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 236 // a simple Feedback controller. Call this from a Ticker.
1856413 0:550f6e86da32 237 potMeterPosition2 = q2;
1856413 0:550f6e86da32 238 currentPosition2 = MeasureEncoderPosition2();
1856413 0:550f6e86da32 239 motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
1856413 0:550f6e86da32 240 SetMotor2(motorValue2);
1856413 1:2219a519e2bf 241 }*/
1856413 0:550f6e86da32 242
1856413 1:2219a519e2bf 243 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 244 // MAIN
1856413 1:2219a519e2bf 245 int main()
1856413 1:2219a519e2bf 246 {
1856413 0:550f6e86da32 247 pc.baud(115200);
1856413 1:2219a519e2bf 248
1856413 1:2219a519e2bf 249 while (true) {
1856413 1:2219a519e2bf 250 currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
1856413 1:2219a519e2bf 251 currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
1856413 1:2219a519e2bf 252 J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
1856413 1:2219a519e2bf 253 V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
1856413 1:2219a519e2bf 254 Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
1856413 1:2219a519e2bf 255 q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector
1856413 1:2219a519e2bf 256 q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector
1856413 1:2219a519e2bf 257 if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
1856413 1:2219a519e2bf 258 q1 = q1;
1856413 1:2219a519e2bf 259 } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
1856413 1:2219a519e2bf 260 q1 = 1.047;
1856413 1:2219a519e2bf 261 }
1856413 1:2219a519e2bf 262 if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
1856413 1:2219a519e2bf 263 q2 = 0.61; // Stay at mimimum angle
1856413 1:2219a519e2bf 264 } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
1856413 1:2219a519e2bf 265 q2 = 1.57; // Stay at maximum angle
1856413 1:2219a519e2bf 266 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 1:2219a519e2bf 267 q2 = q2;
1856413 1:2219a519e2bf 268 }
1856413 1:2219a519e2bf 269 // Determine error and add PID control
1856413 1:2219a519e2bf 270 double errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 1:2219a519e2bf 271 double errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
1856413 1:2219a519e2bf 272 // Determine motorValues
1856413 1:2219a519e2bf 273 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 1:2219a519e2bf 274 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 1:2219a519e2bf 275 // Make Motor move
1856413 1:2219a519e2bf 276 SetMotor1(motorValue1);
1856413 1:2219a519e2bf 277 SetMotor2(motorValue2);
1856413 0:550f6e86da32 278
1856413 1:2219a519e2bf 279 // Press button to switch velocity direction
1856413 1:2219a519e2bf 280 if (!button2.read()) {
1856413 1:2219a519e2bf 281 //SwitchPotmeterVelocity1();
1856413 1:2219a519e2bf 282 vy_des=vx_des;
1856413 1:2219a519e2bf 283 vx_des=0;
1856413 1:2219a519e2bf 284 } else {
1856413 1:2219a519e2bf 285 vy_des=0;
1856413 1:2219a519e2bf 286 }
1856413 1:2219a519e2bf 287 pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
1856413 1:2219a519e2bf 288 pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
1856413 1:2219a519e2bf 289 pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
1856413 1:2219a519e2bf 290 wait(0.01);
1856413 0:550f6e86da32 291 }
1856413 0:550f6e86da32 292 }