Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@16:1dd69f935b80, 2017-10-31 (annotated)

Committer:
EvB
Date:
Tue Oct 31 15:45:03 2017 +0000
Revision:
16:1dd69f935b80
Parent:
15:207d01972e0b
Child:
17:ee159658326e
Control motor with EMG, inverse kinematics and proper control. Control can be tuned somewhat more.

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 16:1dd69f935b80 57 double Kp = 260.0;// you can set these constants however you like depending on trial & error
EvB 15:207d01972e0b 58 double Ki = 10.0;
EvB 16:1dd69f935b80 59 double Kd = 15.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 15:207d01972e0b 101 const float SampleTime = 0.002; // Time interval of the mainticker
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
EvB 13:acd120520e80 186 void Jacobian(float vx_des, float vy_des, float vz_des) // Function to determine the velocities with desired velocity as input
vera1 11:bda77916d2ea 187 {
EvB 13:acd120520e80 188 //kinematics_go = false; // putting boolean to false
EvB 13:acd120520e80 189
EvB 13:acd120520e80 190 // Finding joint positions
EvB 13:acd120520e80 191 Q1 = Pos1; // Position of joint 1 [m]
EvB 13:acd120520e80 192 Q2 = Pos2; // Position of joint 2 [rad]
EvB 13:acd120520e80 193 Q3 = 0; // Position of joint 3 [rad] --> need this from Servo!
EvB 13:acd120520e80 194
EvB 13:acd120520e80 195
EvB 13:acd120520e80 196 Brockett(Q1,Q2,Q3); // Start with conducting Brockett to obtain the end-effector position
EvB 13:acd120520e80 197
EvB 13:acd120520e80 198 Matrix T_des(3,1); // Twist of desired linear velocities
EvB 13:acd120520e80 199 T_des(1,1) = vx_des;
EvB 13:acd120520e80 200 T_des(2,1) = vy_des;
EvB 13:acd120520e80 201 T_des(3,1) = vz_des;
EvB 13:acd120520e80 202
EvB 13:acd120520e80 203
EvB 13:acd120520e80 204 Matrix T100j(1,4); // Unit twist of the first frame in current configuration
EvB 13:acd120520e80 205 T100j(1,4) = 1;
EvB 13:acd120520e80 206
EvB 13:acd120520e80 207 Matrix T210j(1,4); // Unit twist of the second frame in current configuration
EvB 13:acd120520e80 208 T210j(1,1) = 1;
EvB 13:acd120520e80 209
EvB 13:acd120520e80 210 Matrix T320j(1,4); // Unit twist of the third frame in current configuration
EvB 13:acd120520e80 211 T320j(1,1) = 1;
EvB 13:acd120520e80 212 T320j(1,2) = -L1*sin(Q2);
EvB 13:acd120520e80 213 T320j(1,3) = -L1 * cos(Q2);
EvB 13:acd120520e80 214
EvB 13:acd120520e80 215 Matrix J(4,3); // Jacobian matrix
EvB 13:acd120520e80 216
EvB 13:acd120520e80 217 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 218 J(i,1) = T100j(1,i);
EvB 13:acd120520e80 219 }
EvB 13:acd120520e80 220
EvB 13:acd120520e80 221 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 222 J(i,2) = T210j(1,i);
EvB 13:acd120520e80 223 }
EvB 13:acd120520e80 224
EvB 13:acd120520e80 225 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 226 J(i,3) = T320j(1,i);
EvB 13:acd120520e80 227 }
EvB 13:acd120520e80 228
EvB 13:acd120520e80 229
EvB 13:acd120520e80 230 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 231 H0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 232 H0f(1,4) = -xe;
EvB 13:acd120520e80 233 H0f(2,4) = -ye;
EvB 13:acd120520e80 234 H0f(3,4) = -ze;
EvB 13:acd120520e80 235
EvB 13:acd120520e80 236 Matrix AdH0f(4,4); // Adjoint of H0f, necessary for twist transformations
EvB 13:acd120520e80 237 AdH0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 238 AdH0f(2,1) = H0f(2,4);
EvB 13:acd120520e80 239 AdH0f(3,1) = -H0f(1,4);
EvB 13:acd120520e80 240
EvB 13:acd120520e80 241 Matrix Jprime(4,3); // Transformed Jacobian
EvB 13:acd120520e80 242 Jprime = AdH0f*J;
EvB 13:acd120520e80 243
EvB 13:acd120520e80 244 Matrix Jprimeprime(3,3); // Truncated Jacobian
EvB 13:acd120520e80 245 for (int i = 1 ; i <4 ; i++) {
EvB 13:acd120520e80 246 for (int j = 1 ; j < 4 ; j++) {
EvB 13:acd120520e80 247 Jprimeprime(i,j) = Jprime(i+1,j);
EvB 13:acd120520e80 248 }
EvB 13:acd120520e80 249 }
EvB 13:acd120520e80 250
EvB 13:acd120520e80 251 Matrix JprimeprimeT(3,3); // The transpose of the truncated Jacobian
EvB 13:acd120520e80 252 JprimeprimeT = MatrixMath::Transpose(Jprimeprime);
EvB 13:acd120520e80 253
EvB 13:acd120520e80 254 Matrix PseudoInverse(3,3); // The Pseudo-inverse of the truncated Jacobian
EvB 13:acd120520e80 255 PseudoInverse = MatrixMath::Inv(JprimeprimeT*Jprimeprime)*JprimeprimeT;
EvB 13:acd120520e80 256
EvB 13:acd120520e80 257 Matrix Q_dot(1,3); // Desired joint velocities
EvB 13:acd120520e80 258 Q_dot = PseudoInverse * T_des;
EvB 13:acd120520e80 259
EvB 13:acd120520e80 260 V1 = Q_dot(1,1);
EvB 13:acd120520e80 261 W2 = Q_dot(2,1);
EvB 13:acd120520e80 262 W3 = Q_dot(3,1);
EvB 16:1dd69f935b80 263
EvB 15:207d01972e0b 264
EvB 16:1dd69f935b80 265 Q1 += V1*SampleTime; // Predicted value for q1 [m]
EvB 16:1dd69f935b80 266 Q2 += W2*SampleTime; // Predicted value for q2 [rad]
EvB 16:1dd69f935b80 267 Q3 += W3*SampleTime; // Predicted value for q3 [rad]
EvB 13:acd120520e80 268
EvB 16:1dd69f935b80 269 if (count%100 == 0){
EvB 16:1dd69f935b80 270 pc.printf("\r\n Pos1 = %f, Pos2 = %f, Q1 = %f, Q2 = %f, Q3 = %f\r\n", Pos1, Pos2, Q1, Q2, Q3);
EvB 16:1dd69f935b80 271 pc.printf("\r\n V1 = %f, W2 = %f, W3 = %f\r\n", V1, W2, W3);}
vera1 11:bda77916d2ea 272 }
vera1 11:bda77916d2ea 273
vera1 11:bda77916d2ea 274 //--- State switch function -----
vera1 11:bda77916d2ea 275 void r_processStateSwitch()
MMartens 5:8c6d66a7c5da 276 {
EvB 13:acd120520e80 277 go_switch = false;
EvB 13:acd120520e80 278 switch(state) {
EvB 13:acd120520e80 279 case R_HORIZONTAL:
EvB 13:acd120520e80 280 state = R_VERTICAL;
EvB 13:acd120520e80 281 ledblue = 1;
EvB 13:acd120520e80 282 ledred = 1;
EvB 13:acd120520e80 283 ledgreen = 0;
EvB 13:acd120520e80 284 break;
EvB 13:acd120520e80 285 case R_VERTICAL:
EvB 13:acd120520e80 286 state = R_BASE;
EvB 13:acd120520e80 287 ledgreen = 1;
EvB 13:acd120520e80 288 ledblue = 1;
EvB 13:acd120520e80 289 ledred = 0;
EvB 13:acd120520e80 290 break;
EvB 13:acd120520e80 291 case R_BASE:
EvB 13:acd120520e80 292 state = R_HORIZONTAL;
EvB 13:acd120520e80 293 ledgreen = 1;
EvB 13:acd120520e80 294 ledred = 1;
EvB 13:acd120520e80 295 ledblue = 0;
EvB 13:acd120520e80 296 break;
EvB 13:acd120520e80 297 }
EvB 13:acd120520e80 298 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 299 ledstatedef = 1;
EvB 13:acd120520e80 300 ledstateswitch = 0;
EvB 13:acd120520e80 301 go_move = true;
MMartens 5:8c6d66a7c5da 302 }
vera1 8:9edf32e021a5 303
EvB 13:acd120520e80 304 // --- Determine the setpoints of the joint velocities
EvB 15:207d01972e0b 305 void setpointreadout(float& v_des) // something goes wrong here
vera1 8:9edf32e021a5 306 {
EvB 13:acd120520e80 307 /*
vera1 11:bda77916d2ea 308 potvalue = pot.read();
vera1 11:bda77916d2ea 309 setpoint = emg1.normalized;
EvB 13:acd120520e80 310
vera1 11:bda77916d2ea 311 potvalue2 = pot2.read();
vera1 11:bda77916d2ea 312 setpoint2 = emg2.normalized;
EvB 13:acd120520e80 313 */
EvB 15:207d01972e0b 314 Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
EvB 15:207d01972e0b 315 Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
EvB 13:acd120520e80 316
EvB 16:1dd69f935b80 317 v_des = 0;
EvB 15:207d01972e0b 318 /*
EvB 15:207d01972e0b 319 if (Pos2 >= EMG_threshold) // (pot.read()>= EMG_threshold)
EvB 15:207d01972e0b 320 {
EvB 15:207d01972e0b 321 v_des = 0.1; // pot.read();
EvB 15:207d01972e0b 322
EvB 15:207d01972e0b 323 }
EvB 14:7a95e57b5364 324
EvB 14:7a95e57b5364 325 if (pot2.read() >= EMG_threshold)
EvB 14:7a95e57b5364 326 {
EvB 15:207d01972e0b 327 v_des = -0.1; // -pot2.read();
EvB 14:7a95e57b5364 328 }
EvB 15:207d01972e0b 329 */
EvB 15:207d01972e0b 330
EvB 13:acd120520e80 331 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
EvB 16:1dd69f935b80 332 v_des = 0.8;
EvB 15:207d01972e0b 333
EvB 13:acd120520e80 334 }
EvB 15:207d01972e0b 335
EvB 15:207d01972e0b 336 if (emg2.normalized >= EMG_threshold && emg1.normalized <= EMG_threshold) { // Negative direction
EvB 16:1dd69f935b80 337 v_des = -0.8;
EvB 16:1dd69f935b80 338 }
EvB 16:1dd69f935b80 339
EvB 15:207d01972e0b 340 }
MMartens 2:7c6391c8ca71 341
EvB 13:acd120520e80 342
EvB 13:acd120520e80 343 void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 344 {
EvB 13:acd120520e80 345
MMartens 1:f3fe6d2b7639 346 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 347 //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 348
EvB 13:acd120520e80 349
EvB 13:acd120520e80 350 error1 = q1 - Pos1; // Position error of link 1 [m]
EvB 13:acd120520e80 351 error2 = q2 - Pos2; // Position error of link 2 [m]
EvB 16:1dd69f935b80 352
EvB 16:1dd69f935b80 353
EvB 15:207d01972e0b 354 changeError = (error1 - last_error)/SampleTime; // derivative term
EvB 15:207d01972e0b 355 totalError += error1*SampleTime; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 356 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 357 pidTerm = pidTerm;
EvB 15:207d01972e0b 358 if (pidTerm >= 10) {
EvB 15:207d01972e0b 359 pidTerm = 10;
EvB 15:207d01972e0b 360 } else if (pidTerm <= -10) {
EvB 15:207d01972e0b 361 pidTerm = -10;
MMartens 3:e888f52a46bc 362 } else {
MMartens 3:e888f52a46bc 363 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 364 }
MMartens 3:e888f52a46bc 365 //constraining to appropriate value
EvB 13:acd120520e80 366 if (pidTerm >= 0) {
vera1 12:5be2001abe62 367 dir1 = 0;// Forward motion
MMartens 1:f3fe6d2b7639 368 } else {
vera1 12:5be2001abe62 369 dir1 = 1;//Reverse motion
MMartens 1:f3fe6d2b7639 370 }
EvB 15:207d01972e0b 371 pidTerm_scaled = abs(pidTerm); ///10.0f; //make sure it's a positive value
EvB 13:acd120520e80 372 if(pidTerm_scaled >= 0.55f) {
MMartens 3:e888f52a46bc 373 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 374 }
EvB 13:acd120520e80 375
EvB 15:207d01972e0b 376 changeError2 = (error2 - last_error2)/SampleTime; // derivative term
EvB 15:207d01972e0b 377 totalError2 += error2*SampleTime; //accumalate errors to find integral term
MMartens 4:75f6e4845194 378 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 379 pidTerm2 = pidTerm2;
EvB 15:207d01972e0b 380 if (pidTerm2 >= 10) {
EvB 15:207d01972e0b 381 pidTerm2 = 10;
EvB 15:207d01972e0b 382 } else if (pidTerm2 <= -10) {
EvB 15:207d01972e0b 383 pidTerm2 = -10;
MMartens 4:75f6e4845194 384 } else {
MMartens 4:75f6e4845194 385 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 386 }
MMartens 4:75f6e4845194 387 //constraining to appropriate value
EvB 13:acd120520e80 388 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 389 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 390 } else {
MMartens 4:75f6e4845194 391 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 392 }
EvB 15:207d01972e0b 393 pidTerm_scaled2 = abs(pidTerm2); ///10.0f; //make sure it's a positive value
EvB 13:acd120520e80 394 if(pidTerm_scaled2 >= 0.55f) {
MMartens 4:75f6e4845194 395 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 396 }
EvB 13:acd120520e80 397
EvB 15:207d01972e0b 398 if (count%100 == 0){
EvB 16:1dd69f935b80 399 //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);
EvB 16:1dd69f935b80 400 }
EvB 15:207d01972e0b 401
MMartens 1:f3fe6d2b7639 402 last_error = error1;
MMartens 3:e888f52a46bc 403 speed1 = pidTerm_scaled+0.45f;
vera1 12:5be2001abe62 404 //speedservo1 = speed1;
MMartens 4:75f6e4845194 405 last_error2 = error2;
MMartens 4:75f6e4845194 406 speed2 = pidTerm_scaled2+0.45f;
vera1 12:5be2001abe62 407 //speedservo2 = speed2;
EvB 16:1dd69f935b80 408
EvB 16:1dd69f935b80 409 if (count%100 == 0){
EvB 16:1dd69f935b80 410 pc.printf("error1 = %f, error2 = %f, pidTerm = %f, pidTerm2 = %f\r\n", error1, error2, pidTerm_scaled, pidTerm_scaled2);}
MMartens 0:9167ae5d9927 411 }
MMartens 0:9167ae5d9927 412
EvB 13:acd120520e80 413 // --- Motor movements ---
EvB 13:acd120520e80 414 void r_movehorizontal()
EvB 13:acd120520e80 415 {
EvB 13:acd120520e80 416 setpointreadout(Vx_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 417 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 418 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 419 }
EvB 13:acd120520e80 420
EvB 13:acd120520e80 421 void r_movevertical()
EvB 13:acd120520e80 422 {
EvB 13:acd120520e80 423 setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 424 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 425 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 16:1dd69f935b80 426
EvB 13:acd120520e80 427 }
EvB 13:acd120520e80 428
EvB 13:acd120520e80 429 void r_movebase()
EvB 13:acd120520e80 430 {
EvB 13:acd120520e80 431 setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 432 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 433 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 434 }
EvB 13:acd120520e80 435 //--------------------------------
EvB 13:acd120520e80 436
EvB 13:acd120520e80 437
vera1 8:9edf32e021a5 438 void maintickerfunction()
EvB 13:acd120520e80 439 {
EvB 15:207d01972e0b 440 duration.reset();
EvB 15:207d01972e0b 441 duration.start();
EvB 13:acd120520e80 442 count++;
EvB 15:207d01972e0b 443
EvB 15:207d01972e0b 444
EvB 15:207d01972e0b 445 //if (count%2 == 0){ // Sample at 500 Hz
EvB 15:207d01972e0b 446 processEMG();
EvB 15:207d01972e0b 447 //}
EvB 15:207d01972e0b 448
EvB 13:acd120520e80 449 if (go_switch) {
EvB 13:acd120520e80 450 r_processStateSwitch();
EvB 13:acd120520e80 451 }
EvB 13:acd120520e80 452
EvB 13:acd120520e80 453 if (go_move) {
EvB 14:7a95e57b5364 454 switch(state) {
EvB 14:7a95e57b5364 455 case R_HORIZONTAL:
EvB 14:7a95e57b5364 456 r_movehorizontal();
EvB 14:7a95e57b5364 457 break;
EvB 14:7a95e57b5364 458 case R_VERTICAL:
EvB 14:7a95e57b5364 459 r_movevertical();
EvB 14:7a95e57b5364 460 break;
EvB 14:7a95e57b5364 461 case R_BASE:
EvB 14:7a95e57b5364 462 r_movebase();
EvB 14:7a95e57b5364 463 break;
EvB 13:acd120520e80 464 }
vera1 11:bda77916d2ea 465 }
EvB 13:acd120520e80 466
EvB 15:207d01972e0b 467 duration.stop();
EvB 15:207d01972e0b 468 //pc.printf("duration = %f\r\n", duration.read());
vera1 8:9edf32e021a5 469 }
vera1 8:9edf32e021a5 470
MMartens 1:f3fe6d2b7639 471 int main()
EvB 13:acd120520e80 472 {
EvB 16:1dd69f935b80 473 pc.printf("Hello Serotonin");
EvB 14:7a95e57b5364 474 pc.baud(115200);
vera1 8:9edf32e021a5 475 go_calibration = true; // Setting the timeout variable
EvB 14:7a95e57b5364 476 go_switch = false; // Setting ticker variables
EvB 13:acd120520e80 477 go_move = true;
MMartens 1:f3fe6d2b7639 478 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 479 speed2.period(PwmPeriod);
EvB 15:207d01972e0b 480 state = R_VERTICAL; // Initialize the state of the robot
vera1 8:9edf32e021a5 481 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
EvB 15:207d01972e0b 482 mainticker.attach(&calibrationEMG, SampleTime); // Attach ticker to the calibration function
EvB 13:acd120520e80 483 wait(5.0f);
EvB 15:207d01972e0b 484 mainticker.attach(&maintickerfunction,SampleTime);
EvB 13:acd120520e80 485
MMartens 1:f3fe6d2b7639 486 while(true) {
EvB 15:207d01972e0b 487
vera1 11:bda77916d2ea 488 ledstatedef = 1;
EvB 15:207d01972e0b 489 if(emg1.normalized >= 0.6 && emg2.normalized >= 0.6) {
vera1 11:bda77916d2ea 490 ledstateswitch = 1;
vera1 11:bda77916d2ea 491 ledstatedef = 0;
vera1 11:bda77916d2ea 492 go_switch = true;
EvB 13:acd120520e80 493 go_move = false;
EvB 13:acd120520e80 494
EvB 15:207d01972e0b 495 }
EvB 14:7a95e57b5364 496
EvB 15:207d01972e0b 497 if(count%100 == 0) {
EvB 15:207d01972e0b 498
EvB 15:207d01972e0b 499 //pc.printf("MVC1 = %f, MVC2 = %f\r\n", emg1.MVC, emg2.MVC);
EvB 16:1dd69f935b80 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 15:207d01972e0b 506 wait(SampleTime);
EvB 15:207d01972e0b 507
vera1 6:fc46581fe3e0 508 }
MMartens 1:f3fe6d2b7639 509
MMartens 0:9167ae5d9927 510 }
MMartens 0:9167ae5d9927 511
MMartens 0:9167ae5d9927 512