Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@17:ee159658326e, 2017-10-31 (annotated)

Committer:
EvB
Date:
Tue Oct 31 16:29:25 2017 +0000
Revision:
17:ee159658326e
Parent:
16:1dd69f935b80 
Set Kd to zero. Doesn't give very much different response. Pos1 and Pos2 still weird values.

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 17:ee159658326e 57 double Kp = 50.0;// you can set these constants however you like depending on trial & error
EvB 17:ee159658326e 58 double Ki = 25.0;
EvB 17:ee159658326e 59 double Kd = 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 17:ee159658326e 271 pc.printf("\r\n V1 = %f, W2 = %f, W3 = %f\r\n", V1, W2, W3);
EvB 17:ee159658326e 272 pc.printf("\r\n encoder = %d,\r\n", motor1.getPosition());}
vera1 11:bda77916d2ea 273 }
vera1 11:bda77916d2ea 274
vera1 11:bda77916d2ea 275 //--- State switch function -----
vera1 11:bda77916d2ea 276 void r_processStateSwitch()
MMartens 5:8c6d66a7c5da 277 {
EvB 13:acd120520e80 278 go_switch = false;
EvB 13:acd120520e80 279 switch(state) {
EvB 13:acd120520e80 280 case R_HORIZONTAL:
EvB 13:acd120520e80 281 state = R_VERTICAL;
EvB 13:acd120520e80 282 ledblue = 1;
EvB 13:acd120520e80 283 ledred = 1;
EvB 13:acd120520e80 284 ledgreen = 0;
EvB 13:acd120520e80 285 break;
EvB 13:acd120520e80 286 case R_VERTICAL:
EvB 13:acd120520e80 287 state = R_BASE;
EvB 13:acd120520e80 288 ledgreen = 1;
EvB 13:acd120520e80 289 ledblue = 1;
EvB 13:acd120520e80 290 ledred = 0;
EvB 13:acd120520e80 291 break;
EvB 13:acd120520e80 292 case R_BASE:
EvB 13:acd120520e80 293 state = R_HORIZONTAL;
EvB 13:acd120520e80 294 ledgreen = 1;
EvB 13:acd120520e80 295 ledred = 1;
EvB 13:acd120520e80 296 ledblue = 0;
EvB 13:acd120520e80 297 break;
EvB 13:acd120520e80 298 }
EvB 13:acd120520e80 299 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 300 ledstatedef = 1;
EvB 13:acd120520e80 301 ledstateswitch = 0;
EvB 13:acd120520e80 302 go_move = true;
MMartens 5:8c6d66a7c5da 303 }
vera1 8:9edf32e021a5 304
EvB 13:acd120520e80 305 // --- Determine the setpoints of the joint velocities
EvB 15:207d01972e0b 306 void setpointreadout(float& v_des) // something goes wrong here
vera1 8:9edf32e021a5 307 {
EvB 13:acd120520e80 308 /*
vera1 11:bda77916d2ea 309 potvalue = pot.read();
vera1 11:bda77916d2ea 310 setpoint = emg1.normalized;
EvB 13:acd120520e80 311
vera1 11:bda77916d2ea 312 potvalue2 = pot2.read();
vera1 11:bda77916d2ea 313 setpoint2 = emg2.normalized;
EvB 13:acd120520e80 314 */
EvB 15:207d01972e0b 315 Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
EvB 15:207d01972e0b 316 Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
EvB 13:acd120520e80 317
EvB 16:1dd69f935b80 318 v_des = 0;
EvB 15:207d01972e0b 319 /*
EvB 15:207d01972e0b 320 if (Pos2 >= EMG_threshold) // (pot.read()>= EMG_threshold)
EvB 15:207d01972e0b 321 {
EvB 15:207d01972e0b 322 v_des = 0.1; // pot.read();
EvB 15:207d01972e0b 323
EvB 15:207d01972e0b 324 }
EvB 14:7a95e57b5364 325
EvB 14:7a95e57b5364 326 if (pot2.read() >= EMG_threshold)
EvB 14:7a95e57b5364 327 {
EvB 15:207d01972e0b 328 v_des = -0.1; // -pot2.read();
EvB 14:7a95e57b5364 329 }
EvB 15:207d01972e0b 330 */
EvB 15:207d01972e0b 331
EvB 13:acd120520e80 332 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
EvB 16:1dd69f935b80 333 v_des = 0.8;
EvB 15:207d01972e0b 334
EvB 13:acd120520e80 335 }
EvB 15:207d01972e0b 336
EvB 15:207d01972e0b 337 if (emg2.normalized >= EMG_threshold && emg1.normalized <= EMG_threshold) { // Negative direction
EvB 16:1dd69f935b80 338 v_des = -0.8;
EvB 16:1dd69f935b80 339 }
EvB 16:1dd69f935b80 340
EvB 15:207d01972e0b 341 }
MMartens 2:7c6391c8ca71 342
EvB 13:acd120520e80 343
EvB 13:acd120520e80 344 void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 345 {
EvB 13:acd120520e80 346
MMartens 1:f3fe6d2b7639 347 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 348 //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 349
EvB 13:acd120520e80 350
EvB 13:acd120520e80 351 error1 = q1 - Pos1; // Position error of link 1 [m]
EvB 13:acd120520e80 352 error2 = q2 - Pos2; // Position error of link 2 [m]
EvB 16:1dd69f935b80 353
EvB 16:1dd69f935b80 354
EvB 15:207d01972e0b 355 changeError = (error1 - last_error)/SampleTime; // derivative term
EvB 15:207d01972e0b 356 totalError += error1*SampleTime; //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;
EvB 15:207d01972e0b 359 if (pidTerm >= 10) {
EvB 15:207d01972e0b 360 pidTerm = 10;
EvB 15:207d01972e0b 361 } else if (pidTerm <= -10) {
EvB 15:207d01972e0b 362 pidTerm = -10;
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 }
EvB 15:207d01972e0b 372 pidTerm_scaled = abs(pidTerm); ///10.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
EvB 15:207d01972e0b 377 changeError2 = (error2 - last_error2)/SampleTime; // derivative term
EvB 15:207d01972e0b 378 totalError2 += error2*SampleTime; //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;
EvB 15:207d01972e0b 381 if (pidTerm2 >= 10) {
EvB 15:207d01972e0b 382 pidTerm2 = 10;
EvB 15:207d01972e0b 383 } else if (pidTerm2 <= -10) {
EvB 15:207d01972e0b 384 pidTerm2 = -10;
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 }
EvB 15:207d01972e0b 394 pidTerm_scaled2 = abs(pidTerm2); ///10.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
EvB 15:207d01972e0b 399 if (count%100 == 0){
EvB 17:ee159658326e 400 pc.printf("TotalError = %f, error = %f\r\n", totalError, error1);
EvB 16:1dd69f935b80 401 }
EvB 15:207d01972e0b 402
MMartens 1:f3fe6d2b7639 403 last_error = error1;
MMartens 3:e888f52a46bc 404 speed1 = pidTerm_scaled+0.45f;
vera1 12:5be2001abe62 405 //speedservo1 = speed1;
MMartens 4:75f6e4845194 406 last_error2 = error2;
MMartens 4:75f6e4845194 407 speed2 = pidTerm_scaled2+0.45f;
vera1 12:5be2001abe62 408 //speedservo2 = speed2;
EvB 16:1dd69f935b80 409
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 13:acd120520e80 418 }
EvB 13:acd120520e80 419
EvB 13:acd120520e80 420 void r_movevertical()
EvB 13:acd120520e80 421 {
EvB 13:acd120520e80 422 setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 423 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 424 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 425
EvB 13:acd120520e80 426 }
EvB 13:acd120520e80 427
EvB 13:acd120520e80 428 void r_movebase()
EvB 13:acd120520e80 429 {
EvB 13:acd120520e80 430 setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
EvB 17:ee159658326e 431 Jacobian(0, 0, 4*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 432 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 433 }
EvB 13:acd120520e80 434 //--------------------------------
EvB 13:acd120520e80 435
EvB 13:acd120520e80 436
vera1 8:9edf32e021a5 437 void maintickerfunction()
EvB 13:acd120520e80 438 {
EvB 15:207d01972e0b 439 duration.reset();
EvB 15:207d01972e0b 440 duration.start();
EvB 13:acd120520e80 441 count++;
EvB 15:207d01972e0b 442
EvB 15:207d01972e0b 443
EvB 15:207d01972e0b 444 //if (count%2 == 0){ // Sample at 500 Hz
EvB 15:207d01972e0b 445 processEMG();
EvB 15:207d01972e0b 446 //}
EvB 15:207d01972e0b 447
EvB 13:acd120520e80 448 if (go_switch) {
EvB 13:acd120520e80 449 r_processStateSwitch();
EvB 13:acd120520e80 450 }
EvB 13:acd120520e80 451
EvB 13:acd120520e80 452 if (go_move) {
EvB 14:7a95e57b5364 453 switch(state) {
EvB 14:7a95e57b5364 454 case R_HORIZONTAL:
EvB 14:7a95e57b5364 455 r_movehorizontal();
EvB 14:7a95e57b5364 456 break;
EvB 14:7a95e57b5364 457 case R_VERTICAL:
EvB 14:7a95e57b5364 458 r_movevertical();
EvB 14:7a95e57b5364 459 break;
EvB 14:7a95e57b5364 460 case R_BASE:
EvB 14:7a95e57b5364 461 r_movebase();
EvB 14:7a95e57b5364 462 break;
EvB 13:acd120520e80 463 }
vera1 11:bda77916d2ea 464 }
EvB 13:acd120520e80 465
EvB 15:207d01972e0b 466 duration.stop();
EvB 15:207d01972e0b 467 //pc.printf("duration = %f\r\n", duration.read());
vera1 8:9edf32e021a5 468 }
vera1 8:9edf32e021a5 469
MMartens 1:f3fe6d2b7639 470 int main()
EvB 13:acd120520e80 471 {
EvB 16:1dd69f935b80 472 pc.printf("Hello Serotonin");
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 15:207d01972e0b 479 state = R_VERTICAL; // Initialize the state of the robot
vera1 8:9edf32e021a5 480 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
EvB 15:207d01972e0b 481 mainticker.attach(&calibrationEMG, SampleTime); // Attach ticker to the calibration function
EvB 13:acd120520e80 482 wait(5.0f);
EvB 15:207d01972e0b 483 mainticker.attach(&maintickerfunction,SampleTime);
EvB 13:acd120520e80 484
MMartens 1:f3fe6d2b7639 485 while(true) {
EvB 15:207d01972e0b 486
vera1 11:bda77916d2ea 487 ledstatedef = 1;
EvB 15:207d01972e0b 488 if(emg1.normalized >= 0.6 && emg2.normalized >= 0.6) {
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
EvB 15:207d01972e0b 494 }
EvB 14:7a95e57b5364 495
EvB 15:207d01972e0b 496 if(count%100 == 0) {
EvB 15:207d01972e0b 497
EvB 15:207d01972e0b 498 //pc.printf("MVC1 = %f, MVC2 = %f\r\n", emg1.MVC, emg2.MVC);
EvB 16:1dd69f935b80 499 pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 11:bda77916d2ea 500 //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
vera1 11:bda77916d2ea 501 //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 502 }
EvB 13:acd120520e80 503
EvB 13:acd120520e80 504
EvB 15:207d01972e0b 505 wait(SampleTime);
EvB 15:207d01972e0b 506
vera1 6:fc46581fe3e0 507 }
MMartens 1:f3fe6d2b7639 508
MMartens 0:9167ae5d9927 509 }
MMartens 0:9167ae5d9927 510
MMartens 0:9167ae5d9927 511