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@8:3d2228402c71, 2018-11-06 (annotated)

Committer:
1856413
Date:
Tue Nov 06 20:25:40 2018 +0000
Revision:
8:3d2228402c71
Parent:
7:fb3da4df4269
Child:
9:7443c37f0f7b
Final demo script

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 8:3d2228402c71 30 const float L0 = 0.1; // Horizontal length from base to first joint [m]
1856413 8:3d2228402c71 31 const float L1 = 0.3; // Length link 1 [m]
1856413 8:3d2228402c71 32 const float L2 = 0.3; // Length link 2 [m]
1856413 8:3d2228402c71 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 8:3d2228402c71 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 8:3d2228402c71 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 8:3d2228402c71 231 if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
1856413 8:3d2228402c71 232 q1 = q1;
1856413 8:3d2228402c71 233 } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
1856413 8:3d2228402c71 234 q1 = 1.047;
1856413 8:3d2228402c71 235 }
1856413 8:3d2228402c71 236 if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
1856413 8:3d2228402c71 237 q2 = 0.61; // Stay at mimimum angle
1856413 8:3d2228402c71 238 } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
1856413 8:3d2228402c71 239 q2 = 1.57; // Stay at maximum angle
1856413 8:3d2228402c71 240 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 8:3d2228402c71 241 q2 = q2;
1856413 8:3d2228402c71 242 }
1856413 8:3d2228402c71 243
nicollevanrijswijk 5:aca2af310419 244 // Determine error and add PID control
1856413 4:854aa2e7eeb2 245 errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 4:854aa2e7eeb2 246 errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
nicollevanrijswijk 5:aca2af310419 247 // Determine motorValues
1856413 4:854aa2e7eeb2 248 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 4:854aa2e7eeb2 249 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 8:3d2228402c71 250 if (errorIK1<0.5);
1856413 8:3d2228402c71 251 {
1856413 8:3d2228402c71 252 TurnMotorsOff();
1856413 8:3d2228402c71 253 }
nicollevanrijswijk 5:aca2af310419 254 // Make Motor move
1856413 4:854aa2e7eeb2 255 SetMotor1(motorValue1);
1856413 4:854aa2e7eeb2 256 SetMotor2(motorValue2);
1856413 0:550f6e86da32 257
nicollevanrijswijk 5:aca2af310419 258 // Press button to switch velocity direction
1856413 8:3d2228402c71 259 if (!button2.read()) { //keep pressing button2
1856413 4:854aa2e7eeb2 260 vy_des=vx_des;
nicollevanrijswijk 5:aca2af310419 261 vx_des=0.0;
1856413 4:854aa2e7eeb2 262 } else {
nicollevanrijswijk 5:aca2af310419 263 vy_des=0.0;
1856413 4:854aa2e7eeb2 264 }
1856413 4:854aa2e7eeb2 265 }
1856413 4:854aa2e7eeb2 266
1856413 8:3d2228402c71 267 /*void PrintValues()
1856413 4:854aa2e7eeb2 268 {
1856413 4:854aa2e7eeb2 269 pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
1856413 4:854aa2e7eeb2 270 pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
1856413 4:854aa2e7eeb2 271 pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
nicollevanrijswijk 5:aca2af310419 272 pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2);
1856413 8:3d2228402c71 273 }*/
1856413 1:2219a519e2bf 274 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 275 // MAIN
1856413 1:2219a519e2bf 276 int main()
1856413 1:2219a519e2bf 277 {
1856413 0:550f6e86da32 278 pc.baud(115200);
1856413 8:3d2228402c71 279 potmeterTicker.attach(GetPotMeterVelocity1, 0.02);
1856413 8:3d2228402c71 280 measurecontrolTicker.attach(MeasureAndInverseK, 0.01);
1856413 8:3d2228402c71 281 /*printTicker.attach(printen, 0.5);*/
1856413 4:854aa2e7eeb2 282
1856413 1:2219a519e2bf 283 while (true) {
nicollevanrijswijk 5:aca2af310419 284 Led = !Led;
nicollevanrijswijk 5:aca2af310419 285 wait(0.5);
1856413 0:550f6e86da32 286 }
1856413 0:550f6e86da32 287 }