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@3:d16182dd3a2a, 2018-11-01 (annotated)

Committer:
1856413
Date:
Thu Nov 01 20:15:22 2018 +0000
Revision:
3:d16182dd3a2a
Parent:
2:638c6155d0af
Child:
4:854aa2e7eeb2
New pc.printf added

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 2:638c6155d0af 11 DigitalIn button2(SW3); //dit is nog button op mbed bor
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 2:638c6155d0af 20 Ticker potmeterTicker;
1856413 0:550f6e86da32 21
1856413 0:550f6e86da32 22 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 23 //Global Variables
1856413 1:2219a519e2bf 24
1856413 1:2219a519e2bf 25 double currentPosition1 = 0.0; // Starting position of motor 1 [rad]
1856413 1:2219a519e2bf 26 double currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad]
1856413 1:2219a519e2bf 27 int a = 0;
1856413 1:2219a519e2bf 28
1856413 0:550f6e86da32 29 //Inverse Kinematics
1856413 0:550f6e86da32 30 const double L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!!
1856413 0:550f6e86da32 31 const double L1 = 0.3; // Length link 1 [m] NAMETEN!!
1856413 0:550f6e86da32 32 const double L2 = 0.3; // Length link 2 [m] NAMETEN!!
1856413 0:550f6e86da32 33 const double L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN!
1856413 0:550f6e86da32 34 volatile double q1 = 0.0; // Starting reference angle of first link [rad]
1856413 0:550f6e86da32 35 volatile double q2 = -(0.5*3.14); // Starting reference angle of second link wrt first link [rad]
1856413 0:550f6e86da32 36 double t = 2.0; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!!
1856413 1:2219a519e2bf 37 volatile double vx_des = 0.0; // Starting velocity in x-direction [rad/s]
1856413 1:2219a519e2bf 38 volatile double vy_des = 0.0; // Starting velocity in y-direction [rad/s]
1856413 0:550f6e86da32 39 Matrix Q_set(2,1); // Setting a matrix for storing the angular velocities [rad/sec]
1856413 0:550f6e86da32 40 Matrix J(2,2); // Setting a matrix for the Jacobian
1856413 0:550f6e86da32 41 Matrix V(2,1); // Setting a matrix for storing the EMG-measured velocities
1856413 1:2219a519e2bf 42
1856413 0:550f6e86da32 43 // Motor Control
1856413 0:550f6e86da32 44 volatile double potMeterPosition1 = 0.0;
1856413 2:638c6155d0af 45 //volatile double potMeterPosition2 = 0.0;
1856413 0:550f6e86da32 46 volatile double motorValue1 = 0.01;
1856413 0:550f6e86da32 47 volatile double motorValue2 = 0.01;
1856413 0:550f6e86da32 48 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
1856413 0:550f6e86da32 49 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
1856413 0:550f6e86da32 50 volatile double Kd = 0.0;
1856413 0:550f6e86da32 51 volatile double Ts = 0.01; // SAMPLING TIJD!!
1856413 1:2219a519e2bf 52
1856413 0:550f6e86da32 53
1856413 0:550f6e86da32 54 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 55 // Potmeter values TO DETERMINE VX_DES, VY_DES
1856413 2:638c6155d0af 56 void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION
1856413 0:550f6e86da32 57 {
1856413 0:550f6e86da32 58 double potMeter1In = potMeter1.read();
1856413 2:638c6155d0af 59 //vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions
1856413 2:638c6155d0af 60 vx_des = 3.7;
1856413 2:638c6155d0af 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 2:638c6155d0af 226 MeterPosition1 = 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 2:638c6155d0af 248 potmeterTicker.attach(GetPotMeterVelocity1, 0.01);
1856413 1:2219a519e2bf 249
1856413 1:2219a519e2bf 250 while (true) {
1856413 1:2219a519e2bf 251 currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
1856413 1:2219a519e2bf 252 currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
1856413 1:2219a519e2bf 253 J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
1856413 1:2219a519e2bf 254 V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
1856413 1:2219a519e2bf 255 Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
1856413 1:2219a519e2bf 256 q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector
1856413 1:2219a519e2bf 257 q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector
1856413 1:2219a519e2bf 258 if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
1856413 1:2219a519e2bf 259 q1 = q1;
1856413 1:2219a519e2bf 260 } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
1856413 1:2219a519e2bf 261 q1 = 1.047;
1856413 1:2219a519e2bf 262 }
1856413 1:2219a519e2bf 263 if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
1856413 1:2219a519e2bf 264 q2 = 0.61; // Stay at mimimum angle
1856413 1:2219a519e2bf 265 } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
1856413 1:2219a519e2bf 266 q2 = 1.57; // Stay at maximum angle
1856413 1:2219a519e2bf 267 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 1:2219a519e2bf 268 q2 = q2;
1856413 1:2219a519e2bf 269 }
1856413 1:2219a519e2bf 270 // Determine error and add PID control
1856413 1:2219a519e2bf 271 double errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 1:2219a519e2bf 272 double errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
1856413 1:2219a519e2bf 273 // Determine motorValues
1856413 1:2219a519e2bf 274 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 1:2219a519e2bf 275 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 1:2219a519e2bf 276 // Make Motor move
1856413 1:2219a519e2bf 277 SetMotor1(motorValue1);
1856413 1:2219a519e2bf 278 SetMotor2(motorValue2);
1856413 0:550f6e86da32 279
1856413 1:2219a519e2bf 280 // Press button to switch velocity direction
1856413 2:638c6155d0af 281 //if (!button2.read()) {
1856413 2:638c6155d0af 282 if (!button2.read()) { //button2 blijven indrukken
1856413 1:2219a519e2bf 283 //SwitchPotmeterVelocity1();
1856413 1:2219a519e2bf 284 vy_des=vx_des;
1856413 1:2219a519e2bf 285 vx_des=0;
1856413 1:2219a519e2bf 286 } else {
1856413 1:2219a519e2bf 287 vy_des=0;
1856413 1:2219a519e2bf 288 }
1856413 1:2219a519e2bf 289 pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
1856413 1:2219a519e2bf 290 pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
1856413 1:2219a519e2bf 291 pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
1856413 3:d16182dd3a2a 292 pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q_set(1,1), Q_set(2,1));
1856413 1:2219a519e2bf 293 wait(0.01);
1856413 0:550f6e86da32 294 }
1856413 0:550f6e86da32 295 }