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@9:7443c37f0f7b, 2018-11-06 (annotated)

Committer:
1856413
Date:
Tue Nov 06 20:44:11 2018 +0000
Revision:
9:7443c37f0f7b
Parent:
8:3d2228402c71 
Final final

;

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 8:3d2228402c71 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 2:638c6155d0af 20 Ticker potmeterTicker;
1856413 8:3d2228402c71 21 Ticker measurecontrolTicker;
1856413 4:854aa2e7eeb2 22 Ticker printTicker;
1856413 0:550f6e86da32 23 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 24 //Global Variables
1856413 4:854aa2e7eeb2 25
1856413 8:3d2228402c71 26 volatile float currentPosition1 = 0.1; // Starting position of motor 1 [rad]
1856413 8:3d2228402c71 27 volatile float currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad]
1856413 1:2219a519e2bf 28
1856413 0:550f6e86da32 29 //Inverse Kinematics
1856413 9:7443c37f0f7b 30 const float L0 = 0.1; // Horizontal length from base to first joint [m]
1856413 9:7443c37f0f7b 31 const float L1 = 0.3; // Length link 1 [m]
1856413 9:7443c37f0f7b 32 const float L2 = 0.3; // Length link 2 [m]
1856413 9:7443c37f0f7b 33 const float L3 = 0.1; // Vertical length from base to first joint [m]
nicollevanrijswijk 5:aca2af310419 34 volatile float q1 = 0.1; // Starting reference angle of first link [rad]
nicollevanrijswijk 5:aca2af310419 35 volatile float q2 = -(0.5*3.12); // Starting reference angle of second link wrt first link [rad]
1856413 8:3d2228402c71 36
1856413 8:3d2228402c71 37 float t = 0.02; // Time interval [s]
nicollevanrijswijk 5:aca2af310419 38 volatile float vx_des = 0.0; // Starting velocity in x-direction [rad/s]
nicollevanrijswijk 5:aca2af310419 39 volatile float vy_des = 0.0; // Starting velocity in y-direction [rad/s]
nicollevanrijswijk 5:aca2af310419 40 volatile float errorIK1 = 0.0;
nicollevanrijswijk 5:aca2af310419 41 volatile float errorIK2 = 0.0;
nicollevanrijswijk 5:aca2af310419 42 volatile float J1 = 0.0;
nicollevanrijswijk 5:aca2af310419 43 volatile float J2 = 0.0;
nicollevanrijswijk 5:aca2af310419 44 volatile float J3 = 0.0;
nicollevanrijswijk 5:aca2af310419 45 volatile float J4 = 0.0;
nicollevanrijswijk 5:aca2af310419 46 volatile float V1 = 0.0;
nicollevanrijswijk 5:aca2af310419 47 volatile float V2 = 0.0;
nicollevanrijswijk 5:aca2af310419 48 volatile float Q1 = 0.0;
nicollevanrijswijk 5:aca2af310419 49 volatile float Q2 = 0.0;
1856413 1:2219a519e2bf 50
1856413 0:550f6e86da32 51 // Motor Control
nicollevanrijswijk 5:aca2af310419 52 volatile float potMeterPosition1 = 0.0;
nicollevanrijswijk 5:aca2af310419 53 int counts1i = 0;
nicollevanrijswijk 5:aca2af310419 54 int counts2i = 0;
nicollevanrijswijk 5:aca2af310419 55 //volatile float potMeterPosition2 = 0.0;
nicollevanrijswijk 5:aca2af310419 56 volatile float motorValue1 = 0.01;
nicollevanrijswijk 5:aca2af310419 57 volatile float motorValue2 = 0.01;
1856413 8:3d2228402c71 58 volatile float Kp = 0.34;
1856413 8:3d2228402c71 59 volatile float Ki = 0.0;
nicollevanrijswijk 5:aca2af310419 60 volatile float Kd = 0.0;
1856413 9:7443c37f0f7b 61 volatile float Ts = 0.02;
1856413 1:2219a519e2bf 62
1856413 0:550f6e86da32 63
1856413 0:550f6e86da32 64 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 65 // Potmeter values TO DETERMINE VX_DES, VY_DES
1856413 2:638c6155d0af 66 void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION
1856413 0:550f6e86da32 67 {
nicollevanrijswijk 5:aca2af310419 68 float potMeter1In = potMeter1.read();
1856413 8:3d2228402c71 69 vx_des = 4.0*3.14*potMeter1In - 2.0*3.14;
1856413 1:2219a519e2bf 70 }
1856413 1:2219a519e2bf 71
nicollevanrijswijk 5:aca2af310419 72 float SwitchPotmeterVelocity1() //Button has been pressed,
1856413 4:854aa2e7eeb2 73 {
1856413 1:2219a519e2bf 74 vy_des = vx_des;
1856413 1:2219a519e2bf 75 return vy_des;
1856413 1:2219a519e2bf 76 }
1856413 1:2219a519e2bf 77
1856413 1:2219a519e2bf 78 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 79 // KINEMATIC FUNCTIONS
1856413 1:2219a519e2bf 80
nicollevanrijswijk 5:aca2af310419 81 void ComputeJ(void)
1856413 1:2219a519e2bf 82 {
nicollevanrijswijk 7:fb3da4df4269 83 J1 = -sin(q1)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
nicollevanrijswijk 7:fb3da4df4269 84 J2 = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
nicollevanrijswijk 7:fb3da4df4269 85 J3 = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
nicollevanrijswijk 7:fb3da4df4269 86 J4 = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 87 }
1856413 1:2219a519e2bf 88
nicollevanrijswijk 5:aca2af310419 89 void ComputeV(void)
1856413 1:2219a519e2bf 90 {
nicollevanrijswijk 5:aca2af310419 91 V1 = vx_des;
nicollevanrijswijk 5:aca2af310419 92 V2 = vy_des;
1856413 1:2219a519e2bf 93 }
1856413 1:2219a519e2bf 94
nicollevanrijswijk 5:aca2af310419 95 float IntegrateQ1()
1856413 1:2219a519e2bf 96 {
nicollevanrijswijk 5:aca2af310419 97 q1 = q1 + (Q1*t); // new value for q1
1856413 1:2219a519e2bf 98 return q1;
1856413 1:2219a519e2bf 99 }
1856413 1:2219a519e2bf 100
nicollevanrijswijk 5:aca2af310419 101 float IntegrateQ2()
1856413 1:2219a519e2bf 102 {
nicollevanrijswijk 5:aca2af310419 103 q2 = q2 + (Q2*t); // new value for q2
1856413 1:2219a519e2bf 104 return q2;
1856413 1:2219a519e2bf 105 }
1856413 1:2219a519e2bf 106
nicollevanrijswijk 5:aca2af310419 107 void ComputeQ_set()
1856413 1:2219a519e2bf 108 {
nicollevanrijswijk 5:aca2af310419 109 Q1 = (J1*V1) + (J2*V2);
nicollevanrijswijk 5:aca2af310419 110 Q2 = (J3*V1) + (J4*V2);
1856413 1:2219a519e2bf 111 }
1856413 1:2219a519e2bf 112
nicollevanrijswijk 5:aca2af310419 113 float ErrorInverseKinematics1()
1856413 1:2219a519e2bf 114 {
nicollevanrijswijk 5:aca2af310419 115 float errorIK1 = q1 - currentPosition1;
1856413 1:2219a519e2bf 116 return errorIK1;
1856413 0:550f6e86da32 117 }
1856413 4:854aa2e7eeb2 118
nicollevanrijswijk 5:aca2af310419 119 float ErrorInverseKinematics2()
1856413 0:550f6e86da32 120 {
nicollevanrijswijk 5:aca2af310419 121 float errorIK2 = q2 - currentPosition2;
1856413 1:2219a519e2bf 122 return errorIK2;
1856413 1:2219a519e2bf 123 }
1856413 1:2219a519e2bf 124 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 125 // MOTOR PART
1856413 8:3d2228402c71 126 // Failure function
1856413 8:3d2228402c71 127 void TurnMotorsOff() // Turn motors off immediately
1856413 8:3d2228402c71 128 {
1856413 8:3d2228402c71 129 motor1PWM = 0;
1856413 8:3d2228402c71 130 motor2PWM = 0;
1856413 8:3d2228402c71 131 }
1856413 8:3d2228402c71 132
1856413 1:2219a519e2bf 133 //Encoder Posities
nicollevanrijswijk 5:aca2af310419 134 void MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad]
1856413 1:2219a519e2bf 135 {
nicollevanrijswijk 5:aca2af310419 136 counts1i = Encoder2.getPulses();
nicollevanrijswijk 5:aca2af310419 137 float counts1 = counts1i*1.0f;
nicollevanrijswijk 5:aca2af310419 138 currentPosition1 = (counts1/8400)*6.28; //Rotational position in radians
1856413 0:550f6e86da32 139 }
1856413 0:550f6e86da32 140
nicollevanrijswijk 5:aca2af310419 141 void MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad]
1856413 1:2219a519e2bf 142 {
nicollevanrijswijk 5:aca2af310419 143 counts2i = Encoder2.getPulses();
nicollevanrijswijk 5:aca2af310419 144 float counts2 = counts2i*1.0f;
nicollevanrijswijk 5:aca2af310419 145 currentPosition2 = (counts2/8400)*6.28; //Rotational position in radians
1856413 1:2219a519e2bf 146 }
1856413 1:2219a519e2bf 147
nicollevanrijswijk 5:aca2af310419 148 float FeedbackControl1(float Error1)
1856413 0:550f6e86da32 149 {
nicollevanrijswijk 5:aca2af310419 150 static float Error_integral1 = 0;
nicollevanrijswijk 5:aca2af310419 151 static float Error_prev1 = Error1;
1856413 0:550f6e86da32 152 // Proportional part:
nicollevanrijswijk 5:aca2af310419 153 float u_k1 = Kp * Error1;
1856413 0:550f6e86da32 154 // Integral part:
1856413 0:550f6e86da32 155 Error_integral1 = Error_integral1 + Error1 * Ts;
nicollevanrijswijk 5:aca2af310419 156 float u_i1 = Ki * Error_integral1;
1856413 1:2219a519e2bf 157 // Derivative part:
nicollevanrijswijk 5:aca2af310419 158 float Error_derivative1 = (Error1 - Error_prev1)/Ts;
nicollevanrijswijk 5:aca2af310419 159 float u_d1 = Kd * Error_derivative1;
1856413 0:550f6e86da32 160 Error_prev1 = Error1;
1856413 0:550f6e86da32 161 // Sum all parts and return it
1856413 0:550f6e86da32 162 return u_k1 + u_i1 + u_d1; //motorValue
1856413 0:550f6e86da32 163 }
1856413 1:2219a519e2bf 164
nicollevanrijswijk 5:aca2af310419 165 float FeedbackControl2(float Error2)
1856413 0:550f6e86da32 166 {
nicollevanrijswijk 5:aca2af310419 167 static float Error_integral2 = 0;
nicollevanrijswijk 5:aca2af310419 168 static float Error_prev2 = Error2;
1856413 0:550f6e86da32 169 // Proportional part:
nicollevanrijswijk 5:aca2af310419 170 float u_k2 = Kp * Error2;
1856413 0:550f6e86da32 171 // Integral part:
1856413 0:550f6e86da32 172 Error_integral2 = Error_integral2 + Error2 * Ts;
nicollevanrijswijk 5:aca2af310419 173 float u_i2 = Ki * Error_integral2;
1856413 1:2219a519e2bf 174 // Derivative part:
nicollevanrijswijk 5:aca2af310419 175 float Error_derivative2 = (Error2 - Error_prev2)/Ts;
nicollevanrijswijk 5:aca2af310419 176 float u_d2 = Kd * Error_derivative2;
1856413 0:550f6e86da32 177 Error_prev2 = Error2;
1856413 0:550f6e86da32 178 // Sum all parts and return it
1856413 0:550f6e86da32 179 return u_k2 + u_i2 + u_d2; //motorValue
1856413 0:550f6e86da32 180 }
1856413 1:2219a519e2bf 181
nicollevanrijswijk 5:aca2af310419 182 void SetMotor1(float motorValue1)
1856413 0:550f6e86da32 183 {
1856413 0:550f6e86da32 184 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 185 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 186 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 187 // within range
1856413 0:550f6e86da32 188 // 0 = clockwise motor direction
1856413 0:550f6e86da32 189 // 1 = counterclockwise motor direction
1856413 0:550f6e86da32 190 if (motorValue1 >=0) {
1856413 0:550f6e86da32 191 motor1Direction=0;
1856413 0:550f6e86da32 192 } else {
1856413 0:550f6e86da32 193 motor1Direction=1;
1856413 0:550f6e86da32 194 }
1856413 0:550f6e86da32 195 if (fabs(motorValue1)>1) {
1856413 0:550f6e86da32 196 motor1PWM = 1;
1856413 0:550f6e86da32 197 } else {
1856413 0:550f6e86da32 198 motor1PWM = fabs(motorValue1);
1856413 0:550f6e86da32 199 }
1856413 0:550f6e86da32 200 }
1856413 1:2219a519e2bf 201
nicollevanrijswijk 5:aca2af310419 202 void SetMotor2(float motorValue2)
1856413 0:550f6e86da32 203 {
1856413 0:550f6e86da32 204 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 205 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 206 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 207 // within range
1856413 0:550f6e86da32 208 // 0 = counterclockwise motor direction
1856413 0:550f6e86da32 209 // 1 = clockwise motor direction
1856413 0:550f6e86da32 210 if (motorValue2 >=0) {
1856413 0:550f6e86da32 211 motor2Direction=1;
1856413 0:550f6e86da32 212 } else {
1856413 0:550f6e86da32 213 motor2Direction=0;
1856413 0:550f6e86da32 214 }
1856413 0:550f6e86da32 215 if (fabs(motorValue2)>1) {
1856413 0:550f6e86da32 216 motor2PWM = 1;
1856413 0:550f6e86da32 217 } else {
1856413 0:550f6e86da32 218 motor2PWM = fabs(motorValue2);
1856413 0:550f6e86da32 219 }
1856413 0:550f6e86da32 220 }
1856413 1:2219a519e2bf 221
1856413 9:7443c37f0f7b 222 void MeasureAndInverseK()
1856413 4:854aa2e7eeb2 223 {
nicollevanrijswijk 5:aca2af310419 224 MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
nicollevanrijswijk 5:aca2af310419 225 MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
nicollevanrijswijk 5:aca2af310419 226 ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
nicollevanrijswijk 5:aca2af310419 227 ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
nicollevanrijswijk 5:aca2af310419 228 ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
1856413 4:854aa2e7eeb2 229 q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector
1856413 4:854aa2e7eeb2 230 q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector
1856413 9:7443c37f0f7b 231 if (q > 0.0) { // q1 cannot be greater than 0
1856413 9:7443c37f0f7b 232 q = 0.0;
1856413 9:7443c37f0f7b 233 } else if (q < - 1.047) { // q1 cannot be smaller than 60 degrees
1856413 9:7443c37f0f7b 234 q1 = -1.047;
1856413 9:7443c37f0f7b 235 } else { // q1 can be computed q1 between intervals
1856413 9:7443c37f0f7b 236 q1 = q1;
1856413 9:7443c37f0f7b 237 }
1856413 9:7443c37f0f7b 238 if (q2 > -0.61) { // q2 cannot be smaller than 0.61 rad
1856413 9:7443c37f0f7b 239 q2 = -0.61; // Stay at mimimum angle
1856413 9:7443c37f0f7b 240 } else if (q2 < -1.57) { // q2 cannot be greater than 1.57 rad
1856413 9:7443c37f0f7b 241 q2 = -1.57; // Stay at maximum angle
1856413 9:7443c37f0f7b 242 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 9:7443c37f0f7b 243 q2 = q2;
1856413 9:7443c37f0f7b 244 // Determine error and add PID control
1856413 9:7443c37f0f7b 245 errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 9:7443c37f0f7b 246 errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
1856413 9:7443c37f0f7b 247 // Determine motorValues
1856413 9:7443c37f0f7b 248 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 9:7443c37f0f7b 249 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 9:7443c37f0f7b 250 if (errorIK1<0.1) { // When error is small enough, stop turning
1856413 9:7443c37f0f7b 251 if (errorIK2<0.1) {
1856413 9:7443c37f0f7b 252 TurnMotorsOff();
1856413 9:7443c37f0f7b 253 } else {
1856413 9:7443c37f0f7b 254 motor1PWM = 0.0;
1856413 9:7443c37f0f7b 255 SetMotor2();
1856413 8:3d2228402c71 256 }
1856413 9:7443c37f0f7b 257 } else if (errorIK2<0.1) {
1856413 9:7443c37f0f7b 258 if (errorIK1<0.1) {
1856413 9:7443c37f0f7b 259 TurnMotorsOff();
1856413 9:7443c37f0f7b 260 } else {
1856413 9:7443c37f0f7b 261 motor2PWM = 0.0;
1856413 9:7443c37f0f7b 262 SetMotor1();
1856413 8:3d2228402c71 263 }
1856413 9:7443c37f0f7b 264 }
1856413 8:3d2228402c71 265
nicollevanrijswijk 5:aca2af310419 266 // Make Motor move
1856413 9:7443c37f0f7b 267 SetMotor1(motorValue1);
1856413 9:7443c37f0f7b 268 SetMotor2(motorValue2);
1856413 0:550f6e86da32 269
nicollevanrijswijk 5:aca2af310419 270 // Press button to switch velocity direction
1856413 9:7443c37f0f7b 271 if (!button2.read()) { //keep pressing button2
1856413 9:7443c37f0f7b 272 vy_des=vx_des;
1856413 9:7443c37f0f7b 273 vx_des=0.0;
1856413 9:7443c37f0f7b 274 } else {
1856413 9:7443c37f0f7b 275 vy_des=0.0;
1856413 9:7443c37f0f7b 276 }
1856413 4:854aa2e7eeb2 277 }
1856413 4:854aa2e7eeb2 278
1856413 9:7443c37f0f7b 279 /*void PrintValues()
1856413 9:7443c37f0f7b 280 {
1856413 9:7443c37f0f7b 281 pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
1856413 9:7443c37f0f7b 282 pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
1856413 9:7443c37f0f7b 283 pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
1856413 9:7443c37f0f7b 284 pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2);
1856413 9:7443c37f0f7b 285 }*/
1856413 1:2219a519e2bf 286 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 287 // MAIN
1856413 9:7443c37f0f7b 288 int main() {
1856413 9:7443c37f0f7b 289 pc.baud(115200);
1856413 9:7443c37f0f7b 290 potmeterTicker.attach(GetPotMeterVelocity1, 0.02);
1856413 9:7443c37f0f7b 291 measurecontrolTicker.attach(MeasureAndInverseK, 0.01);
1856413 9:7443c37f0f7b 292 /*printTicker.attach(printen, 0.5);*/
1856413 4:854aa2e7eeb2 293
1856413 9:7443c37f0f7b 294 while (true) {
1856413 9:7443c37f0f7b 295 Led = !Led;
1856413 9:7443c37f0f7b 296 wait(0.5);
1856413 9:7443c37f0f7b 297 }
1856413 9:7443c37f0f7b 298 }