Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@14:7a95e57b5364, 2017-10-30 (annotated)

Committer:
EvB
Date:
Mon Oct 30 16:35:20 2017 +0000
Revision:
14:7a95e57b5364
Parent:
13:acd120520e80
Child:
15:207d01972e0b
Everything put together, but isn't working.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MMartens 0:9167ae5d9927 1 #include "mbed.h"
MMartens 0:9167ae5d9927 2 #include "MODSERIAL.h"
MMartens 0:9167ae5d9927 3 #include "encoder.h"
MMartens 0:9167ae5d9927 4 #include "math.h"
MMartens 3:e888f52a46bc 5 #include "HIDScope.h"
vera1 8:9edf32e021a5 6 #include "BiQuad.h"
vera1 8:9edf32e021a5 7 #include "EMG_filter.h"
EvB 13:acd120520e80 8 #include "Matrix.h"
EvB 13:acd120520e80 9 #include "MatrixMath.h"
vera1 8:9edf32e021a5 10
EvB 13:acd120520e80 11 const double pi = 3.14159265359; // Defining the constant pi
vera1 11:bda77916d2ea 12
EvB 13:acd120520e80 13 MODSERIAL pc(USBTX,USBRX); // Enabling communication with the pc.
EvB 13:acd120520e80 14
EvB 13:acd120520e80 15 enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE}; // Defining states of the robot
vera1 11:bda77916d2ea 16 r_states state;
vera1 11:bda77916d2ea 17
vera1 8:9edf32e021a5 18 // ---- EMG parameters ----
EvB 13:acd120520e80 19 //HIDScope scope (2);
vera1 8:9edf32e021a5 20 EMG_filter emg1(A0);
vera1 11:bda77916d2ea 21 EMG_filter emg2(A1);
EvB 13:acd120520e80 22 float EMG_threshold = 0.2; // Threshold for the EMG signal
EvB 13:acd120520e80 23 // ------------------------
vera1 8:9edf32e021a5 24
vera1 8:9edf32e021a5 25 // ---- Motor input and outputs ----
MMartens 0:9167ae5d9927 26 PwmOut speed1(D5);
MMartens 4:75f6e4845194 27 PwmOut speed2(D6);
vera1 12:5be2001abe62 28 PwmOut speedservo1(D11);
vera1 12:5be2001abe62 29 //PwmOut speedservo2(D8);
MMartens 0:9167ae5d9927 30 DigitalOut dir1(D4);
MMartens 4:75f6e4845194 31 DigitalOut dir2(D7);
vera1 6:fc46581fe3e0 32 DigitalIn press(PTA4);
vera1 11:bda77916d2ea 33 DigitalOut ledred(LED_RED);
vera1 11:bda77916d2ea 34 DigitalOut ledgreen(LED_GREEN);
vera1 11:bda77916d2ea 35 DigitalOut ledblue(LED_BLUE);
vera1 12:5be2001abe62 36 DigitalOut ledstateswitch(D3);
vera1 12:5be2001abe62 37 DigitalOut ledstatedef(D2);
MMartens 4:75f6e4845194 38 AnalogIn pot(A2);
vera1 11:bda77916d2ea 39 AnalogIn pot2(A3);
MMartens 0:9167ae5d9927 40 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 41 Encoder motor2(D12,D13);
vera1 8:9edf32e021a5 42 // ----------------------------------
vera1 8:9edf32e021a5 43
vera1 8:9edf32e021a5 44 // --- Define Ticker and Timeout ---
EvB 14:7a95e57b5364 45 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_calibration
vera1 8:9edf32e021a5 46 Timeout calibrationgo; // Timeout that will determine the duration of the calibration.
vera1 8:9edf32e021a5 47 // ---------------------------------
vera1 8:9edf32e021a5 48
MMartens 0:9167ae5d9927 49
MMartens 1:f3fe6d2b7639 50 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 51
EvB 13:acd120520e80 52 volatile double Pos1; // Encoder readout of motor 1 [counts]
EvB 13:acd120520e80 53 volatile double Pos2; // Encoder readout of motor 2 [counts]
EvB 14:7a95e57b5364 54 volatile int count = 0; // Loop counts
MMartens 0:9167ae5d9927 55
vera1 12:5be2001abe62 56 double Kp = 250;// you can set these constants however you like depending on trial & error
MMartens 3:e888f52a46bc 57 double Ki = 100;
MMartens 2:7c6391c8ca71 58 double Kd = 0;
MMartens 0:9167ae5d9927 59
MMartens 0:9167ae5d9927 60 float last_error = 0;
MMartens 1:f3fe6d2b7639 61 float error1 = 0;
MMartens 0:9167ae5d9927 62 float changeError = 0;
MMartens 0:9167ae5d9927 63 float totalError = 0;
MMartens 0:9167ae5d9927 64 float pidTerm = 0;
MMartens 4:75f6e4845194 65 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 66
MMartens 4:75f6e4845194 67 float last_error2 = 0;
MMartens 4:75f6e4845194 68 float error2 = 0;
MMartens 4:75f6e4845194 69 float changeError2 = 0;
MMartens 4:75f6e4845194 70 float totalError2 = 0;
MMartens 4:75f6e4845194 71 float pidTerm2 = 0;
MMartens 4:75f6e4845194 72 float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
MMartens 0:9167ae5d9927 73
MMartens 0:9167ae5d9927 74 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 75 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 76 volatile double position = 0.0;
MMartens 4:75f6e4845194 77 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 78
EvB 13:acd120520e80 79 // --- Kinematics parameter ---
EvB 13:acd120520e80 80 float L1 = 0.38; // length of link 1 [m]
EvB 13:acd120520e80 81 float L5 = 0.25; // length of link 5 [m]
EvB 13:acd120520e80 82 float d = 0.03; // diameter of the pulley [m]
EvB 13:acd120520e80 83
EvB 13:acd120520e80 84 volatile float xe; // x-coordinate of end-effector
EvB 13:acd120520e80 85 volatile float ye; // y-coordinate of end-effector
EvB 13:acd120520e80 86 volatile float ze; // z-coordinate of end-effector
EvB 13:acd120520e80 87
EvB 13:acd120520e80 88 volatile float Q1; // Position of joint 1 [m]
EvB 13:acd120520e80 89 volatile float Q2; // Position of joint 2 [rad]
EvB 13:acd120520e80 90 volatile float Q3; // Position of joint 3 [rad]
EvB 13:acd120520e80 91
EvB 13:acd120520e80 92 volatile float V1; // Desired velocity of joint 1 [m/s]; --> or normalize it?
EvB 13:acd120520e80 93 volatile float W2; // Desired velocity of joint 2 [rad/s];
EvB 13:acd120520e80 94 volatile float W3; // Desired velocity of joint 3 [rad/s];
EvB 13:acd120520e80 95
EvB 13:acd120520e80 96 volatile float Vx_des; // Desired velocity end-effector in x-direction [m/s]
EvB 13:acd120520e80 97 volatile float Vy_des; // Desired velocity end-effector in y-direction [m/s]
EvB 13:acd120520e80 98 volatile float Vz_des; // Desired velocity end-effector in z-direction [m/s]
EvB 13:acd120520e80 99
EvB 13:acd120520e80 100 const float T = 0.002; // Time interval of the readout of the EMG --> Check this with the ticker, etc!!!
EvB 13:acd120520e80 101
EvB 13:acd120520e80 102
vera1 8:9edf32e021a5 103 // --- Booleans for the maintickerfunction ---
EvB 13:acd120520e80 104 bool go_calibration; // Boolean to put on/off the calibration of the EMG
EvB 13:acd120520e80 105 bool go_switch; // Boolean to go to different state
EvB 13:acd120520e80 106 bool go_move; //Boolean to move the robot arm and base
EvB 13:acd120520e80 107 bool go_kinematics; // Boolean which will determine whether a new position will be calculated or not --> replace by go_move?
vera1 8:9edf32e021a5 108 // -------------------------------------------
vera1 8:9edf32e021a5 109
vera1 8:9edf32e021a5 110 // --- calibrate EMG signal ----
vera1 8:9edf32e021a5 111
vera1 8:9edf32e021a5 112 void calibrationGO() // Function for go or no go of calibration
vera1 8:9edf32e021a5 113 {
vera1 8:9edf32e021a5 114 go_calibration = false;
EvB 13:acd120520e80 115
vera1 8:9edf32e021a5 116 }
vera1 8:9edf32e021a5 117
vera1 8:9edf32e021a5 118 /*
vera1 8:9edf32e021a5 119 Calibration of the robot works as follows:
vera1 8:9edf32e021a5 120 EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
EvB 13:acd120520e80 121 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 8:9edf32e021a5 122 During the calibration the user should exert maximum force.
vera1 8:9edf32e021a5 123 */
vera1 8:9edf32e021a5 124 void calibrationEMG() // Function for calibrating the EMG signal
vera1 8:9edf32e021a5 125 {
EvB 13:acd120520e80 126 if (go_calibration) {
EvB 13:acd120520e80 127
EvB 13:acd120520e80 128 emg1.calibration(); // Using the calibration function of the EMG_filter class
EvB 13:acd120520e80 129 emg2.calibration();
vera1 8:9edf32e021a5 130 }
vera1 8:9edf32e021a5 131 }
vera1 8:9edf32e021a5 132
vera1 8:9edf32e021a5 133 // ------------------------------
EvB 13:acd120520e80 134 //Function that reads out the raw EMG and filters the signal
EvB 13:acd120520e80 135 void processEMG ()
EvB 13:acd120520e80 136 {
MMartens 3:e888f52a46bc 137
EvB 13:acd120520e80 138
EvB 13:acd120520e80 139 emg1.filter(); //filter the incoming emg signal
EvB 13:acd120520e80 140 emg2.filter();
EvB 13:acd120520e80 141 //emg3.filter();
EvB 14:7a95e57b5364 142 /*
EvB 14:7a95e57b5364 143 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
EvB 14:7a95e57b5364 144 scope.set(1, emg2.normalized);
EvB 14:7a95e57b5364 145 scope.send();
EvB 14:7a95e57b5364 146 */
EvB 13:acd120520e80 147 }
EvB 13:acd120520e80 148
EvB 13:acd120520e80 149 // --- Kinematics functions ---
EvB 13:acd120520e80 150
EvB 13:acd120520e80 151 void Brockett(float q1, float q2, float q3) // Determine position of end-effector using Brockett
vera1 11:bda77916d2ea 152 {
vera1 11:bda77916d2ea 153
EvB 13:acd120520e80 154 Matrix E100b(4,4); // Complete matrix form of 1st joint
EvB 13:acd120520e80 155 E100b = MatrixMath::Eye(4);
EvB 13:acd120520e80 156 E100b(3,4) = q1;
EvB 13:acd120520e80 157
EvB 13:acd120520e80 158
EvB 13:acd120520e80 159 Matrix E210b(4,4); // Complete matrix form of 2nd joint
EvB 13:acd120520e80 160 E210b = MatrixMath::RotZ(q2);
EvB 13:acd120520e80 161
EvB 13:acd120520e80 162
EvB 13:acd120520e80 163 Matrix E320b(4,4); // Complete matrix form of 3rd joint
EvB 13:acd120520e80 164 E320b = MatrixMath::RotZ(q3-q2);
EvB 13:acd120520e80 165 E320b(1,4) = L1 - L1*cos(q3-q2);
EvB 13:acd120520e80 166 E320b(2,4) = -L1*sin(q3-q2);
EvB 13:acd120520e80 167
EvB 13:acd120520e80 168
EvB 13:acd120520e80 169 Matrix HE0ref(4,4); // H-matrix of the robot in reference position
EvB 13:acd120520e80 170 HE0ref = MatrixMath::Eye(4);
EvB 13:acd120520e80 171 HE0ref(1,4) = L1;
EvB 13:acd120520e80 172 HE0ref(2,4) = -L5;
EvB 13:acd120520e80 173
EvB 13:acd120520e80 174
EvB 13:acd120520e80 175 Matrix HE0(4,4); // H-matrix to find the position of the end-effector, expressed in frame 0.
EvB 13:acd120520e80 176 HE0 = E100b * E210b * E320b * HE0ref; // This H-matrix is not really correct. Check with matlab!!!
EvB 13:acd120520e80 177
EvB 13:acd120520e80 178
EvB 13:acd120520e80 179 // Determine new position of end effector
EvB 13:acd120520e80 180 xe = HE0(1,4); // New x-coordinate of the end-effector
EvB 13:acd120520e80 181 ye = HE0(2,4); // New y-coordinate of the end-effector
EvB 13:acd120520e80 182 ze = HE0(3,4); // New z-coordinate of the end-effector
EvB 13:acd120520e80 183
vera1 11:bda77916d2ea 184
vera1 11:bda77916d2ea 185 }
vera1 11:bda77916d2ea 186
EvB 13:acd120520e80 187 void Jacobian(float vx_des, float vy_des, float vz_des) // Function to determine the velocities with desired velocity as input
vera1 11:bda77916d2ea 188 {
EvB 13:acd120520e80 189 //kinematics_go = false; // putting boolean to false
EvB 13:acd120520e80 190
EvB 13:acd120520e80 191 // Finding joint positions
EvB 13:acd120520e80 192 Q1 = Pos1; // Position of joint 1 [m]
EvB 13:acd120520e80 193 Q2 = Pos2; // Position of joint 2 [rad]
EvB 13:acd120520e80 194 Q3 = 0; // Position of joint 3 [rad] --> need this from Servo!
EvB 13:acd120520e80 195
EvB 13:acd120520e80 196 //pc.printf("\r\nq1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1, q2, q3);
EvB 13:acd120520e80 197
EvB 13:acd120520e80 198 Brockett(Q1,Q2,Q3); // Start with conducting Brockett to obtain the end-effector position
EvB 13:acd120520e80 199
EvB 13:acd120520e80 200 Matrix T_des(3,1); // Twist of desired linear velocities
EvB 13:acd120520e80 201 T_des(1,1) = vx_des;
EvB 13:acd120520e80 202 T_des(2,1) = vy_des;
EvB 13:acd120520e80 203 T_des(3,1) = vz_des;
EvB 13:acd120520e80 204
EvB 13:acd120520e80 205
EvB 13:acd120520e80 206 Matrix T100j(1,4); // Unit twist of the first frame in current configuration
EvB 13:acd120520e80 207 T100j(1,4) = 1;
EvB 13:acd120520e80 208
EvB 13:acd120520e80 209 Matrix T210j(1,4); // Unit twist of the second frame in current configuration
EvB 13:acd120520e80 210 T210j(1,1) = 1;
EvB 13:acd120520e80 211
EvB 13:acd120520e80 212 Matrix T320j(1,4); // Unit twist of the third frame in current configuration
EvB 13:acd120520e80 213 T320j(1,1) = 1;
EvB 13:acd120520e80 214 T320j(1,2) = -L1*sin(Q2);
EvB 13:acd120520e80 215 T320j(1,3) = -L1 * cos(Q2);
EvB 13:acd120520e80 216
EvB 13:acd120520e80 217 Matrix J(4,3); // Jacobian matrix
EvB 13:acd120520e80 218
EvB 13:acd120520e80 219 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 220 J(i,1) = T100j(1,i);
EvB 13:acd120520e80 221 }
EvB 13:acd120520e80 222
EvB 13:acd120520e80 223 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 224 J(i,2) = T210j(1,i);
EvB 13:acd120520e80 225 }
EvB 13:acd120520e80 226
EvB 13:acd120520e80 227 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 228 J(i,3) = T320j(1,i);
EvB 13:acd120520e80 229 }
EvB 13:acd120520e80 230
EvB 13:acd120520e80 231
EvB 13:acd120520e80 232 Matrix H0f(4,4); // H-matrix to transform Jacobian from frame 0 to frame f, which is attached to the end-effector, but not rotating with it.
EvB 13:acd120520e80 233 H0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 234 H0f(1,4) = -xe;
EvB 13:acd120520e80 235 H0f(2,4) = -ye;
EvB 13:acd120520e80 236 H0f(3,4) = -ze;
EvB 13:acd120520e80 237
EvB 13:acd120520e80 238 Matrix AdH0f(4,4); // Adjoint of H0f, necessary for twist transformations
EvB 13:acd120520e80 239 AdH0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 240 AdH0f(2,1) = H0f(2,4);
EvB 13:acd120520e80 241 AdH0f(3,1) = -H0f(1,4);
EvB 13:acd120520e80 242
EvB 13:acd120520e80 243 Matrix Jprime(4,3); // Transformed Jacobian
EvB 13:acd120520e80 244 Jprime = AdH0f*J;
EvB 13:acd120520e80 245
EvB 13:acd120520e80 246 Matrix Jprimeprime(3,3); // Truncated Jacobian
EvB 13:acd120520e80 247 for (int i = 1 ; i <4 ; i++) {
EvB 13:acd120520e80 248 for (int j = 1 ; j < 4 ; j++) {
EvB 13:acd120520e80 249 Jprimeprime(i,j) = Jprime(i+1,j);
EvB 13:acd120520e80 250 }
EvB 13:acd120520e80 251 }
EvB 13:acd120520e80 252
EvB 13:acd120520e80 253 Matrix JprimeprimeT(3,3); // The transpose of the truncated Jacobian
EvB 13:acd120520e80 254 JprimeprimeT = MatrixMath::Transpose(Jprimeprime);
EvB 13:acd120520e80 255
EvB 13:acd120520e80 256 Matrix PseudoInverse(3,3); // The Pseudo-inverse of the truncated Jacobian
EvB 13:acd120520e80 257 PseudoInverse = MatrixMath::Inv(JprimeprimeT*Jprimeprime)*JprimeprimeT;
EvB 13:acd120520e80 258
EvB 13:acd120520e80 259 Matrix Q_dot(1,3); // Desired joint velocities
EvB 13:acd120520e80 260 Q_dot = PseudoInverse * T_des;
EvB 13:acd120520e80 261
EvB 13:acd120520e80 262 V1 = Q_dot(1,1);
EvB 13:acd120520e80 263 W2 = Q_dot(2,1);
EvB 13:acd120520e80 264 W3 = Q_dot(3,1);
EvB 13:acd120520e80 265
EvB 13:acd120520e80 266 // pc.printf("\r\nv1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
EvB 13:acd120520e80 267
EvB 13:acd120520e80 268 // Eva - not sure if we need this... --> I think for the setpoint of the PID controller
EvB 13:acd120520e80 269 Q1 += V1*T; // Predicted value for q1 [m]
EvB 13:acd120520e80 270 Q2 += W2*T; // Predicted value for q2 [rad]
EvB 13:acd120520e80 271 Q3 += W3*T; // Predicted value for q3 [rad]
EvB 13:acd120520e80 272
EvB 14:7a95e57b5364 273 if (count == 100) {
EvB 14:7a95e57b5364 274 //pc.printf("\r\n Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
EvB 14:7a95e57b5364 275 }
vera1 11:bda77916d2ea 276
vera1 11:bda77916d2ea 277 }
vera1 11:bda77916d2ea 278
vera1 11:bda77916d2ea 279 //--- State switch function -----
vera1 11:bda77916d2ea 280 void r_processStateSwitch()
MMartens 5:8c6d66a7c5da 281 {
EvB 13:acd120520e80 282 go_switch = false;
EvB 13:acd120520e80 283 switch(state) {
EvB 13:acd120520e80 284 case R_HORIZONTAL:
EvB 13:acd120520e80 285 state = R_VERTICAL;
EvB 13:acd120520e80 286 ledblue = 1;
EvB 13:acd120520e80 287 ledred = 1;
EvB 13:acd120520e80 288 ledgreen = 0;
EvB 13:acd120520e80 289 break;
EvB 13:acd120520e80 290 case R_VERTICAL:
EvB 13:acd120520e80 291 state = R_BASE;
EvB 13:acd120520e80 292 ledgreen = 1;
EvB 13:acd120520e80 293 ledblue = 1;
EvB 13:acd120520e80 294 ledred = 0;
EvB 13:acd120520e80 295 break;
EvB 13:acd120520e80 296 case R_BASE:
EvB 13:acd120520e80 297 state = R_HORIZONTAL;
EvB 13:acd120520e80 298 ledgreen = 1;
EvB 13:acd120520e80 299 ledred = 1;
EvB 13:acd120520e80 300 ledblue = 0;
EvB 13:acd120520e80 301 break;
EvB 13:acd120520e80 302 }
EvB 13:acd120520e80 303 wait(1.0f); // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
EvB 13:acd120520e80 304 ledstatedef = 1;
EvB 13:acd120520e80 305 ledstateswitch = 0;
EvB 13:acd120520e80 306 go_move = true;
MMartens 5:8c6d66a7c5da 307 }
vera1 8:9edf32e021a5 308
EvB 13:acd120520e80 309 // --- Determine the setpoints of the joint velocities
EvB 14:7a95e57b5364 310 void setpointreadout(float v_des) // something goes wrong here
vera1 8:9edf32e021a5 311 {
EvB 13:acd120520e80 312 /*
vera1 11:bda77916d2ea 313 potvalue = pot.read();
vera1 11:bda77916d2ea 314 setpoint = emg1.normalized;
EvB 13:acd120520e80 315
vera1 11:bda77916d2ea 316 potvalue2 = pot2.read();
vera1 11:bda77916d2ea 317 setpoint2 = emg2.normalized;
EvB 13:acd120520e80 318 */
EvB 13:acd120520e80 319
EvB 13:acd120520e80 320 Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
EvB 13:acd120520e80 321 Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
EvB 13:acd120520e80 322
EvB 14:7a95e57b5364 323
EvB 14:7a95e57b5364 324 if(pot.read()>= EMG_threshold)
EvB 14:7a95e57b5364 325 {
EvB 14:7a95e57b5364 326 v_des = pot.read();
EvB 14:7a95e57b5364 327 pc.printf("potvalue is %f\r\n", v_des);
EvB 14:7a95e57b5364 328 }
EvB 14:7a95e57b5364 329 if (pot2.read() >= EMG_threshold)
EvB 14:7a95e57b5364 330 {
EvB 14:7a95e57b5364 331 v_des = -pot2.read();
EvB 14:7a95e57b5364 332 }
EvB 14:7a95e57b5364 333 /*
EvB 13:acd120520e80 334 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
EvB 13:acd120520e80 335 v_des = emg1.normalized;
EvB 13:acd120520e80 336 }
MMartens 2:7c6391c8ca71 337
EvB 13:acd120520e80 338 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Negative direction
EvB 13:acd120520e80 339 v_des = -emg2.normalized;
EvB 13:acd120520e80 340 }
EvB 14:7a95e57b5364 341 */
EvB 13:acd120520e80 342
EvB 13:acd120520e80 343 //pc.printf("\r\nv_des in setpoint function= %f\r\n", v_des);
EvB 13:acd120520e80 344 }
EvB 13:acd120520e80 345 void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 346 {
EvB 13:acd120520e80 347
MMartens 1:f3fe6d2b7639 348 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 349 //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
MMartens 2:7c6391c8ca71 350
EvB 13:acd120520e80 351
EvB 13:acd120520e80 352 error1 = q1 - Pos1; // Position error of link 1 [m]
EvB 13:acd120520e80 353 error2 = q2 - Pos2; // Position error of link 2 [m]
EvB 13:acd120520e80 354
MMartens 3:e888f52a46bc 355 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 356 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 357 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 358 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 359 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 360 pidTerm = 1000;
MMartens 3:e888f52a46bc 361 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 362 pidTerm = -1000;
MMartens 3:e888f52a46bc 363 } else {
MMartens 3:e888f52a46bc 364 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 365 }
MMartens 3:e888f52a46bc 366 //constraining to appropriate value
EvB 13:acd120520e80 367 if (pidTerm >= 0) {
vera1 12:5be2001abe62 368 dir1 = 0;// Forward motion
MMartens 1:f3fe6d2b7639 369 } else {
vera1 12:5be2001abe62 370 dir1 = 1;//Reverse motion
MMartens 1:f3fe6d2b7639 371 }
MMartens 3:e888f52a46bc 372 pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
EvB 13:acd120520e80 373 if(pidTerm_scaled >= 0.55f) {
MMartens 3:e888f52a46bc 374 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 375 }
EvB 13:acd120520e80 376
MMartens 4:75f6e4845194 377 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 378 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 4:75f6e4845194 379 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 380 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 381 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 382 pidTerm2 = 1000;
MMartens 4:75f6e4845194 383 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 384 pidTerm2 = -1000;
MMartens 4:75f6e4845194 385 } else {
MMartens 4:75f6e4845194 386 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 387 }
MMartens 4:75f6e4845194 388 //constraining to appropriate value
EvB 13:acd120520e80 389 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 390 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 391 } else {
MMartens 4:75f6e4845194 392 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 393 }
MMartens 4:75f6e4845194 394 pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
EvB 13:acd120520e80 395 if(pidTerm_scaled2 >= 0.55f) {
MMartens 4:75f6e4845194 396 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 397 }
EvB 13:acd120520e80 398
MMartens 1:f3fe6d2b7639 399 last_error = error1;
MMartens 3:e888f52a46bc 400 speed1 = pidTerm_scaled+0.45f;
vera1 12:5be2001abe62 401 //speedservo1 = speed1;
MMartens 4:75f6e4845194 402 last_error2 = error2;
MMartens 4:75f6e4845194 403 speed2 = pidTerm_scaled2+0.45f;
vera1 12:5be2001abe62 404 //speedservo2 = speed2;
MMartens 0:9167ae5d9927 405 }
MMartens 0:9167ae5d9927 406
EvB 13:acd120520e80 407 // --- Motor movements ---
EvB 13:acd120520e80 408 void r_movehorizontal()
EvB 13:acd120520e80 409 {
EvB 13:acd120520e80 410 setpointreadout(Vx_des); // Start with obtaining the position of the robot and the desired velocities
EvB 14:7a95e57b5364 411 if (count == 100) {
EvB 14:7a95e57b5364 412 //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
EvB 14:7a95e57b5364 413 }
EvB 13:acd120520e80 414 Jacobian(Vx_des, 0, 0); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
EvB 13:acd120520e80 415 PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
EvB 13:acd120520e80 416
EvB 13:acd120520e80 417 }
EvB 13:acd120520e80 418
EvB 13:acd120520e80 419 void r_movevertical()
EvB 13:acd120520e80 420 {
EvB 13:acd120520e80 421 setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
EvB 14:7a95e57b5364 422 if (count == 100) {
EvB 14:7a95e57b5364 423 // pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
EvB 14:7a95e57b5364 424 }
EvB 13:acd120520e80 425 Jacobian(0, Vy_des, 0); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
EvB 13:acd120520e80 426 PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
EvB 13:acd120520e80 427
EvB 13:acd120520e80 428 }
EvB 13:acd120520e80 429
EvB 13:acd120520e80 430 void r_movebase()
EvB 13:acd120520e80 431 {
EvB 13:acd120520e80 432 setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
EvB 14:7a95e57b5364 433 if (count == 100) {
EvB 14:7a95e57b5364 434 pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
EvB 14:7a95e57b5364 435 }
EvB 13:acd120520e80 436 Jacobian(0, 0, Vz_des); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
EvB 13:acd120520e80 437 PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
EvB 13:acd120520e80 438 }
EvB 13:acd120520e80 439 //--------------------------------
EvB 13:acd120520e80 440
EvB 13:acd120520e80 441
vera1 8:9edf32e021a5 442 void maintickerfunction()
EvB 13:acd120520e80 443 {
EvB 13:acd120520e80 444 count++;
EvB 13:acd120520e80 445 if (go_switch) {
EvB 13:acd120520e80 446 r_processStateSwitch();
EvB 13:acd120520e80 447 }
EvB 13:acd120520e80 448
EvB 14:7a95e57b5364 449 //if(1) { // Sample EMG with 500 Hz
EvB 14:7a95e57b5364 450 processEMG();
EvB 14:7a95e57b5364 451
EvB 14:7a95e57b5364 452 //}
EvB 14:7a95e57b5364 453
EvB 13:acd120520e80 454 if (go_move) {
EvB 14:7a95e57b5364 455 switch(state) {
EvB 14:7a95e57b5364 456 case R_HORIZONTAL:
EvB 14:7a95e57b5364 457 r_movehorizontal();
EvB 14:7a95e57b5364 458 break;
EvB 14:7a95e57b5364 459 case R_VERTICAL:
EvB 14:7a95e57b5364 460 r_movevertical();
EvB 14:7a95e57b5364 461 break;
EvB 14:7a95e57b5364 462 case R_BASE:
EvB 14:7a95e57b5364 463 r_movebase();
EvB 14:7a95e57b5364 464 break;
EvB 13:acd120520e80 465 }
vera1 11:bda77916d2ea 466 }
EvB 13:acd120520e80 467
EvB 13:acd120520e80 468
vera1 8:9edf32e021a5 469 }
vera1 8:9edf32e021a5 470
MMartens 1:f3fe6d2b7639 471 int main()
EvB 13:acd120520e80 472 {
EvB 14:7a95e57b5364 473 pc.baud(115200);
vera1 8:9edf32e021a5 474 go_calibration = true; // Setting the timeout variable
EvB 14:7a95e57b5364 475 go_switch = false; // Setting ticker variables
EvB 13:acd120520e80 476 go_move = true;
MMartens 1:f3fe6d2b7639 477 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 478 speed2.period(PwmPeriod);
EvB 14:7a95e57b5364 479 state = R_BASE;
vera1 8:9edf32e021a5 480 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 8:9edf32e021a5 481 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
EvB 13:acd120520e80 482 wait(5.0f);
EvB 14:7a95e57b5364 483 mainticker.attach(&maintickerfunction,0.002f);
EvB 13:acd120520e80 484
MMartens 1:f3fe6d2b7639 485 while(true) {
EvB 13:acd120520e80 486
vera1 11:bda77916d2ea 487 ledstatedef = 1;
vera1 11:bda77916d2ea 488 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 11:bda77916d2ea 489 ledstateswitch = 1;
vera1 11:bda77916d2ea 490 ledstatedef = 0;
vera1 11:bda77916d2ea 491 go_switch = true;
EvB 13:acd120520e80 492 go_move = false;
EvB 13:acd120520e80 493
vera1 11:bda77916d2ea 494 }
EvB 13:acd120520e80 495
EvB 14:7a95e57b5364 496
vera1 11:bda77916d2ea 497
EvB 13:acd120520e80 498 if(count == 100) {
MMartens 3:e888f52a46bc 499 count = 0;
EvB 14:7a95e57b5364 500 //pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 11:bda77916d2ea 501 //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
vera1 11:bda77916d2ea 502 //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
MMartens 3:e888f52a46bc 503 }
EvB 13:acd120520e80 504
EvB 13:acd120520e80 505
EvB 13:acd120520e80 506 wait(0.001f);
vera1 6:fc46581fe3e0 507 }
MMartens 1:f3fe6d2b7639 508
MMartens 0:9167ae5d9927 509 }
MMartens 0:9167ae5d9927 510
MMartens 0:9167ae5d9927 511