Biorobotics / Robot-Software

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

main.cpp@46:9b60a3c1acab, 2018-11-01 (annotated)

Committer:
Spekdon
Date:
Thu Nov 01 20:54:51 2018 +0000
Revision:
46:9b60a3c1acab
Parent:
45:829a3992d689 
Presentation code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2 #include "MODSERIAL.h"
MaikOvermars 0:4cb1de41d049 3 #include "QEI.h"
MaikOvermars 0:4cb1de41d049 4 #include "HIDScope.h"
MaikOvermars 0:4cb1de41d049 5 #include "BiQuad.h"
MaikOvermars 0:4cb1de41d049 6 #include "PID_controller.h"
Spekdon 45:829a3992d689 7 //#include "kinematics.h"
Spekdon 45:829a3992d689 8 #include "Servo.h"
MaikOvermars 17:1f93c83e211f 9 #include "processing_chain_emg.h"
Spekdon 43:8e2ea92fee01 10 #include <vector>
Spekdon 43:8e2ea92fee01 11
Spekdon 45:829a3992d689 12 //pc
Spekdon 45:829a3992d689 13 MODSERIAL pc(USBTX, USBRX);
MaikOvermars 0:4cb1de41d049 14
MaikOvermars 10:7339dca7d604 15 // emg inputs
MaikOvermars 0:4cb1de41d049 16 AnalogIn emg0( A0 );
MaikOvermars 0:4cb1de41d049 17 AnalogIn emg1( A1 );
Spekdon 45:829a3992d689 18 AnalogIn emg2( A2 );
Spekdon 45:829a3992d689 19 AnalogIn emg3( A3 );
Spekdon 46:9b60a3c1acab 20 Servo gripper(A5);
Spekdon 45:829a3992d689 21 //InterruptIn button1(SW2);
Spekdon 46:9b60a3c1acab 22 DigitalIn buttongripper(D1);
Spekdon 45:829a3992d689 23 double range;
MaikOvermars 0:4cb1de41d049 24
MaikOvermars 10:7339dca7d604 25 // motor ouptuts
MaikOvermars 0:4cb1de41d049 26 PwmOut motor1_pwm(D5);
MaikOvermars 0:4cb1de41d049 27 DigitalOut motor1_dir(D4);
bjonkheer 38:0f824c45f717 28 PwmOut motor2_pwm(D6);
bjonkheer 38:0f824c45f717 29 DigitalOut motor2_dir(D7);
Spekdon 45:829a3992d689 30 AnalogIn vx_adjustment(A4);
Spekdon 45:829a3992d689 31 AnalogIn vy_adjustment(A5);
MaikOvermars 0:4cb1de41d049 32
SvenD97 14:4744cc6c90f4 33 // defining encoders
MaikOvermars 16:0280a604cf7e 34 QEI motor_1_encoder(D12,D13,NC,32);
MaikOvermars 16:0280a604cf7e 35 QEI motor_2_encoder(D10,D11,NC,32);
SvenD97 14:4744cc6c90f4 36
MaikOvermars 17:1f93c83e211f 37 // other objects
MaikOvermars 0:4cb1de41d049 38 DigitalIn button(D0);
MaikOvermars 40:e217056584be 39 DigitalIn emgstop(SW2);
SvenD97 31:393a7ec1d396 40 DigitalOut led_R(LED_RED);
SvenD97 31:393a7ec1d396 41 DigitalOut led_B(LED_BLUE);
SvenD97 31:393a7ec1d396 42 DigitalOut led_G(LED_GREEN);
MaikOvermars 0:4cb1de41d049 43
SvenD97 14:4744cc6c90f4 44 // tickers and timers
MaikOvermars 16:0280a604cf7e 45 Ticker loop_ticker;
MaikOvermars 0:4cb1de41d049 46 Timer state_timer;
SvenD97 27:eee900e0a47d 47 Timer emg_timer;
MaikOvermars 0:4cb1de41d049 48
SvenD97 34:7d672cd04486 49 // Timeouts and related variables
SvenD97 34:7d672cd04486 50 Timeout make_button_active;
SvenD97 34:7d672cd04486 51 bool button_suppressed = false;
Spekdon 46:9b60a3c1acab 52 bool gripperopen = true;
SvenD97 34:7d672cd04486 53
SvenD97 34:7d672cd04486 54 // States
SvenD97 31:393a7ec1d396 55 enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
SvenD97 27:eee900e0a47d 56 enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
SvenD97 37:5ddfd9e6cdb2 57 enum calib_enc_states {not_in_calib_enc, calib_enc_q1, calib_enc_q2};
MaikOvermars 0:4cb1de41d049 58
MaikOvermars 0:4cb1de41d049 59 //Global variables/objects
MaikOvermars 0:4cb1de41d049 60 States current_state;
SvenD97 31:393a7ec1d396 61 Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
SvenD97 37:5ddfd9e6cdb2 62 calib_enc_states calib_enc_state = not_in_calib_enc;
MaikOvermars 16:0280a604cf7e 63
Spekdon 45:829a3992d689 64 double des_vx, des_vy, x, y, q1, q2, e1, e2, x_norm, y_norm; //will be set by the motor_controller function
bjonkheer 38:0f824c45f717 65 double u1, u2;
MaikOvermars 17:1f93c83e211f 66 double vxmax = 1.0, vymax = 1.0;
SvenD97 27:eee900e0a47d 67 double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
MaikOvermars 40:e217056584be 68 double scaling_right_bicep = 1.0, scaling_right_tricep = 1.0, scaling_left_bicep = 1.0, scaling_left_tricep = 1.0;
SvenD97 27:eee900e0a47d 69
SvenD97 31:393a7ec1d396 70 // Variables for emg
SvenD97 31:393a7ec1d396 71 double raw_emg_0, process_emg_0;
SvenD97 31:393a7ec1d396 72 double raw_emg_1, process_emg_1;
SvenD97 31:393a7ec1d396 73 double raw_emg_2, process_emg_2;
SvenD97 31:393a7ec1d396 74 double raw_emg_3, process_emg_3;
SvenD97 31:393a7ec1d396 75
SvenD97 31:393a7ec1d396 76
SvenD97 33:eb77ed8d167c 77 // Variables for calibration
SvenD97 35:6110d0b5513b 78 double calib_q1 = 3.1415926535f;
SvenD97 37:5ddfd9e6cdb2 79 double calib_q2 = double(7)/double(6) * 3.1415926535f;
Spekdon 45:829a3992d689 80 double off_set_q1 = 0.5f*3.1415926535f; // This variable is used to determine the off set from our definition from q1 and q2.
Spekdon 45:829a3992d689 81 double off_set_q2 = 3.1415926535f;
SvenD97 33:eb77ed8d167c 82
SvenD97 30:7a66951a0122 83 // Variables defined for the homing state
SvenD97 35:6110d0b5513b 84 double q1_homing = 0.5f*3.1415926535f, q2_homing = 3.1415926535f;
Spekdon 45:829a3992d689 85 double qref1 = 0.5f*3.1415926535f, qref2 = 3.1415926535f;
Spekdon 45:829a3992d689 86 double beta = 0.05;
Spekdon 45:829a3992d689 87 double k_hom = 0.05;
SvenD97 31:393a7ec1d396 88
SvenD97 31:393a7ec1d396 89 // For the state calib_enc
SvenD97 31:393a7ec1d396 90 double q1old;
SvenD97 31:393a7ec1d396 91 double q2old;
SvenD97 30:7a66951a0122 92
Spekdon 43:8e2ea92fee01 93 vector<double> currentconfig;
Spekdon 43:8e2ea92fee01 94 vector<double> newconfig;
Spekdon 43:8e2ea92fee01 95 vector<double> ref;
Spekdon 45:829a3992d689 96 vector<double> testconfig;
Spekdon 43:8e2ea92fee01 97
Spekdon 43:8e2ea92fee01 98 double currentx;
Spekdon 43:8e2ea92fee01 99 double currenty;
Spekdon 43:8e2ea92fee01 100
Spekdon 43:8e2ea92fee01 101 double L1 = 0.4388;
Spekdon 43:8e2ea92fee01 102 double L2 = 0.4508;
Spekdon 43:8e2ea92fee01 103
SvenD97 27:eee900e0a47d 104 // Meaning of process_emg_0 and such
SvenD97 27:eee900e0a47d 105 // - process_emg_0 is right biceps
SvenD97 27:eee900e0a47d 106 // - process_emg_1 is right triceps
SvenD97 27:eee900e0a47d 107 // - process_emg_2 is left biceps
SvenD97 27:eee900e0a47d 108 // - process_emg_3 is left triceps
bjonkheer 23:7d83af123c43 109
MaikOvermars 0:4cb1de41d049 110 int counts_per_rotation = 32;
MaikOvermars 0:4cb1de41d049 111 bool state_changed = false;
SvenD97 8:bba05e863b68 112 const double T = 0.001;
bjonkheer 23:7d83af123c43 113
Spekdon 45:829a3992d689 114 bool gripperclosed = false;
Spekdon 45:829a3992d689 115
SvenD97 30:7a66951a0122 116 // Resolution of the encoder at the output axle
SvenD97 30:7a66951a0122 117 double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians
SvenD97 30:7a66951a0122 118
MaikOvermars 17:1f93c83e211f 119 // Functions
Spekdon 43:8e2ea92fee01 120
Spekdon 43:8e2ea92fee01 121 vector<double> forwardkinematics_function(double& q1, double& q2) {
Spekdon 43:8e2ea92fee01 122 // input are joint angles, output are x and y position of end effector
Spekdon 43:8e2ea92fee01 123
Spekdon 43:8e2ea92fee01 124 //previously +x01 and +y01
Spekdon 43:8e2ea92fee01 125 currentx = L1*cos(q1)-L2*cos(q2);
Spekdon 43:8e2ea92fee01 126 currenty = L1 * sin(q1) - L2 * sin(q2);
Spekdon 43:8e2ea92fee01 127
Spekdon 43:8e2ea92fee01 128 vector<double> xy;
Spekdon 43:8e2ea92fee01 129 xy.push_back(currentx);
Spekdon 43:8e2ea92fee01 130 xy.push_back(currenty);
Spekdon 43:8e2ea92fee01 131 return xy;
Spekdon 43:8e2ea92fee01 132
Spekdon 43:8e2ea92fee01 133 }
Spekdon 43:8e2ea92fee01 134
Spekdon 45:829a3992d689 135 vector<double> inversekinematics_function(const double& T, double ref1, double ref2, double& q1, double& q2, double& des_vx, double& des_vy) {
Spekdon 43:8e2ea92fee01 136 // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
Spekdon 43:8e2ea92fee01 137
Spekdon 43:8e2ea92fee01 138 // pseudo inverse jacobian to get joint speeds
Spekdon 43:8e2ea92fee01 139 // input are desired vx and vy of end effector, output joint angle speeds
Spekdon 43:8e2ea92fee01 140
Spekdon 43:8e2ea92fee01 141 double q1_star_des; // desired joint velocity of q1_star
Spekdon 43:8e2ea92fee01 142 double q2_star_des; // same as above but then for q2_star
Spekdon 43:8e2ea92fee01 143 double C;
Spekdon 43:8e2ea92fee01 144
Spekdon 43:8e2ea92fee01 145 // The calculation below assumes that the end effector position is calculated before this function is executed
Spekdon 43:8e2ea92fee01 146 // In our case the determinant will not equal zero, hence no problems with singularies I think.
Spekdon 43:8e2ea92fee01 147
Spekdon 43:8e2ea92fee01 148 C = L1*L2*sin(ref1)*cos(ref2) - L1*L2*sin(ref2)*cos(ref1);
Spekdon 45:829a3992d689 149 q1_star_des = double(1/C)*(-L2*cos(ref2)*des_vx - L2*sin(ref2)*des_vy);
Spekdon 45:829a3992d689 150 q2_star_des = double(1/C)*((L2*cos(ref2)-L1*cos(ref1))*des_vx + (L2*sin(ref2)-L1*sin(ref1))*des_vy);
Spekdon 43:8e2ea92fee01 151
Spekdon 43:8e2ea92fee01 152 qref1 = T*q1_star_des;
Spekdon 43:8e2ea92fee01 153 qref2 = T*(q2_star_des+q1_star_des);
Spekdon 43:8e2ea92fee01 154
Spekdon 43:8e2ea92fee01 155 double testq1 = ref1+qref1;
Spekdon 43:8e2ea92fee01 156 double testq2 = ref2+qref2;
Spekdon 43:8e2ea92fee01 157 newconfig = forwardkinematics_function(testq1, testq2);
Spekdon 43:8e2ea92fee01 158
Spekdon 45:829a3992d689 159 if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 1)
Spekdon 43:8e2ea92fee01 160 {
Spekdon 43:8e2ea92fee01 161 qref1 = ref1;
Spekdon 43:8e2ea92fee01 162 qref2 = ref2;
Spekdon 43:8e2ea92fee01 163 }
Spekdon 43:8e2ea92fee01 164 else
Spekdon 43:8e2ea92fee01 165 {
Spekdon 43:8e2ea92fee01 166 qref1 = ref1 + qref1;
Spekdon 43:8e2ea92fee01 167 qref2 = ref2 + qref2;
Spekdon 43:8e2ea92fee01 168 }
Spekdon 43:8e2ea92fee01 169
Spekdon 43:8e2ea92fee01 170 vector<double> thetas;
Spekdon 43:8e2ea92fee01 171 thetas.push_back(qref1);
Spekdon 43:8e2ea92fee01 172 thetas.push_back(qref2);
Spekdon 43:8e2ea92fee01 173 return thetas;
Spekdon 43:8e2ea92fee01 174 }
Spekdon 43:8e2ea92fee01 175
MaikOvermars 0:4cb1de41d049 176 void measure_all()
MaikOvermars 0:4cb1de41d049 177 {
Spekdon 45:829a3992d689 178 q1 = (motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
Spekdon 45:829a3992d689 179 q2 = (motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q2;
Spekdon 43:8e2ea92fee01 180 currentconfig = forwardkinematics_function(q1,q2); //previously x,y also input
MaikOvermars 17:1f93c83e211f 181 raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
MaikOvermars 17:1f93c83e211f 182 raw_emg_1 = emg1.read();
Spekdon 45:829a3992d689 183 raw_emg_2 = emg2.read();
Spekdon 45:829a3992d689 184 raw_emg_3 = emg3.read();
MaikOvermars 41:e9d6fdf02074 185 processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3); // processes the emg signals
MaikOvermars 0:4cb1de41d049 186 }
MaikOvermars 0:4cb1de41d049 187
MaikOvermars 0:4cb1de41d049 188 void output_all() {
Spekdon 46:9b60a3c1acab 189 motor1_pwm = 0.4+0.6*fabs(u1);
Spekdon 46:9b60a3c1acab 190
Spekdon 45:829a3992d689 191 if (motor1_pwm > 1.0f){
Spekdon 45:829a3992d689 192 motor1_pwm = 1.0f;
Spekdon 45:829a3992d689 193 }
MaikOvermars 16:0280a604cf7e 194 motor1_dir = u1 > 0.0f;
Spekdon 46:9b60a3c1acab 195 motor2_pwm = 0.4+0.6*fabs(u2);
Spekdon 45:829a3992d689 196 if (motor2_pwm > 1.0f){
Spekdon 45:829a3992d689 197 motor2_pwm = 1.0f;
Spekdon 45:829a3992d689 198 }
MaikOvermars 16:0280a604cf7e 199 motor2_dir = u2 > 0.0f;
MaikOvermars 0:4cb1de41d049 200 static int output_counter = 0;
MaikOvermars 0:4cb1de41d049 201 output_counter++;
MaikOvermars 0:4cb1de41d049 202 }
MaikOvermars 0:4cb1de41d049 203
SvenD97 34:7d672cd04486 204 void unsuppressing_button(){
SvenD97 34:7d672cd04486 205 button_suppressed = false;
SvenD97 34:7d672cd04486 206 }
SvenD97 34:7d672cd04486 207
MaikOvermars 0:4cb1de41d049 208 void state_machine() {
MaikOvermars 0:4cb1de41d049 209 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
MaikOvermars 0:4cb1de41d049 210 case waiting: //Nothing useful here, maybe a blinking LED for fun and communication to the user
SvenD97 37:5ddfd9e6cdb2 211 if (button.read()==false)
MaikOvermars 0:4cb1de41d049 212 {
Spekdon 45:829a3992d689 213 current_state = calib_emg; //the NEXT loop we will be in calib_enc state
Spekdon 45:829a3992d689 214 current_emg_calibration_state = calib_right_bicep;
SvenD97 37:5ddfd9e6cdb2 215 calib_enc_state = calib_enc_q2;
SvenD97 31:393a7ec1d396 216 state_changed = true;
MaikOvermars 0:4cb1de41d049 217 }
bjonkheer 29:d1e8eb135e6c 218
MaikOvermars 0:4cb1de41d049 219 break; //to avoid falling through to the next state, although this can sometimes be very useful.
MaikOvermars 0:4cb1de41d049 220
MaikOvermars 0:4cb1de41d049 221 case calib_enc:
bjonkheer 29:d1e8eb135e6c 222
MaikOvermars 0:4cb1de41d049 223 if (state_changed==true)
MaikOvermars 0:4cb1de41d049 224 {
MaikOvermars 0:4cb1de41d049 225 state_timer.reset();
MaikOvermars 0:4cb1de41d049 226 state_timer.start();
MaikOvermars 0:4cb1de41d049 227 state_changed = false;
bjonkheer 29:d1e8eb135e6c 228 led_G = 0;
bjonkheer 29:d1e8eb135e6c 229 led_B = 1;
bjonkheer 29:d1e8eb135e6c 230 led_R = 1;
SvenD97 44:25eec278c623 231 //u1 = 0.2;
SvenD97 44:25eec278c623 232 //u2 = -0.65f;
bjonkheer 29:d1e8eb135e6c 233 q1old = 0;
bjonkheer 29:d1e8eb135e6c 234 q2old = 0;
MaikOvermars 0:4cb1de41d049 235 }
bjonkheer 29:d1e8eb135e6c 236
SvenD97 37:5ddfd9e6cdb2 237 switch(calib_enc_state){
SvenD97 37:5ddfd9e6cdb2 238 case calib_enc_q2:
SvenD97 37:5ddfd9e6cdb2 239 if (q2 - q2old == 0.0 && state_timer.read() > 5.0f)
SvenD97 37:5ddfd9e6cdb2 240 {
SvenD97 37:5ddfd9e6cdb2 241 calib_enc_state = calib_enc_q1;
SvenD97 37:5ddfd9e6cdb2 242 off_set_q2 = calib_q2 - q2;
SvenD97 44:25eec278c623 243 //u2 = -0.4;
SvenD97 44:25eec278c623 244 //u1 = -0.75;
SvenD97 37:5ddfd9e6cdb2 245 state_timer.reset();
SvenD97 37:5ddfd9e6cdb2 246 state_timer.start();
SvenD97 37:5ddfd9e6cdb2 247 }
SvenD97 37:5ddfd9e6cdb2 248 q2old = q2;
SvenD97 37:5ddfd9e6cdb2 249 break;
SvenD97 37:5ddfd9e6cdb2 250 case calib_enc_q1:
SvenD97 37:5ddfd9e6cdb2 251 if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
SvenD97 37:5ddfd9e6cdb2 252 {
SvenD97 37:5ddfd9e6cdb2 253 calib_enc_state = not_in_calib_enc;
SvenD97 37:5ddfd9e6cdb2 254 current_emg_calibration_state = calib_right_bicep;
Spekdon 43:8e2ea92fee01 255 current_state = calib_emg;
SvenD97 37:5ddfd9e6cdb2 256 state_changed = true;
SvenD97 37:5ddfd9e6cdb2 257 off_set_q1 = calib_q1 - q1;
SvenD97 44:25eec278c623 258 //u1 = 0;
SvenD97 37:5ddfd9e6cdb2 259 state_timer.reset();
SvenD97 37:5ddfd9e6cdb2 260 state_timer.start();
SvenD97 37:5ddfd9e6cdb2 261 }
bjonkheer 38:0f824c45f717 262 q1old = q1;
SvenD97 37:5ddfd9e6cdb2 263 break;
SvenD97 37:5ddfd9e6cdb2 264 default:
SvenD97 37:5ddfd9e6cdb2 265 current_state = failure;
MaikOvermars 0:4cb1de41d049 266 }
MaikOvermars 0:4cb1de41d049 267 break;
MaikOvermars 0:4cb1de41d049 268
MaikOvermars 0:4cb1de41d049 269 case calib_emg: //calibrate emg-signals
SvenD97 27:eee900e0a47d 270 if (state_changed == true){
SvenD97 27:eee900e0a47d 271 state_timer.reset();
SvenD97 27:eee900e0a47d 272 state_timer.start();
SvenD97 27:eee900e0a47d 273 emg_timer.reset();
SvenD97 27:eee900e0a47d 274 emg_timer.start();
SvenD97 27:eee900e0a47d 275 state_changed = false;
SvenD97 27:eee900e0a47d 276 }
MaikOvermars 42:bcd496523c66 277 // first the led will be purple, during this time the right biceps should be maximally contracted
MaikOvermars 42:bcd496523c66 278 // after five seconds and when the right biceps are relaxed, it switches to a blue led
MaikOvermars 42:bcd496523c66 279 // when the blue led is active the right triceps should be maximally contracted
MaikOvermars 42:bcd496523c66 280 // then again it switches to purple for left biceps and after that blue for left triceps
MaikOvermars 42:bcd496523c66 281 // after this, the emg signals are calibrated
SvenD97 31:393a7ec1d396 282 switch(current_emg_calibration_state){
SvenD97 27:eee900e0a47d 283 case calib_right_bicep:
MaikOvermars 40:e217056584be 284 led_R = 0;
MaikOvermars 40:e217056584be 285 led_G = 1;
MaikOvermars 42:bcd496523c66 286 led_B = 0;
SvenD97 27:eee900e0a47d 287 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 288 if (process_emg_0 > right_bicep_max){
SvenD97 27:eee900e0a47d 289 right_bicep_max = process_emg_0;
SvenD97 27:eee900e0a47d 290 }
SvenD97 27:eee900e0a47d 291 }
MaikOvermars 40:e217056584be 292 else if ((process_emg_0 < 0.1*right_bicep_max) || (emgstop.read() == false)){
MaikOvermars 40:e217056584be 293 scaling_right_bicep = 1.0/ right_bicep_max;
SvenD97 27:eee900e0a47d 294 current_emg_calibration_state = calib_right_tricep;
SvenD97 27:eee900e0a47d 295 emg_timer.reset();
SvenD97 27:eee900e0a47d 296 emg_timer.start();
SvenD97 27:eee900e0a47d 297 }
SvenD97 27:eee900e0a47d 298 break;
SvenD97 27:eee900e0a47d 299 case calib_right_tricep:
MaikOvermars 40:e217056584be 300 led_R = 1;
MaikOvermars 40:e217056584be 301 led_G = 1;
MaikOvermars 40:e217056584be 302 led_B = 0;
SvenD97 27:eee900e0a47d 303 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 304 if (process_emg_1 > right_tricep_max){
SvenD97 27:eee900e0a47d 305 right_tricep_max = process_emg_1;
SvenD97 27:eee900e0a47d 306 }
SvenD97 27:eee900e0a47d 307 }
MaikOvermars 40:e217056584be 308 else if ((process_emg_1 < 0.1*right_tricep_max) || (emgstop.read() == false)){
MaikOvermars 40:e217056584be 309 scaling_right_tricep = 1.0/ right_tricep_max;
SvenD97 30:7a66951a0122 310 current_emg_calibration_state = calib_left_bicep;
SvenD97 30:7a66951a0122 311 emg_timer.reset();
SvenD97 30:7a66951a0122 312 emg_timer.start();
SvenD97 30:7a66951a0122 313 }
SvenD97 27:eee900e0a47d 314 break;
MaikOvermars 40:e217056584be 315 case calib_left_bicep:
MaikOvermars 40:e217056584be 316 led_R = 0;
MaikOvermars 40:e217056584be 317 led_G = 1;
MaikOvermars 42:bcd496523c66 318 led_B = 0;
SvenD97 30:7a66951a0122 319 if(emg_timer < 5.0f){
SvenD97 30:7a66951a0122 320 if (process_emg_2 > left_bicep_max){
SvenD97 30:7a66951a0122 321 left_bicep_max = process_emg_2;
SvenD97 27:eee900e0a47d 322 }
SvenD97 30:7a66951a0122 323 }
MaikOvermars 40:e217056584be 324 else if ((process_emg_2 < 0.1*left_bicep_max) || (emgstop.read() == false)){
MaikOvermars 40:e217056584be 325 scaling_left_bicep = 1.0/ left_bicep_max;
SvenD97 30:7a66951a0122 326 current_emg_calibration_state = calib_left_tricep;
SvenD97 30:7a66951a0122 327 emg_timer.reset();
SvenD97 30:7a66951a0122 328 emg_timer.start();
SvenD97 30:7a66951a0122 329 }
SvenD97 27:eee900e0a47d 330 break;
SvenD97 27:eee900e0a47d 331 case calib_left_tricep:
MaikOvermars 40:e217056584be 332 led_R = 1;
MaikOvermars 40:e217056584be 333 led_G = 1;
MaikOvermars 40:e217056584be 334 led_B = 0;
MaikOvermars 40:e217056584be 335 if(emg_timer < 5.0f){
MaikOvermars 40:e217056584be 336 if (process_emg_3 > left_tricep_max){
MaikOvermars 40:e217056584be 337 left_tricep_max = process_emg_3;
MaikOvermars 40:e217056584be 338 }
MaikOvermars 40:e217056584be 339 }
MaikOvermars 40:e217056584be 340 else if ((process_emg_3 < 0.1*left_tricep_max) || (emgstop.read() == false)){
MaikOvermars 40:e217056584be 341 scaling_left_tricep = 1.0/ left_tricep_max;
MaikOvermars 40:e217056584be 342 current_emg_calibration_state = not_in_calib_emg;
MaikOvermars 42:bcd496523c66 343 current_state = operational;
MaikOvermars 40:e217056584be 344 state_changed = true;
MaikOvermars 40:e217056584be 345 emg_timer.reset();
MaikOvermars 42:bcd496523c66 346 led_R = 1;
MaikOvermars 42:bcd496523c66 347 led_G = 1;
MaikOvermars 42:bcd496523c66 348 led_B = 1;
MaikOvermars 40:e217056584be 349 }
SvenD97 27:eee900e0a47d 350 break;
SvenD97 27:eee900e0a47d 351 default:
SvenD97 27:eee900e0a47d 352 current_state = failure;
SvenD97 27:eee900e0a47d 353 state_changed = true;
SvenD97 31:393a7ec1d396 354 }
SvenD97 31:393a7ec1d396 355
SvenD97 31:393a7ec1d396 356 break;
SvenD97 27:eee900e0a47d 357
SvenD97 30:7a66951a0122 358 case homing:
SvenD97 30:7a66951a0122 359 if (state_changed == true){
SvenD97 30:7a66951a0122 360 state_timer.reset();
SvenD97 30:7a66951a0122 361 state_timer.start();
SvenD97 30:7a66951a0122 362 qref1 = q1; //NOT SURE IF WE NEED THIS. I do not think so, but just to be sure.
SvenD97 30:7a66951a0122 363 qref2 = q2;
SvenD97 30:7a66951a0122 364 }
SvenD97 33:eb77ed8d167c 365 des_vx = min(beta, k_hom*(q1 - q1_homing)); // Little bit different then that Arvid told us, but now it works with the motor controller
SvenD97 33:eb77ed8d167c 366 des_vy = min(beta, k_hom*(q2 - q2_homing));
SvenD97 30:7a66951a0122 367
SvenD97 31:393a7ec1d396 368 // The value of 3.0 and 2*resolution can be changed
SvenD97 31:393a7ec1d396 369 if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
SvenD97 31:393a7ec1d396 370 if (state_timer > 3.0f){
SvenD97 31:393a7ec1d396 371 current_state = operational;
SvenD97 31:393a7ec1d396 372 state_changed = true;
SvenD97 33:eb77ed8d167c 373 des_vx = 0; // Not sure if needed but added it anyways.
SvenD97 33:eb77ed8d167c 374 des_vy = 0;
SvenD97 30:7a66951a0122 375 }
SvenD97 30:7a66951a0122 376 }
SvenD97 31:393a7ec1d396 377 else{
SvenD97 31:393a7ec1d396 378 state_timer.reset();
SvenD97 31:393a7ec1d396 379 }
SvenD97 31:393a7ec1d396 380 break;
MaikOvermars 0:4cb1de41d049 381
MaikOvermars 0:4cb1de41d049 382 case operational: //interpreting emg-signals to move the end effector
SvenD97 30:7a66951a0122 383 if (state_changed == true){
SvenD97 30:7a66951a0122 384 state_changed = false;
MaikOvermars 39:5db36ce5e620 385 }
MaikOvermars 40:e217056584be 386
MaikOvermars 40:e217056584be 387 // normalization
Spekdon 45:829a3992d689 388 x_norm = process_emg_0 * scaling_right_bicep - process_emg_1 * scaling_right_tricep;
Spekdon 45:829a3992d689 389 y_norm = process_emg_2 * scaling_left_bicep - process_emg_3 * scaling_left_tricep;
Spekdon 45:829a3992d689 390
Spekdon 45:829a3992d689 391 //x_norm = -1+2*vx_adjustment.read();
Spekdon 45:829a3992d689 392 //y_norm = -1+2*vy_adjustment.read();
MaikOvermars 40:e217056584be 393
MaikOvermars 17:1f93c83e211f 394 // here we have to determine the desired velocity based on the processed emg signals and calibration
MaikOvermars 40:e217056584be 395 // x velocity
Spekdon 43:8e2ea92fee01 396 if (x_norm >= 0.8) { des_vx = 0.2; }
Spekdon 43:8e2ea92fee01 397 else if(x_norm >= 0.6) { des_vx = 0.15; }
Spekdon 43:8e2ea92fee01 398 else if(x_norm >= 0.4) { des_vx = 0.1; }
Spekdon 43:8e2ea92fee01 399 else if(x_norm >= 0.2) { des_vx = 0.05; }
Spekdon 43:8e2ea92fee01 400 else if(x_norm <= -0.8) { des_vx = -0.2; }
Spekdon 43:8e2ea92fee01 401 else if(x_norm <= -0.6) { des_vx = -0.15; }
Spekdon 43:8e2ea92fee01 402 else if(x_norm <= -0.4) { des_vx = -0.1; }
Spekdon 43:8e2ea92fee01 403 else if(x_norm <= -0.2) { des_vx = -0.05; }
MaikOvermars 17:1f93c83e211f 404 else { des_vx = 0; }
MaikOvermars 17:1f93c83e211f 405
MaikOvermars 40:e217056584be 406 // y velocity
Spekdon 43:8e2ea92fee01 407 if (y_norm >= 0.8) { des_vy = 0.2; }
Spekdon 43:8e2ea92fee01 408 else if(y_norm >= 0.6) { des_vy = 0.15; }
Spekdon 43:8e2ea92fee01 409 else if(y_norm >= 0.4) { des_vy = 0.1; }
Spekdon 43:8e2ea92fee01 410 else if(y_norm >= 0.2) { des_vy = 0.05; }
Spekdon 43:8e2ea92fee01 411 else if(y_norm <= -0.8) { des_vy = -0.2; }
Spekdon 43:8e2ea92fee01 412 else if(y_norm <= -0.6) { des_vy = -0.15; }
Spekdon 43:8e2ea92fee01 413 else if(y_norm <= -0.4) { des_vy = -0.1; }
Spekdon 43:8e2ea92fee01 414 else if(y_norm <= -0.2) { des_vy = -0.05; }
MaikOvermars 17:1f93c83e211f 415 else { des_vy = 0; }
Spekdon 45:829a3992d689 416
Spekdon 45:829a3992d689 417 if (button.read() == false && button_suppressed == false ) {
SvenD97 30:7a66951a0122 418 current_state = demo;
SvenD97 30:7a66951a0122 419 state_changed = true;
SvenD97 34:7d672cd04486 420 button_suppressed = true;
SvenD97 34:7d672cd04486 421
SvenD97 34:7d672cd04486 422 make_button_active.attach(&unsuppressing_button,0.5);
SvenD97 34:7d672cd04486 423
MaikOvermars 39:5db36ce5e620 424 }
Spekdon 46:9b60a3c1acab 425
Spekdon 46:9b60a3c1acab 426 if (buttongripper.read() == false && button_suppressed == false ) {
Spekdon 46:9b60a3c1acab 427 button_suppressed = true;
Spekdon 46:9b60a3c1acab 428 if (gripperopen){gripper = 0; gripperopen = false;}
Spekdon 46:9b60a3c1acab 429 else {gripper = 1; gripperopen = true;}
Spekdon 46:9b60a3c1acab 430
Spekdon 46:9b60a3c1acab 431 make_button_active.attach(&unsuppressing_button,0.5);
Spekdon 46:9b60a3c1acab 432 }
Spekdon 46:9b60a3c1acab 433
MaikOvermars 0:4cb1de41d049 434 break;
MaikOvermars 0:4cb1de41d049 435
MaikOvermars 0:4cb1de41d049 436 case demo: //moving according to a specified trajectory
SvenD97 33:eb77ed8d167c 437 // We want to draw a square. Hence, first move to a certain point and then start moving a square.
SvenD97 30:7a66951a0122 438 if (state_changed == true){
SvenD97 30:7a66951a0122 439 state_changed = false;
MaikOvermars 39:5db36ce5e620 440 state_timer.reset();
MaikOvermars 39:5db36ce5e620 441 state_timer.start();
Spekdon 45:829a3992d689 442 des_vx = 0.1;
Spekdon 45:829a3992d689 443 des_vy = 0;
Spekdon 45:829a3992d689 444 }
Spekdon 43:8e2ea92fee01 445 double old_vx;
Spekdon 43:8e2ea92fee01 446 double old_vy;
Spekdon 43:8e2ea92fee01 447 if (state_timer > 2.0f){
Spekdon 43:8e2ea92fee01 448 old_vx = des_vx;
Spekdon 43:8e2ea92fee01 449 old_vy = des_vy;
Spekdon 45:829a3992d689 450
Spekdon 45:829a3992d689 451 des_vy = -old_vx;
Spekdon 45:829a3992d689 452 des_vx = old_vy;
Spekdon 45:829a3992d689 453
Spekdon 45:829a3992d689 454 state_timer.reset();
Spekdon 45:829a3992d689 455 state_timer.start();
Spekdon 43:8e2ea92fee01 456 }
Spekdon 45:829a3992d689 457
Spekdon 45:829a3992d689 458
MaikOvermars 42:bcd496523c66 459 // des_vx = 0.1; // first we move to the right
MaikOvermars 42:bcd496523c66 460 // des_vy = 0.0;
MaikOvermars 42:bcd496523c66 461
MaikOvermars 42:bcd496523c66 462 //static double new_vx, new_vy;
MaikOvermars 39:5db36ce5e620 463
MaikOvermars 42:bcd496523c66 464 //if(state_timer > 3.0f){ // after waiting an extra second we start on another side of the square
MaikOvermars 42:bcd496523c66 465 // state_timer.reset();
MaikOvermars 42:bcd496523c66 466 // state_timer.start();
MaikOvermars 42:bcd496523c66 467 // des_vx = new_vx;
MaikOvermars 42:bcd496523c66 468 // des_vy = new_vy;
MaikOvermars 42:bcd496523c66 469 //}
MaikOvermars 42:bcd496523c66 470 //else if(state_timer > 2.0f){ // we have a velocity of 0.1 m/s for 2 seconds, so we make a square of 20 by 20 cm
MaikOvermars 42:bcd496523c66 471 // if(des_vx > 0){new_vx = 0.0; new_vy = 0.1;} // here we check which side of the square we were on
MaikOvermars 42:bcd496523c66 472 // else if(des_vy > 0){new_vx = -0.1; new_vy = 0.0;}
MaikOvermars 42:bcd496523c66 473 // else if(des_vx < 0){new_vx = 0.0; new_vy = -0.1;}
MaikOvermars 42:bcd496523c66 474 // else if(des_vy < 0){new_vx = 0.1; new_vy = 0.0;}
MaikOvermars 42:bcd496523c66 475 // des_vx = 0;
MaikOvermars 42:bcd496523c66 476 // des_vy = 0;
MaikOvermars 42:bcd496523c66 477 //}
MaikOvermars 39:5db36ce5e620 478
SvenD97 30:7a66951a0122 479
Spekdon 45:829a3992d689 480 if (button.read() == false && button_suppressed == false ) {
SvenD97 30:7a66951a0122 481 current_state = operational;
SvenD97 30:7a66951a0122 482 state_changed = true;
SvenD97 34:7d672cd04486 483
SvenD97 34:7d672cd04486 484 button_suppressed = true;
SvenD97 34:7d672cd04486 485
SvenD97 34:7d672cd04486 486 make_button_active.attach(&unsuppressing_button,0.5);
SvenD97 30:7a66951a0122 487 }
MaikOvermars 0:4cb1de41d049 488
MaikOvermars 0:4cb1de41d049 489 break;
MaikOvermars 0:4cb1de41d049 490
MaikOvermars 0:4cb1de41d049 491 case failure: //no way to get out
MaikOvermars 0:4cb1de41d049 492 u1 = 0.0f;
MaikOvermars 17:1f93c83e211f 493 u2 = 0.0f;
bjonkheer 29:d1e8eb135e6c 494 led_R = 0;
bjonkheer 29:d1e8eb135e6c 495 led_G = 1;
bjonkheer 29:d1e8eb135e6c 496 led_B = 1;
MaikOvermars 0:4cb1de41d049 497 break;
MaikOvermars 0:4cb1de41d049 498 }
MaikOvermars 0:4cb1de41d049 499 }
MaikOvermars 0:4cb1de41d049 500
MaikOvermars 0:4cb1de41d049 501 void motor_controller()
MaikOvermars 0:4cb1de41d049 502 {
MaikOvermars 0:4cb1de41d049 503 if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
Spekdon 43:8e2ea92fee01 504 ref = inversekinematics_function(T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
Spekdon 43:8e2ea92fee01 505 qref1 = ref[0];
Spekdon 43:8e2ea92fee01 506 qref2 = ref[1];
MaikOvermars 16:0280a604cf7e 507 e1 = qref1 - q1; //tracking error (q_ref - q_meas)
MaikOvermars 16:0280a604cf7e 508 e2 = qref2 - q2;
Spekdon 45:829a3992d689 509 PID_controller(e1*(180/3.14),e2*(180/3.14),u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
MaikOvermars 0:4cb1de41d049 510 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
MaikOvermars 0:4cb1de41d049 511 }
MaikOvermars 0:4cb1de41d049 512
MaikOvermars 0:4cb1de41d049 513
MaikOvermars 0:4cb1de41d049 514 void loop_function() {
MaikOvermars 0:4cb1de41d049 515 measure_all(); //measure all signals
MaikOvermars 0:4cb1de41d049 516 state_machine(); //Do relevant state dependent things
MaikOvermars 0:4cb1de41d049 517 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
MaikOvermars 0:4cb1de41d049 518 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
MaikOvermars 0:4cb1de41d049 519 }
MaikOvermars 0:4cb1de41d049 520
MaikOvermars 0:4cb1de41d049 521
MaikOvermars 0:4cb1de41d049 522 int main()
MaikOvermars 0:4cb1de41d049 523 {
Spekdon 45:829a3992d689 524 pc.baud(115200);
MaikOvermars 0:4cb1de41d049 525 motor1_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 526 motor2_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 527 current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
MaikOvermars 0:4cb1de41d049 528 u1 = 0.0f; //initial output to motors is 0.
MaikOvermars 10:7339dca7d604 529 u2 = 0.0f;
MaikOvermars 41:e9d6fdf02074 530 bqc0.add(&bq0high).add(&bq0notch).add(&bq0notch2).add(&bq0notch3).add(&bq0notch4); // filter cascade for emg 0
MaikOvermars 41:e9d6fdf02074 531 bqc1.add(&bq1high).add(&bq1notch).add(&bq1notch2).add(&bq1notch3).add(&bq1notch4); // filter cascade for emg 1
MaikOvermars 41:e9d6fdf02074 532 bqc2.add(&bq2high).add(&bq2notch).add(&bq2notch2).add(&bq2notch3).add(&bq2notch4); // filter cascade for emg 2
MaikOvermars 41:e9d6fdf02074 533 bqc3.add(&bq3high).add(&bq3notch).add(&bq3notch2).add(&bq3notch3).add(&bq3notch4); // filter cascade for emg 3
MaikOvermars 17:1f93c83e211f 534 loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
SvenD97 32:2596cc9156ec 535 led_R = 1;
SvenD97 32:2596cc9156ec 536 led_B = 1;
SvenD97 32:2596cc9156ec 537 led_G = 1;
Spekdon 45:829a3992d689 538
Spekdon 46:9b60a3c1acab 539 gripper = 1;
Spekdon 46:9b60a3c1acab 540 int m = 0;
Spekdon 46:9b60a3c1acab 541 while (true) {
Spekdon 46:9b60a3c1acab 542 m++;
Spekdon 46:9b60a3c1acab 543 if (m%1000)
Spekdon 46:9b60a3c1acab 544 { pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f, gripper: \r\n", e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, q1, q2);}
Spekdon 46:9b60a3c1acab 545 //testconfig = forwardkinematics_function(qref1,qref2);
Spekdon 45:829a3992d689 546 //double x = testconfig[0];
Spekdon 45:829a3992d689 547 //double y = testconfig[1];
Spekdon 45:829a3992d689 548 //pc.printf("x: %f, y: %f \r\n", x,y);
Spekdon 45:829a3992d689 549 } //Do nothing here (timing purposes)
MaikOvermars 0:4cb1de41d049 550 }