Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@15:207d01972e0b, 2017-10-31 (annotated)

Committer:
EvB
Date:
Tue Oct 31 11:07:42 2017 +0000
Revision:
15:207d01972e0b
Parent:
14:7a95e57b5364
Child:
16:1dd69f935b80
Motor draait links naar recht op commando. Testen: hoek onverandert als we van state switchen.

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 15:207d01972e0b 22 float EMG_threshold = 0.25; // 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.
EvB 15:207d01972e0b 47 Timer duration; // Timer to see how long the maintickerfunction takes to compute
vera1 8:9edf32e021a5 48 // ---------------------------------
vera1 8:9edf32e021a5 49
MMartens 0:9167ae5d9927 50
MMartens 1:f3fe6d2b7639 51 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 52
EvB 13:acd120520e80 53 volatile double Pos1; // Encoder readout of motor 1 [counts]
EvB 13:acd120520e80 54 volatile double Pos2; // Encoder readout of motor 2 [counts]
EvB 14:7a95e57b5364 55 volatile int count = 0; // Loop counts
MMartens 0:9167ae5d9927 56
EvB 15:207d01972e0b 57 double Kp = 25.0;// you can set these constants however you like depending on trial & error
EvB 15:207d01972e0b 58 double Ki = 10.0;
EvB 15:207d01972e0b 59 double Kd = 10.0;
MMartens 0:9167ae5d9927 60
MMartens 0:9167ae5d9927 61 float last_error = 0;
MMartens 1:f3fe6d2b7639 62 float error1 = 0;
MMartens 0:9167ae5d9927 63 float changeError = 0;
MMartens 0:9167ae5d9927 64 float totalError = 0;
MMartens 0:9167ae5d9927 65 float pidTerm = 0;
MMartens 4:75f6e4845194 66 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 67
MMartens 4:75f6e4845194 68 float last_error2 = 0;
MMartens 4:75f6e4845194 69 float error2 = 0;
MMartens 4:75f6e4845194 70 float changeError2 = 0;
MMartens 4:75f6e4845194 71 float totalError2 = 0;
MMartens 4:75f6e4845194 72 float pidTerm2 = 0;
MMartens 4:75f6e4845194 73 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 74
MMartens 0:9167ae5d9927 75 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 76 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 77 volatile double position = 0.0;
MMartens 4:75f6e4845194 78 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 79
EvB 13:acd120520e80 80 // --- Kinematics parameter ---
EvB 13:acd120520e80 81 float L1 = 0.38; // length of link 1 [m]
EvB 13:acd120520e80 82 float L5 = 0.25; // length of link 5 [m]
EvB 13:acd120520e80 83 float d = 0.03; // diameter of the pulley [m]
EvB 13:acd120520e80 84
EvB 13:acd120520e80 85 volatile float xe; // x-coordinate of end-effector
EvB 13:acd120520e80 86 volatile float ye; // y-coordinate of end-effector
EvB 13:acd120520e80 87 volatile float ze; // z-coordinate of end-effector
EvB 13:acd120520e80 88
EvB 13:acd120520e80 89 volatile float Q1; // Position of joint 1 [m]
EvB 13:acd120520e80 90 volatile float Q2; // Position of joint 2 [rad]
EvB 13:acd120520e80 91 volatile float Q3; // Position of joint 3 [rad]
EvB 13:acd120520e80 92
EvB 13:acd120520e80 93 volatile float V1; // Desired velocity of joint 1 [m/s]; --> or normalize it?
EvB 13:acd120520e80 94 volatile float W2; // Desired velocity of joint 2 [rad/s];
EvB 13:acd120520e80 95 volatile float W3; // Desired velocity of joint 3 [rad/s];
EvB 13:acd120520e80 96
EvB 15:207d01972e0b 97 float Vx_des; // Desired velocity end-effector in x-direction [m/s]
EvB 15:207d01972e0b 98 float Vy_des; // Desired velocity end-effector in y-direction [m/s]
EvB 15:207d01972e0b 99 float Vz_des; // Desired velocity end-effector in z-direction [m/s]
EvB 13:acd120520e80 100
EvB 13:acd120520e80 101 const float T = 0.002; // Time interval of the readout of the EMG --> Check this with the ticker, etc!!!
EvB 15:207d01972e0b 102 const float SampleTime = 0.002; // Time interval of the mainticker
EvB 13:acd120520e80 103
vera1 8:9edf32e021a5 104 // --- Booleans for the maintickerfunction ---
EvB 13:acd120520e80 105 bool go_calibration; // Boolean to put on/off the calibration of the EMG
EvB 13:acd120520e80 106 bool go_switch; // Boolean to go to different state
EvB 13:acd120520e80 107 bool go_move; //Boolean to move the robot arm and base
EvB 13:acd120520e80 108 bool go_kinematics; // Boolean which will determine whether a new position will be calculated or not --> replace by go_move?
vera1 8:9edf32e021a5 109 // -------------------------------------------
vera1 8:9edf32e021a5 110
vera1 8:9edf32e021a5 111 // --- calibrate EMG signal ----
vera1 8:9edf32e021a5 112
vera1 8:9edf32e021a5 113 void calibrationGO() // Function for go or no go of calibration
vera1 8:9edf32e021a5 114 {
vera1 8:9edf32e021a5 115 go_calibration = false;
EvB 13:acd120520e80 116
vera1 8:9edf32e021a5 117 }
vera1 8:9edf32e021a5 118
vera1 8:9edf32e021a5 119 /*
vera1 8:9edf32e021a5 120 Calibration of the robot works as follows:
vera1 8:9edf32e021a5 121 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 122 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 8:9edf32e021a5 123 During the calibration the user should exert maximum force.
vera1 8:9edf32e021a5 124 */
vera1 8:9edf32e021a5 125 void calibrationEMG() // Function for calibrating the EMG signal
vera1 8:9edf32e021a5 126 {
EvB 13:acd120520e80 127 if (go_calibration) {
EvB 13:acd120520e80 128
EvB 13:acd120520e80 129 emg1.calibration(); // Using the calibration function of the EMG_filter class
EvB 13:acd120520e80 130 emg2.calibration();
vera1 8:9edf32e021a5 131 }
vera1 8:9edf32e021a5 132 }
vera1 8:9edf32e021a5 133
vera1 8:9edf32e021a5 134 // ------------------------------
EvB 13:acd120520e80 135 //Function that reads out the raw EMG and filters the signal
EvB 13:acd120520e80 136 void processEMG ()
EvB 13:acd120520e80 137 {
MMartens 3:e888f52a46bc 138
EvB 13:acd120520e80 139
EvB 13:acd120520e80 140 emg1.filter(); //filter the incoming emg signal
EvB 13:acd120520e80 141 emg2.filter();
EvB 13:acd120520e80 142 //emg3.filter();
EvB 14:7a95e57b5364 143 /*
EvB 14:7a95e57b5364 144 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
EvB 14:7a95e57b5364 145 scope.set(1, emg2.normalized);
EvB 14:7a95e57b5364 146 scope.send();
EvB 14:7a95e57b5364 147 */
EvB 13:acd120520e80 148 }
EvB 13:acd120520e80 149
EvB 13:acd120520e80 150 // --- Kinematics functions ---
EvB 13:acd120520e80 151
EvB 13:acd120520e80 152 void Brockett(float q1, float q2, float q3) // Determine position of end-effector using Brockett
vera1 11:bda77916d2ea 153 {
vera1 11:bda77916d2ea 154
EvB 13:acd120520e80 155 Matrix E100b(4,4); // Complete matrix form of 1st joint
EvB 13:acd120520e80 156 E100b = MatrixMath::Eye(4);
EvB 13:acd120520e80 157 E100b(3,4) = q1;
EvB 13:acd120520e80 158
EvB 13:acd120520e80 159
EvB 13:acd120520e80 160 Matrix E210b(4,4); // Complete matrix form of 2nd joint
EvB 13:acd120520e80 161 E210b = MatrixMath::RotZ(q2);
EvB 13:acd120520e80 162
EvB 13:acd120520e80 163
EvB 13:acd120520e80 164 Matrix E320b(4,4); // Complete matrix form of 3rd joint
EvB 13:acd120520e80 165 E320b = MatrixMath::RotZ(q3-q2);
EvB 13:acd120520e80 166 E320b(1,4) = L1 - L1*cos(q3-q2);
EvB 13:acd120520e80 167 E320b(2,4) = -L1*sin(q3-q2);
EvB 13:acd120520e80 168
EvB 13:acd120520e80 169
EvB 13:acd120520e80 170 Matrix HE0ref(4,4); // H-matrix of the robot in reference position
EvB 13:acd120520e80 171 HE0ref = MatrixMath::Eye(4);
EvB 13:acd120520e80 172 HE0ref(1,4) = L1;
EvB 13:acd120520e80 173 HE0ref(2,4) = -L5;
EvB 13:acd120520e80 174
EvB 13:acd120520e80 175
EvB 13:acd120520e80 176 Matrix HE0(4,4); // H-matrix to find the position of the end-effector, expressed in frame 0.
EvB 13:acd120520e80 177 HE0 = E100b * E210b * E320b * HE0ref; // This H-matrix is not really correct. Check with matlab!!!
EvB 13:acd120520e80 178
EvB 13:acd120520e80 179
EvB 13:acd120520e80 180 // Determine new position of end effector
EvB 13:acd120520e80 181 xe = HE0(1,4); // New x-coordinate of the end-effector
EvB 13:acd120520e80 182 ye = HE0(2,4); // New y-coordinate of the end-effector
EvB 13:acd120520e80 183 ze = HE0(3,4); // New z-coordinate of the end-effector
EvB 13:acd120520e80 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
EvB 13:acd120520e80 197 Brockett(Q1,Q2,Q3); // Start with conducting Brockett to obtain the end-effector position
EvB 13:acd120520e80 198
EvB 13:acd120520e80 199 Matrix T_des(3,1); // Twist of desired linear velocities
EvB 13:acd120520e80 200 T_des(1,1) = vx_des;
EvB 13:acd120520e80 201 T_des(2,1) = vy_des;
EvB 13:acd120520e80 202 T_des(3,1) = vz_des;
EvB 13:acd120520e80 203
EvB 13:acd120520e80 204
EvB 13:acd120520e80 205 Matrix T100j(1,4); // Unit twist of the first frame in current configuration
EvB 13:acd120520e80 206 T100j(1,4) = 1;
EvB 13:acd120520e80 207
EvB 13:acd120520e80 208 Matrix T210j(1,4); // Unit twist of the second frame in current configuration
EvB 13:acd120520e80 209 T210j(1,1) = 1;
EvB 13:acd120520e80 210
EvB 13:acd120520e80 211 Matrix T320j(1,4); // Unit twist of the third frame in current configuration
EvB 13:acd120520e80 212 T320j(1,1) = 1;
EvB 13:acd120520e80 213 T320j(1,2) = -L1*sin(Q2);
EvB 13:acd120520e80 214 T320j(1,3) = -L1 * cos(Q2);
EvB 13:acd120520e80 215
EvB 13:acd120520e80 216 Matrix J(4,3); // Jacobian matrix
EvB 13:acd120520e80 217
EvB 13:acd120520e80 218 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 219 J(i,1) = T100j(1,i);
EvB 13:acd120520e80 220 }
EvB 13:acd120520e80 221
EvB 13:acd120520e80 222 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 223 J(i,2) = T210j(1,i);
EvB 13:acd120520e80 224 }
EvB 13:acd120520e80 225
EvB 13:acd120520e80 226 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 227 J(i,3) = T320j(1,i);
EvB 13:acd120520e80 228 }
EvB 13:acd120520e80 229
EvB 13:acd120520e80 230
EvB 13:acd120520e80 231 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 232 H0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 233 H0f(1,4) = -xe;
EvB 13:acd120520e80 234 H0f(2,4) = -ye;
EvB 13:acd120520e80 235 H0f(3,4) = -ze;
EvB 13:acd120520e80 236
EvB 13:acd120520e80 237 Matrix AdH0f(4,4); // Adjoint of H0f, necessary for twist transformations
EvB 13:acd120520e80 238 AdH0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 239 AdH0f(2,1) = H0f(2,4);
EvB 13:acd120520e80 240 AdH0f(3,1) = -H0f(1,4);
EvB 13:acd120520e80 241
EvB 13:acd120520e80 242 Matrix Jprime(4,3); // Transformed Jacobian
EvB 13:acd120520e80 243 Jprime = AdH0f*J;
EvB 13:acd120520e80 244
EvB 13:acd120520e80 245 Matrix Jprimeprime(3,3); // Truncated Jacobian
EvB 13:acd120520e80 246 for (int i = 1 ; i <4 ; i++) {
EvB 13:acd120520e80 247 for (int j = 1 ; j < 4 ; j++) {
EvB 13:acd120520e80 248 Jprimeprime(i,j) = Jprime(i+1,j);
EvB 13:acd120520e80 249 }
EvB 13:acd120520e80 250 }
EvB 13:acd120520e80 251
EvB 13:acd120520e80 252 Matrix JprimeprimeT(3,3); // The transpose of the truncated Jacobian
EvB 13:acd120520e80 253 JprimeprimeT = MatrixMath::Transpose(Jprimeprime);
EvB 13:acd120520e80 254
EvB 13:acd120520e80 255 Matrix PseudoInverse(3,3); // The Pseudo-inverse of the truncated Jacobian
EvB 13:acd120520e80 256 PseudoInverse = MatrixMath::Inv(JprimeprimeT*Jprimeprime)*JprimeprimeT;
EvB 13:acd120520e80 257
EvB 13:acd120520e80 258 Matrix Q_dot(1,3); // Desired joint velocities
EvB 13:acd120520e80 259 Q_dot = PseudoInverse * T_des;
EvB 13:acd120520e80 260
EvB 13:acd120520e80 261 V1 = Q_dot(1,1);
EvB 13:acd120520e80 262 W2 = Q_dot(2,1);
EvB 13:acd120520e80 263 W3 = Q_dot(3,1);
EvB 13:acd120520e80 264
EvB 15:207d01972e0b 265
EvB 13:acd120520e80 266 Q1 += V1*T; // Predicted value for q1 [m]
EvB 13:acd120520e80 267 Q2 += W2*T; // Predicted value for q2 [rad]
EvB 13:acd120520e80 268 Q3 += W3*T; // Predicted value for q3 [rad]
EvB 13:acd120520e80 269
EvB 15:207d01972e0b 270 /*
EvB 14:7a95e57b5364 271 if (count == 100) {
EvB 15:207d01972e0b 272
EvB 15:207d01972e0b 273 pc.printf("Velocities v1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
EvB 15:207d01972e0b 274 pc.printf("After velocities Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
EvB 14:7a95e57b5364 275 }
EvB 15:207d01972e0b 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 15:207d01972e0b 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 15:207d01972e0b 319 Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
EvB 15:207d01972e0b 320 Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
EvB 13:acd120520e80 321
EvB 15:207d01972e0b 322 /*
EvB 15:207d01972e0b 323 if (Pos2 >= EMG_threshold) // (pot.read()>= EMG_threshold)
EvB 15:207d01972e0b 324 {
EvB 15:207d01972e0b 325 v_des = 0.1; // pot.read();
EvB 15:207d01972e0b 326
EvB 15:207d01972e0b 327 }
EvB 14:7a95e57b5364 328
EvB 14:7a95e57b5364 329 if (pot2.read() >= EMG_threshold)
EvB 14:7a95e57b5364 330 {
EvB 15:207d01972e0b 331 v_des = -0.1; // -pot2.read();
EvB 14:7a95e57b5364 332 }
EvB 15:207d01972e0b 333 */
EvB 15:207d01972e0b 334
EvB 13:acd120520e80 335 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
EvB 15:207d01972e0b 336 v_des = emg1.normalized;
EvB 15:207d01972e0b 337
EvB 13:acd120520e80 338 }
EvB 15:207d01972e0b 339
EvB 15:207d01972e0b 340 if (emg2.normalized >= EMG_threshold && emg1.normalized <= EMG_threshold) { // Negative direction
EvB 15:207d01972e0b 341 v_des = -emg2.normalized;
EvB 15:207d01972e0b 342 }
EvB 15:207d01972e0b 343 }
MMartens 2:7c6391c8ca71 344
EvB 13:acd120520e80 345
EvB 13:acd120520e80 346 void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 347 {
EvB 13:acd120520e80 348
MMartens 1:f3fe6d2b7639 349 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 350 //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 351
EvB 13:acd120520e80 352
EvB 13:acd120520e80 353 error1 = q1 - Pos1; // Position error of link 1 [m]
EvB 13:acd120520e80 354 error2 = q2 - Pos2; // Position error of link 2 [m]
EvB 13:acd120520e80 355
EvB 15:207d01972e0b 356
EvB 15:207d01972e0b 357 changeError = (error1 - last_error)/SampleTime; // derivative term
EvB 15:207d01972e0b 358 totalError += error1*SampleTime; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 359 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 360 pidTerm = pidTerm;
EvB 15:207d01972e0b 361 if (pidTerm >= 10) {
EvB 15:207d01972e0b 362 pidTerm = 10;
EvB 15:207d01972e0b 363 } else if (pidTerm <= -10) {
EvB 15:207d01972e0b 364 pidTerm = -10;
MMartens 3:e888f52a46bc 365 } else {
MMartens 3:e888f52a46bc 366 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 367 }
MMartens 3:e888f52a46bc 368 //constraining to appropriate value
EvB 13:acd120520e80 369 if (pidTerm >= 0) {
vera1 12:5be2001abe62 370 dir1 = 0;// Forward motion
MMartens 1:f3fe6d2b7639 371 } else {
vera1 12:5be2001abe62 372 dir1 = 1;//Reverse motion
MMartens 1:f3fe6d2b7639 373 }
EvB 15:207d01972e0b 374 pidTerm_scaled = abs(pidTerm); ///10.0f; //make sure it's a positive value
EvB 13:acd120520e80 375 if(pidTerm_scaled >= 0.55f) {
MMartens 3:e888f52a46bc 376 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 377 }
EvB 13:acd120520e80 378
EvB 15:207d01972e0b 379 changeError2 = (error2 - last_error2)/SampleTime; // derivative term
EvB 15:207d01972e0b 380 totalError2 += error2*SampleTime; //accumalate errors to find integral term
MMartens 4:75f6e4845194 381 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 382 pidTerm2 = pidTerm2;
EvB 15:207d01972e0b 383 if (pidTerm2 >= 10) {
EvB 15:207d01972e0b 384 pidTerm2 = 10;
EvB 15:207d01972e0b 385 } else if (pidTerm2 <= -10) {
EvB 15:207d01972e0b 386 pidTerm2 = -10;
MMartens 4:75f6e4845194 387 } else {
MMartens 4:75f6e4845194 388 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 389 }
MMartens 4:75f6e4845194 390 //constraining to appropriate value
EvB 13:acd120520e80 391 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 392 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 393 } else {
MMartens 4:75f6e4845194 394 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 395 }
EvB 15:207d01972e0b 396 pidTerm_scaled2 = abs(pidTerm2); ///10.0f; //make sure it's a positive value
EvB 13:acd120520e80 397 if(pidTerm_scaled2 >= 0.55f) {
MMartens 4:75f6e4845194 398 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 399 }
EvB 13:acd120520e80 400
EvB 15:207d01972e0b 401 if (count%100 == 0){
EvB 15:207d01972e0b 402 //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);}
EvB 15:207d01972e0b 403
MMartens 1:f3fe6d2b7639 404 last_error = error1;
MMartens 3:e888f52a46bc 405 speed1 = pidTerm_scaled+0.45f;
vera1 12:5be2001abe62 406 //speedservo1 = speed1;
MMartens 4:75f6e4845194 407 last_error2 = error2;
MMartens 4:75f6e4845194 408 speed2 = pidTerm_scaled2+0.45f;
vera1 12:5be2001abe62 409 //speedservo2 = speed2;
MMartens 0:9167ae5d9927 410 }
MMartens 0:9167ae5d9927 411
EvB 13:acd120520e80 412 // --- Motor movements ---
EvB 13:acd120520e80 413 void r_movehorizontal()
EvB 13:acd120520e80 414 {
EvB 13:acd120520e80 415 setpointreadout(Vx_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 416 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 417 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 15:207d01972e0b 418 if (count%100 == 0)[
EvB 15:207d01972e0b 419 pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
EvB 13:acd120520e80 420 }
EvB 13:acd120520e80 421
EvB 13:acd120520e80 422 void r_movevertical()
EvB 13:acd120520e80 423 {
EvB 13:acd120520e80 424 setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
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 15:207d01972e0b 427 if (count%100 == 0)[
EvB 15:207d01972e0b 428 pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
EvB 13:acd120520e80 429 }
EvB 13:acd120520e80 430
EvB 13:acd120520e80 431 void r_movebase()
EvB 13:acd120520e80 432 {
EvB 13:acd120520e80 433 setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 434 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 435 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 15:207d01972e0b 436 if (count%100 == 0)[
EvB 15:207d01972e0b 437 pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
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 15:207d01972e0b 444 duration.reset();
EvB 15:207d01972e0b 445 duration.start();
EvB 13:acd120520e80 446 count++;
EvB 15:207d01972e0b 447
EvB 15:207d01972e0b 448
EvB 15:207d01972e0b 449 //if (count%2 == 0){ // Sample at 500 Hz
EvB 15:207d01972e0b 450 processEMG();
EvB 15:207d01972e0b 451 //}
EvB 15:207d01972e0b 452
EvB 13:acd120520e80 453 if (go_switch) {
EvB 13:acd120520e80 454 r_processStateSwitch();
EvB 13:acd120520e80 455 }
EvB 13:acd120520e80 456
EvB 13:acd120520e80 457 if (go_move) {
EvB 14:7a95e57b5364 458 switch(state) {
EvB 14:7a95e57b5364 459 case R_HORIZONTAL:
EvB 14:7a95e57b5364 460 r_movehorizontal();
EvB 14:7a95e57b5364 461 break;
EvB 14:7a95e57b5364 462 case R_VERTICAL:
EvB 14:7a95e57b5364 463 r_movevertical();
EvB 14:7a95e57b5364 464 break;
EvB 14:7a95e57b5364 465 case R_BASE:
EvB 14:7a95e57b5364 466 r_movebase();
EvB 14:7a95e57b5364 467 break;
EvB 13:acd120520e80 468 }
vera1 11:bda77916d2ea 469 }
EvB 13:acd120520e80 470
EvB 15:207d01972e0b 471 duration.stop();
EvB 15:207d01972e0b 472 //pc.printf("duration = %f\r\n", duration.read());
vera1 8:9edf32e021a5 473 }
vera1 8:9edf32e021a5 474
MMartens 1:f3fe6d2b7639 475 int main()
EvB 13:acd120520e80 476 {
EvB 14:7a95e57b5364 477 pc.baud(115200);
vera1 8:9edf32e021a5 478 go_calibration = true; // Setting the timeout variable
EvB 14:7a95e57b5364 479 go_switch = false; // Setting ticker variables
EvB 13:acd120520e80 480 go_move = true;
MMartens 1:f3fe6d2b7639 481 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 482 speed2.period(PwmPeriod);
EvB 15:207d01972e0b 483 state = R_VERTICAL; // Initialize the state of the robot
vera1 8:9edf32e021a5 484 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
EvB 15:207d01972e0b 485 mainticker.attach(&calibrationEMG, SampleTime); // Attach ticker to the calibration function
EvB 13:acd120520e80 486 wait(5.0f);
EvB 15:207d01972e0b 487 mainticker.attach(&maintickerfunction,SampleTime);
EvB 13:acd120520e80 488
MMartens 1:f3fe6d2b7639 489 while(true) {
EvB 15:207d01972e0b 490
vera1 11:bda77916d2ea 491 ledstatedef = 1;
EvB 15:207d01972e0b 492 if(emg1.normalized >= 0.6 && emg2.normalized >= 0.6) {
vera1 11:bda77916d2ea 493 ledstateswitch = 1;
vera1 11:bda77916d2ea 494 ledstatedef = 0;
vera1 11:bda77916d2ea 495 go_switch = true;
EvB 13:acd120520e80 496 go_move = false;
EvB 13:acd120520e80 497
EvB 15:207d01972e0b 498 }
EvB 14:7a95e57b5364 499
EvB 15:207d01972e0b 500 if(count%100 == 0) {
EvB 15:207d01972e0b 501
EvB 15:207d01972e0b 502 //pc.printf("MVC1 = %f, MVC2 = %f\r\n", emg1.MVC, emg2.MVC);
EvB 14:7a95e57b5364 503 //pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 11:bda77916d2ea 504 //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
vera1 11:bda77916d2ea 505 //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 506 }
EvB 13:acd120520e80 507
EvB 13:acd120520e80 508
EvB 15:207d01972e0b 509 wait(SampleTime);
EvB 15:207d01972e0b 510
vera1 6:fc46581fe3e0 511 }
MMartens 1:f3fe6d2b7639 512
MMartens 0:9167ae5d9927 513 }
MMartens 0:9167ae5d9927 514
MMartens 0:9167ae5d9927 515