Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@13:acd120520e80, 2017-10-30 (annotated)

Committer:
EvB
Date:
Mon Oct 30 15:01:26 2017 +0000
Revision:
13:acd120520e80
Parent:
12:5be2001abe62
Child:
14:7a95e57b5364
Implemented kinematics and movement description. Not tested at all, but compilable.

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 13:acd120520e80 45 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
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 13:acd120520e80 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 ---
vera1 6:fc46581fe3e0 104 //bool readoutsetpoint = true;
vera1 8:9edf32e021a5 105 bool go_EMG; // Boolean to put on/off the EMG readout
EvB 13:acd120520e80 106 bool go_calibration; // Boolean to put on/off the calibration of the EMG
EvB 13:acd120520e80 107 bool go_switch; // Boolean to go to different state
EvB 13:acd120520e80 108 bool go_PID; // Boolean to calculate PID and motor aansturing --> probably replaced by go_move
EvB 13:acd120520e80 109 bool go_move; //Boolean to move the robot arm and base
EvB 13:acd120520e80 110 bool go_kinematics; // Boolean which will determine whether a new position will be calculated or not --> replace by go_move?
vera1 8:9edf32e021a5 111 // -------------------------------------------
vera1 8:9edf32e021a5 112
vera1 8:9edf32e021a5 113 // --- calibrate EMG signal ----
vera1 8:9edf32e021a5 114
vera1 8:9edf32e021a5 115 void calibrationGO() // Function for go or no go of calibration
vera1 8:9edf32e021a5 116 {
vera1 8:9edf32e021a5 117 go_calibration = false;
EvB 13:acd120520e80 118
vera1 8:9edf32e021a5 119 }
vera1 8:9edf32e021a5 120
vera1 8:9edf32e021a5 121 /*
vera1 8:9edf32e021a5 122 Calibration of the robot works as follows:
vera1 8:9edf32e021a5 123 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 124 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 8:9edf32e021a5 125 During the calibration the user should exert maximum force.
vera1 8:9edf32e021a5 126 */
vera1 8:9edf32e021a5 127 void calibrationEMG() // Function for calibrating the EMG signal
vera1 8:9edf32e021a5 128 {
EvB 13:acd120520e80 129 if (go_calibration) {
EvB 13:acd120520e80 130
EvB 13:acd120520e80 131 emg1.calibration(); // Using the calibration function of the EMG_filter class
EvB 13:acd120520e80 132 emg2.calibration();
vera1 8:9edf32e021a5 133 }
vera1 8:9edf32e021a5 134 }
vera1 8:9edf32e021a5 135
vera1 8:9edf32e021a5 136 // ------------------------------
EvB 13:acd120520e80 137 //Function that reads out the raw EMG and filters the signal
EvB 13:acd120520e80 138 void processEMG ()
EvB 13:acd120520e80 139 {
MMartens 3:e888f52a46bc 140
EvB 13:acd120520e80 141
EvB 13:acd120520e80 142 emg1.filter(); //filter the incoming emg signal
EvB 13:acd120520e80 143 emg2.filter();
EvB 13:acd120520e80 144 //emg3.filter();
EvB 13:acd120520e80 145
EvB 13:acd120520e80 146 /* scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
EvB 13:acd120520e80 147 scope.set(1, emg1.emg0);
EvB 13:acd120520e80 148 scope.send();*/
EvB 13:acd120520e80 149
EvB 13:acd120520e80 150 }
EvB 13:acd120520e80 151
EvB 13:acd120520e80 152 // --- Kinematics functions ---
EvB 13:acd120520e80 153
EvB 13:acd120520e80 154 void Brockett(float q1, float q2, float q3) // Determine position of end-effector using Brockett
vera1 11:bda77916d2ea 155 {
vera1 11:bda77916d2ea 156
EvB 13:acd120520e80 157 Matrix E100b(4,4); // Complete matrix form of 1st joint
EvB 13:acd120520e80 158 E100b = MatrixMath::Eye(4);
EvB 13:acd120520e80 159 E100b(3,4) = q1;
EvB 13:acd120520e80 160
EvB 13:acd120520e80 161
EvB 13:acd120520e80 162 Matrix E210b(4,4); // Complete matrix form of 2nd joint
EvB 13:acd120520e80 163 E210b = MatrixMath::RotZ(q2);
EvB 13:acd120520e80 164
EvB 13:acd120520e80 165
EvB 13:acd120520e80 166 Matrix E320b(4,4); // Complete matrix form of 3rd joint
EvB 13:acd120520e80 167 E320b = MatrixMath::RotZ(q3-q2);
EvB 13:acd120520e80 168 E320b(1,4) = L1 - L1*cos(q3-q2);
EvB 13:acd120520e80 169 E320b(2,4) = -L1*sin(q3-q2);
EvB 13:acd120520e80 170
EvB 13:acd120520e80 171
EvB 13:acd120520e80 172 Matrix HE0ref(4,4); // H-matrix of the robot in reference position
EvB 13:acd120520e80 173 HE0ref = MatrixMath::Eye(4);
EvB 13:acd120520e80 174 HE0ref(1,4) = L1;
EvB 13:acd120520e80 175 HE0ref(2,4) = -L5;
EvB 13:acd120520e80 176
EvB 13:acd120520e80 177
EvB 13:acd120520e80 178 Matrix HE0(4,4); // H-matrix to find the position of the end-effector, expressed in frame 0.
EvB 13:acd120520e80 179 HE0 = E100b * E210b * E320b * HE0ref; // This H-matrix is not really correct. Check with matlab!!!
EvB 13:acd120520e80 180
EvB 13:acd120520e80 181
EvB 13:acd120520e80 182 // Determine new position of end effector
EvB 13:acd120520e80 183 xe = HE0(1,4); // New x-coordinate of the end-effector
EvB 13:acd120520e80 184 ye = HE0(2,4); // New y-coordinate of the end-effector
EvB 13:acd120520e80 185 ze = HE0(3,4); // New z-coordinate of the end-effector
EvB 13:acd120520e80 186
vera1 11:bda77916d2ea 187
vera1 11:bda77916d2ea 188 }
vera1 11:bda77916d2ea 189
EvB 13:acd120520e80 190 void Jacobian(float vx_des, float vy_des, float vz_des) // Function to determine the velocities with desired velocity as input
vera1 11:bda77916d2ea 191 {
EvB 13:acd120520e80 192 //kinematics_go = false; // putting boolean to false
EvB 13:acd120520e80 193
EvB 13:acd120520e80 194 // Finding joint positions
EvB 13:acd120520e80 195 Q1 = Pos1; // Position of joint 1 [m]
EvB 13:acd120520e80 196 Q2 = Pos2; // Position of joint 2 [rad]
EvB 13:acd120520e80 197 Q3 = 0; // Position of joint 3 [rad] --> need this from Servo!
EvB 13:acd120520e80 198
EvB 13:acd120520e80 199 //pc.printf("\r\nq1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1, q2, q3);
EvB 13:acd120520e80 200
EvB 13:acd120520e80 201 Brockett(Q1,Q2,Q3); // Start with conducting Brockett to obtain the end-effector position
EvB 13:acd120520e80 202
EvB 13:acd120520e80 203 Matrix T_des(3,1); // Twist of desired linear velocities
EvB 13:acd120520e80 204 T_des(1,1) = vx_des;
EvB 13:acd120520e80 205 T_des(2,1) = vy_des;
EvB 13:acd120520e80 206 T_des(3,1) = vz_des;
EvB 13:acd120520e80 207
EvB 13:acd120520e80 208
EvB 13:acd120520e80 209 Matrix T100j(1,4); // Unit twist of the first frame in current configuration
EvB 13:acd120520e80 210 T100j(1,4) = 1;
EvB 13:acd120520e80 211
EvB 13:acd120520e80 212 Matrix T210j(1,4); // Unit twist of the second frame in current configuration
EvB 13:acd120520e80 213 T210j(1,1) = 1;
EvB 13:acd120520e80 214
EvB 13:acd120520e80 215 Matrix T320j(1,4); // Unit twist of the third frame in current configuration
EvB 13:acd120520e80 216 T320j(1,1) = 1;
EvB 13:acd120520e80 217 T320j(1,2) = -L1*sin(Q2);
EvB 13:acd120520e80 218 T320j(1,3) = -L1 * cos(Q2);
EvB 13:acd120520e80 219
EvB 13:acd120520e80 220 Matrix J(4,3); // Jacobian matrix
EvB 13:acd120520e80 221
EvB 13:acd120520e80 222 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 223 J(i,1) = T100j(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,2) = T210j(1,i);
EvB 13:acd120520e80 228 }
EvB 13:acd120520e80 229
EvB 13:acd120520e80 230 for (int i = 1 ; i < 5 ; i++) {
EvB 13:acd120520e80 231 J(i,3) = T320j(1,i);
EvB 13:acd120520e80 232 }
EvB 13:acd120520e80 233
EvB 13:acd120520e80 234
EvB 13:acd120520e80 235 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 236 H0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 237 H0f(1,4) = -xe;
EvB 13:acd120520e80 238 H0f(2,4) = -ye;
EvB 13:acd120520e80 239 H0f(3,4) = -ze;
EvB 13:acd120520e80 240
EvB 13:acd120520e80 241 Matrix AdH0f(4,4); // Adjoint of H0f, necessary for twist transformations
EvB 13:acd120520e80 242 AdH0f = MatrixMath::Eye(4);
EvB 13:acd120520e80 243 AdH0f(2,1) = H0f(2,4);
EvB 13:acd120520e80 244 AdH0f(3,1) = -H0f(1,4);
EvB 13:acd120520e80 245
EvB 13:acd120520e80 246 Matrix Jprime(4,3); // Transformed Jacobian
EvB 13:acd120520e80 247 Jprime = AdH0f*J;
EvB 13:acd120520e80 248
EvB 13:acd120520e80 249 Matrix Jprimeprime(3,3); // Truncated Jacobian
EvB 13:acd120520e80 250 for (int i = 1 ; i <4 ; i++) {
EvB 13:acd120520e80 251 for (int j = 1 ; j < 4 ; j++) {
EvB 13:acd120520e80 252 Jprimeprime(i,j) = Jprime(i+1,j);
EvB 13:acd120520e80 253 }
EvB 13:acd120520e80 254 }
EvB 13:acd120520e80 255
EvB 13:acd120520e80 256 Matrix JprimeprimeT(3,3); // The transpose of the truncated Jacobian
EvB 13:acd120520e80 257 JprimeprimeT = MatrixMath::Transpose(Jprimeprime);
EvB 13:acd120520e80 258
EvB 13:acd120520e80 259 Matrix PseudoInverse(3,3); // The Pseudo-inverse of the truncated Jacobian
EvB 13:acd120520e80 260 PseudoInverse = MatrixMath::Inv(JprimeprimeT*Jprimeprime)*JprimeprimeT;
EvB 13:acd120520e80 261
EvB 13:acd120520e80 262 Matrix Q_dot(1,3); // Desired joint velocities
EvB 13:acd120520e80 263 Q_dot = PseudoInverse * T_des;
EvB 13:acd120520e80 264
EvB 13:acd120520e80 265 V1 = Q_dot(1,1);
EvB 13:acd120520e80 266 W2 = Q_dot(2,1);
EvB 13:acd120520e80 267 W3 = Q_dot(3,1);
EvB 13:acd120520e80 268
EvB 13:acd120520e80 269 // pc.printf("\r\nv1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
EvB 13:acd120520e80 270
EvB 13:acd120520e80 271 // Eva - not sure if we need this... --> I think for the setpoint of the PID controller
EvB 13:acd120520e80 272 Q1 += V1*T; // Predicted value for q1 [m]
EvB 13:acd120520e80 273 Q2 += W2*T; // Predicted value for q2 [rad]
EvB 13:acd120520e80 274 Q3 += W3*T; // Predicted value for q3 [rad]
EvB 13:acd120520e80 275
EvB 13:acd120520e80 276 // pc.printf("\r\n q1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1,q2,q3);
vera1 11:bda77916d2ea 277
vera1 11:bda77916d2ea 278 }
vera1 11:bda77916d2ea 279
vera1 11:bda77916d2ea 280 //--- State switch function -----
vera1 11:bda77916d2ea 281 void r_processStateSwitch()
MMartens 5:8c6d66a7c5da 282 {
EvB 13:acd120520e80 283 go_switch = false;
EvB 13:acd120520e80 284 switch(state) {
EvB 13:acd120520e80 285 case R_HORIZONTAL:
EvB 13:acd120520e80 286 state = R_VERTICAL;
EvB 13:acd120520e80 287 ledblue = 1;
EvB 13:acd120520e80 288 ledred = 1;
EvB 13:acd120520e80 289 ledgreen = 0;
EvB 13:acd120520e80 290 pc.printf("state is vertical");
EvB 13:acd120520e80 291 break;
EvB 13:acd120520e80 292 case R_VERTICAL:
EvB 13:acd120520e80 293 state = R_BASE;
EvB 13:acd120520e80 294 ledgreen = 1;
EvB 13:acd120520e80 295 ledblue = 1;
EvB 13:acd120520e80 296 ledred = 0;
EvB 13:acd120520e80 297 pc.printf("state is base");
EvB 13:acd120520e80 298 break;
EvB 13:acd120520e80 299 case R_BASE:
EvB 13:acd120520e80 300 state = R_HORIZONTAL;
EvB 13:acd120520e80 301 ledgreen = 1;
EvB 13:acd120520e80 302 ledred = 1;
EvB 13:acd120520e80 303 ledblue = 0;
EvB 13:acd120520e80 304 pc.printf("state is horizontal");
EvB 13:acd120520e80 305 break;
EvB 13:acd120520e80 306 }
EvB 13:acd120520e80 307 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 308 ledstatedef = 1;
EvB 13:acd120520e80 309 ledstateswitch = 0;
EvB 13:acd120520e80 310 go_move = true;
MMartens 5:8c6d66a7c5da 311 }
vera1 8:9edf32e021a5 312
EvB 13:acd120520e80 313 // --- Determine the setpoints of the joint velocities
EvB 13:acd120520e80 314 void setpointreadout(float v_des)
vera1 8:9edf32e021a5 315 {
EvB 13:acd120520e80 316 /*
vera1 11:bda77916d2ea 317 potvalue = pot.read();
vera1 11:bda77916d2ea 318 setpoint = emg1.normalized;
EvB 13:acd120520e80 319
vera1 11:bda77916d2ea 320 potvalue2 = pot2.read();
vera1 11:bda77916d2ea 321 setpoint2 = emg2.normalized;
EvB 13:acd120520e80 322 */
EvB 13:acd120520e80 323
EvB 13:acd120520e80 324 Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
EvB 13:acd120520e80 325 Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
EvB 13:acd120520e80 326
EvB 13:acd120520e80 327 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
EvB 13:acd120520e80 328 v_des = emg1.normalized;
EvB 13:acd120520e80 329 }
MMartens 2:7c6391c8ca71 330
EvB 13:acd120520e80 331 if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Negative direction
EvB 13:acd120520e80 332 v_des = -emg2.normalized;
EvB 13:acd120520e80 333 }
EvB 13:acd120520e80 334
EvB 13:acd120520e80 335 //pc.printf("\r\nv_des in setpoint function= %f\r\n", v_des);
EvB 13:acd120520e80 336 }
EvB 13:acd120520e80 337 void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 338 {
EvB 13:acd120520e80 339
MMartens 1:f3fe6d2b7639 340 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 341 //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 342
EvB 13:acd120520e80 343
EvB 13:acd120520e80 344 error1 = q1 - Pos1; // Position error of link 1 [m]
EvB 13:acd120520e80 345 error2 = q2 - Pos2; // Position error of link 2 [m]
EvB 13:acd120520e80 346
MMartens 3:e888f52a46bc 347 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 348 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 349 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 350 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 351 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 352 pidTerm = 1000;
MMartens 3:e888f52a46bc 353 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 354 pidTerm = -1000;
MMartens 3:e888f52a46bc 355 } else {
MMartens 3:e888f52a46bc 356 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 357 }
MMartens 3:e888f52a46bc 358 //constraining to appropriate value
EvB 13:acd120520e80 359 if (pidTerm >= 0) {
vera1 12:5be2001abe62 360 dir1 = 0;// Forward motion
MMartens 1:f3fe6d2b7639 361 } else {
vera1 12:5be2001abe62 362 dir1 = 1;//Reverse motion
MMartens 1:f3fe6d2b7639 363 }
MMartens 3:e888f52a46bc 364 pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
EvB 13:acd120520e80 365 if(pidTerm_scaled >= 0.55f) {
MMartens 3:e888f52a46bc 366 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 367 }
EvB 13:acd120520e80 368
MMartens 4:75f6e4845194 369 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 370 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 4:75f6e4845194 371 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 372 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 373 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 374 pidTerm2 = 1000;
MMartens 4:75f6e4845194 375 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 376 pidTerm2 = -1000;
MMartens 4:75f6e4845194 377 } else {
MMartens 4:75f6e4845194 378 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 379 }
MMartens 4:75f6e4845194 380 //constraining to appropriate value
EvB 13:acd120520e80 381 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 382 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 383 } else {
MMartens 4:75f6e4845194 384 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 385 }
MMartens 4:75f6e4845194 386 pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
EvB 13:acd120520e80 387 if(pidTerm_scaled2 >= 0.55f) {
MMartens 4:75f6e4845194 388 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 389 }
EvB 13:acd120520e80 390
MMartens 1:f3fe6d2b7639 391 last_error = error1;
MMartens 3:e888f52a46bc 392 speed1 = pidTerm_scaled+0.45f;
vera1 12:5be2001abe62 393 //speedservo1 = speed1;
MMartens 4:75f6e4845194 394 last_error2 = error2;
MMartens 4:75f6e4845194 395 speed2 = pidTerm_scaled2+0.45f;
vera1 12:5be2001abe62 396 //speedservo2 = speed2;
MMartens 0:9167ae5d9927 397 }
MMartens 0:9167ae5d9927 398
EvB 13:acd120520e80 399 // --- Motor movements ---
EvB 13:acd120520e80 400 void r_movehorizontal()
EvB 13:acd120520e80 401 {
EvB 13:acd120520e80 402 setpointreadout(Vx_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 403 //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
EvB 13:acd120520e80 404 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 405 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 406
EvB 13:acd120520e80 407 }
EvB 13:acd120520e80 408
EvB 13:acd120520e80 409 void r_movevertical()
EvB 13:acd120520e80 410 {
EvB 13:acd120520e80 411 setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 412 //pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
EvB 13:acd120520e80 413 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 414 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 415
EvB 13:acd120520e80 416 }
EvB 13:acd120520e80 417
EvB 13:acd120520e80 418 void r_movebase()
EvB 13:acd120520e80 419 {
EvB 13:acd120520e80 420
EvB 13:acd120520e80 421 setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
EvB 13:acd120520e80 422 //pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
EvB 13:acd120520e80 423 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 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 13:acd120520e80 425
EvB 13:acd120520e80 426 }
EvB 13:acd120520e80 427 //--------------------------------
EvB 13:acd120520e80 428
EvB 13:acd120520e80 429
vera1 8:9edf32e021a5 430 void maintickerfunction()
EvB 13:acd120520e80 431 {
EvB 13:acd120520e80 432 count++;
EvB 13:acd120520e80 433 if (go_switch) {
EvB 13:acd120520e80 434 r_processStateSwitch();
EvB 13:acd120520e80 435 }
EvB 13:acd120520e80 436
EvB 13:acd120520e80 437 if(go_EMG && count%2 == 0) {
vera1 8:9edf32e021a5 438 processEMG();
vera1 8:9edf32e021a5 439 }
EvB 13:acd120520e80 440
EvB 13:acd120520e80 441 if (go_move) {
EvB 13:acd120520e80 442 switch(state) {
EvB 13:acd120520e80 443 case R_HORIZONTAL:
EvB 13:acd120520e80 444 r_movehorizontal();
EvB 13:acd120520e80 445 break;
EvB 13:acd120520e80 446 case R_VERTICAL:
EvB 13:acd120520e80 447 r_movevertical();
EvB 13:acd120520e80 448 break;
EvB 13:acd120520e80 449 case R_BASE:
EvB 13:acd120520e80 450 r_movebase();
EvB 13:acd120520e80 451 break;
EvB 13:acd120520e80 452 }
EvB 13:acd120520e80 453 }
EvB 13:acd120520e80 454
EvB 13:acd120520e80 455 /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen.
EvB 13:acd120520e80 456 {
vera1 11:bda77916d2ea 457 go_PID = false;
vera1 11:bda77916d2ea 458 }
vera1 11:bda77916d2ea 459 else{go_PID = true;}*/
EvB 13:acd120520e80 460
EvB 13:acd120520e80 461
vera1 8:9edf32e021a5 462 }
vera1 8:9edf32e021a5 463
MMartens 1:f3fe6d2b7639 464 int main()
EvB 13:acd120520e80 465 {
EvB 13:acd120520e80 466 pc.baud(9600);
EvB 13:acd120520e80 467 go_EMG = true; // Setting ticker variables
vera1 8:9edf32e021a5 468 go_calibration = true; // Setting the timeout variable
vera1 11:bda77916d2ea 469 go_PID = true;
vera1 11:bda77916d2ea 470 go_switch = false;
EvB 13:acd120520e80 471 go_move = true;
MMartens 1:f3fe6d2b7639 472 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 473 speed2.period(PwmPeriod);
EvB 13:acd120520e80 474
vera1 8:9edf32e021a5 475 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 8:9edf32e021a5 476 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
EvB 13:acd120520e80 477 wait(5.0f);
EvB 13:acd120520e80 478 mainticker.attach(&maintickerfunction,0.001f);
EvB 13:acd120520e80 479
MMartens 1:f3fe6d2b7639 480 while(true) {
EvB 13:acd120520e80 481
vera1 11:bda77916d2ea 482 ledstatedef = 1;
vera1 11:bda77916d2ea 483 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 11:bda77916d2ea 484 ledstateswitch = 1;
vera1 11:bda77916d2ea 485 ledstatedef = 0;
vera1 11:bda77916d2ea 486 go_switch = true;
EvB 13:acd120520e80 487 go_move = false;
EvB 13:acd120520e80 488
vera1 11:bda77916d2ea 489 }
EvB 13:acd120520e80 490
MMartens 3:e888f52a46bc 491
vera1 11:bda77916d2ea 492
EvB 13:acd120520e80 493 if(count == 100) {
MMartens 3:e888f52a46bc 494 count = 0;
vera1 11:bda77916d2ea 495 pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 11:bda77916d2ea 496 //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
vera1 11:bda77916d2ea 497 //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 498 }
EvB 13:acd120520e80 499
EvB 13:acd120520e80 500
EvB 13:acd120520e80 501 wait(0.001f);
vera1 6:fc46581fe3e0 502 }
MMartens 1:f3fe6d2b7639 503
MMartens 0:9167ae5d9927 504 }
MMartens 0:9167ae5d9927 505
MMartens 0:9167ae5d9927 506