Biorobotics / Robot-Software

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

main.cpp@29:d1e8eb135e6c, 2018-10-29 (annotated)

Committer:
bjonkheer
Date:
Mon Oct 29 15:20:50 2018 +0000
Revision:
29:d1e8eb135e6c
Parent:
28:d952367fc0fc
Child:
30:7a66951a0122
Encoder calibration almost finished;

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"
MaikOvermars 0:4cb1de41d049 7 #include "kinematics.h"
MaikOvermars 17:1f93c83e211f 8 #include "processing_chain_emg.h"
MaikOvermars 0:4cb1de41d049 9
MaikOvermars 0:4cb1de41d049 10 //Define objects
bjonkheer 29:d1e8eb135e6c 11 MODSERIAL pc(USBTX, USBRX);
MaikOvermars 0:4cb1de41d049 12 HIDScope scope(2);
MaikOvermars 0:4cb1de41d049 13
MaikOvermars 10:7339dca7d604 14 // emg inputs
MaikOvermars 0:4cb1de41d049 15 AnalogIn emg0( A0 );
MaikOvermars 0:4cb1de41d049 16 AnalogIn emg1( A1 );
MaikOvermars 0:4cb1de41d049 17
MaikOvermars 10:7339dca7d604 18 // motor ouptuts
MaikOvermars 0:4cb1de41d049 19 PwmOut motor1_pwm(D5);
MaikOvermars 0:4cb1de41d049 20 DigitalOut motor1_dir(D4);
MaikOvermars 0:4cb1de41d049 21 PwmOut motor2_pwm(D7);
MaikOvermars 0:4cb1de41d049 22 DigitalOut motor2_dir(D6);
MaikOvermars 0:4cb1de41d049 23
SvenD97 14:4744cc6c90f4 24 // defining encoders
MaikOvermars 16:0280a604cf7e 25 QEI motor_1_encoder(D12,D13,NC,32);
MaikOvermars 16:0280a604cf7e 26 QEI motor_2_encoder(D10,D11,NC,32);
SvenD97 14:4744cc6c90f4 27
MaikOvermars 17:1f93c83e211f 28 // other objects
MaikOvermars 0:4cb1de41d049 29 AnalogIn potmeter1(A2);
MaikOvermars 0:4cb1de41d049 30 AnalogIn potmeter2(A3);
MaikOvermars 0:4cb1de41d049 31 DigitalIn button(D0);
bjonkheer 29:d1e8eb135e6c 32 DigitalOut led_R(LED_RED) = 1;
bjonkheer 29:d1e8eb135e6c 33 DigitalOut led_B(LED_BLUE) = 1;
bjonkheer 29:d1e8eb135e6c 34 DigitalOut led_G(LED_GREEN) = 1;
MaikOvermars 0:4cb1de41d049 35
SvenD97 14:4744cc6c90f4 36 // tickers and timers
MaikOvermars 16:0280a604cf7e 37 Ticker loop_ticker;
MaikOvermars 0:4cb1de41d049 38 Timer state_timer;
SvenD97 27:eee900e0a47d 39 Timer emg_timer;
MaikOvermars 0:4cb1de41d049 40
bjonkheer 29:d1e8eb135e6c 41 <<<<<<< local
bjonkheer 29:d1e8eb135e6c 42 enum States {failure, waiting, calib_emg, homing, calib_enc, operational, demo}; //All possible robot states
bjonkheer 29:d1e8eb135e6c 43 =======
MaikOvermars 0:4cb1de41d049 44 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
SvenD97 27:eee900e0a47d 45 enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
bjonkheer 29:d1e8eb135e6c 46 >>>>>>> other
MaikOvermars 0:4cb1de41d049 47
MaikOvermars 0:4cb1de41d049 48 //Global variables/objects
MaikOvermars 0:4cb1de41d049 49 States current_state;
SvenD97 27:eee900e0a47d 50 Emg_measures_states curent_emg_calibration_state = not_in_calib_enc;
MaikOvermars 16:0280a604cf7e 51
MaikOvermars 17:1f93c83e211f 52 double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2, raw_emg_0, process_emg_0, raw_emg_1, process_emg_1; //will be set by the motor_controller function
MaikOvermars 17:1f93c83e211f 53 double vxmax = 1.0, vymax = 1.0;
SvenD97 27:eee900e0a47d 54 double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
SvenD97 27:eee900e0a47d 55
SvenD97 27:eee900e0a47d 56 // Meaning of process_emg_0 and such
SvenD97 27:eee900e0a47d 57 // - process_emg_0 is right biceps
SvenD97 27:eee900e0a47d 58 // - process_emg_1 is right triceps
SvenD97 27:eee900e0a47d 59 // - process_emg_2 is left biceps
SvenD97 27:eee900e0a47d 60 // - process_emg_3 is left triceps
bjonkheer 23:7d83af123c43 61
MaikOvermars 0:4cb1de41d049 62 int counts_per_rotation = 32;
MaikOvermars 0:4cb1de41d049 63 bool state_changed = false;
SvenD97 8:bba05e863b68 64 const double T = 0.001;
bjonkheer 23:7d83af123c43 65
MaikOvermars 0:4cb1de41d049 66
MaikOvermars 17:1f93c83e211f 67 // Functions
MaikOvermars 0:4cb1de41d049 68 void measure_all()
MaikOvermars 0:4cb1de41d049 69 {
bjonkheer 23:7d83af123c43 70
MaikOvermars 16:0280a604cf7e 71 q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
MaikOvermars 16:0280a604cf7e 72 q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation;
SvenD97 14:4744cc6c90f4 73 forwardkinematics_function(q1,q2,x,y); //motor_angle is global, this function ne
MaikOvermars 17:1f93c83e211f 74 raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
MaikOvermars 17:1f93c83e211f 75 raw_emg_1 = emg1.read();
MaikOvermars 17:1f93c83e211f 76 processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1); // processes the emg signals
MaikOvermars 0:4cb1de41d049 77 }
MaikOvermars 0:4cb1de41d049 78
bjonkheer 29:d1e8eb135e6c 79 void homing() {
bjonkheer 29:d1e8eb135e6c 80 PID_controller(0.5*3.1415926535f-q1,3.1415926535f-q2,u1,u2,T)
bjonkheer 29:d1e8eb135e6c 81
MaikOvermars 0:4cb1de41d049 82 void output_all() {
MaikOvermars 0:4cb1de41d049 83 motor1_pwm = fabs(u1);
MaikOvermars 16:0280a604cf7e 84 motor1_dir = u1 > 0.0f;
MaikOvermars 0:4cb1de41d049 85 motor2_pwm = fabs(u2);
MaikOvermars 16:0280a604cf7e 86 motor2_dir = u2 > 0.0f;
MaikOvermars 0:4cb1de41d049 87 static int output_counter = 0;
MaikOvermars 0:4cb1de41d049 88 output_counter++;
MaikOvermars 0:4cb1de41d049 89 if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;} //Print to screen at 10 Hz with MODSERIAL
MaikOvermars 0:4cb1de41d049 90 }
MaikOvermars 0:4cb1de41d049 91
MaikOvermars 0:4cb1de41d049 92 void state_machine() {
MaikOvermars 0:4cb1de41d049 93 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
MaikOvermars 0:4cb1de41d049 94 case waiting: //Nothing useful here, maybe a blinking LED for fun and communication to the user
MaikOvermars 0:4cb1de41d049 95 if (button.read()==true)
MaikOvermars 0:4cb1de41d049 96 {
MaikOvermars 0:4cb1de41d049 97 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
bjonkheer 29:d1e8eb135e6c 98 state_changed == true;
MaikOvermars 0:4cb1de41d049 99 }
bjonkheer 29:d1e8eb135e6c 100
MaikOvermars 0:4cb1de41d049 101 break; //to avoid falling through to the next state, although this can sometimes be very useful.
MaikOvermars 0:4cb1de41d049 102
MaikOvermars 0:4cb1de41d049 103 case calib_enc:
bjonkheer 29:d1e8eb135e6c 104
MaikOvermars 0:4cb1de41d049 105 if (state_changed==true)
MaikOvermars 0:4cb1de41d049 106 {
MaikOvermars 0:4cb1de41d049 107 state_timer.reset();
MaikOvermars 0:4cb1de41d049 108 state_timer.start();
MaikOvermars 0:4cb1de41d049 109 state_changed = false;
bjonkheer 29:d1e8eb135e6c 110 n = 0;
bjonkheer 29:d1e8eb135e6c 111 led_G = 0;
bjonkheer 29:d1e8eb135e6c 112 led_B = 1;
bjonkheer 29:d1e8eb135e6c 113 led_R = 1;
bjonkheer 29:d1e8eb135e6c 114 u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
bjonkheer 29:d1e8eb135e6c 115 u2 = 0.55f;
bjonkheer 29:d1e8eb135e6c 116 q1old = 0;
bjonkheer 29:d1e8eb135e6c 117 q2old = 0;
MaikOvermars 0:4cb1de41d049 118 }
bjonkheer 29:d1e8eb135e6c 119
bjonkheer 29:d1e8eb135e6c 120 if q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f
bjonkheer 29:d1e8eb135e6c 121 {
MaikOvermars 0:4cb1de41d049 122 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
SvenD97 27:eee900e0a47d 123 curent_emg_calibration_state = calib_right_bicep;
MaikOvermars 0:4cb1de41d049 124 state_changed = true;
bjonkheer 29:d1e8eb135e6c 125
bjonkheer 29:d1e8eb135e6c 126 }
bjonkheer 29:d1e8eb135e6c 127 q1old = q1;
bjonkheer 29:d1e8eb135e6c 128 q2old = q2;
bjonkheer 29:d1e8eb135e6c 129
bjonkheer 29:d1e8eb135e6c 130 n++;
bjonkheer 29:d1e8eb135e6c 131 if (n%1000 == 0)
bjonkheer 29:d1e8eb135e6c 132 {
bjonkheer 29:d1e8eb135e6c 133 led_G = !led_G;
MaikOvermars 0:4cb1de41d049 134 }
bjonkheer 29:d1e8eb135e6c 135
MaikOvermars 0:4cb1de41d049 136 break;
MaikOvermars 0:4cb1de41d049 137
MaikOvermars 0:4cb1de41d049 138 case calib_emg: //calibrate emg-signals
SvenD97 27:eee900e0a47d 139 if (state_changed == true){
SvenD97 27:eee900e0a47d 140 state_timer.reset();
SvenD97 27:eee900e0a47d 141 state_timer.start();
SvenD97 27:eee900e0a47d 142 emg_timer.reset();
SvenD97 27:eee900e0a47d 143 emg_timer.start();
SvenD97 27:eee900e0a47d 144 state_changed = false;
SvenD97 27:eee900e0a47d 145 }
SvenD97 27:eee900e0a47d 146
SvenD97 27:eee900e0a47d 147 // calibrating right bicep
SvenD97 27:eee900e0a47d 148 switch(curent_emg_calibration_state){
SvenD97 27:eee900e0a47d 149 case calib_right_bicep:
SvenD97 27:eee900e0a47d 150 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 151 if (process_emg_0 > right_bicep_max){
SvenD97 27:eee900e0a47d 152 right_bicep_max = process_emg_0;
SvenD97 27:eee900e0a47d 153 }
SvenD97 27:eee900e0a47d 154 }
SvenD97 27:eee900e0a47d 155 else{
SvenD97 27:eee900e0a47d 156 current_emg_calibration_state = calib_right_tricep;
SvenD97 27:eee900e0a47d 157 emg_timer.reset();
SvenD97 27:eee900e0a47d 158 emg_timer.start();
SvenD97 27:eee900e0a47d 159 }
SvenD97 27:eee900e0a47d 160 break;
SvenD97 27:eee900e0a47d 161 case calib_right_tricep:
SvenD97 27:eee900e0a47d 162 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 163 if (process_emg_1 > right_tricep_max){
SvenD97 27:eee900e0a47d 164 right_tricep_max = process_emg_1;
SvenD97 27:eee900e0a47d 165 }
SvenD97 27:eee900e0a47d 166 }
SvenD97 27:eee900e0a47d 167 else{
SvenD97 27:eee900e0a47d 168 current_emg_calibration_state = calib_left_bicep;
SvenD97 27:eee900e0a47d 169 emg_timer.reset();
SvenD97 27:eee900e0a47d 170 emg_timer.start();
SvenD97 27:eee900e0a47d 171 }
SvenD97 27:eee900e0a47d 172 break;
SvenD97 27:eee900e0a47d 173 case calib_left_bicep:
SvenD97 27:eee900e0a47d 174 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 175 if (process_emg_2 > left_bicep_max){
SvenD97 27:eee900e0a47d 176 left_bicep_max = process_emg_2;
SvenD97 27:eee900e0a47d 177 }
SvenD97 27:eee900e0a47d 178 }
SvenD97 27:eee900e0a47d 179 else{
SvenD97 27:eee900e0a47d 180 current_emg_calibration_state = calib_left_tricep;
SvenD97 27:eee900e0a47d 181 emg_timer.reset();
SvenD97 27:eee900e0a47d 182 emg_timer.start();
SvenD97 27:eee900e0a47d 183 }
SvenD97 27:eee900e0a47d 184 break;
SvenD97 27:eee900e0a47d 185 case calib_left_tricep:
SvenD97 27:eee900e0a47d 186 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 187 if (process_emg_3 > left_tricep_max){
SvenD97 27:eee900e0a47d 188 left_tricep_max = process_emg_3;
SvenD97 27:eee900e0a47d 189 }
SvenD97 27:eee900e0a47d 190 }
SvenD97 27:eee900e0a47d 191 else{
SvenD97 27:eee900e0a47d 192 current_emg_calibration_state = not_in_calib_emg;
SvenD97 27:eee900e0a47d 193 current_state = operational;
SvenD97 27:eee900e0a47d 194 state_changed = true;
SvenD97 27:eee900e0a47d 195 emg_timer.reset();
SvenD97 27:eee900e0a47d 196 emg_timer.start();
SvenD97 27:eee900e0a47d 197 }
SvenD97 27:eee900e0a47d 198 break;
SvenD97 27:eee900e0a47d 199 default:
SvenD97 27:eee900e0a47d 200 current_state = failure;
SvenD97 27:eee900e0a47d 201 state_changed = true;
SvenD97 27:eee900e0a47d 202
MaikOvermars 0:4cb1de41d049 203
MaikOvermars 0:4cb1de41d049 204 case operational: //interpreting emg-signals to move the end effector
bjonkheer 23:7d83af123c43 205
MaikOvermars 17:1f93c83e211f 206 // here we have to determine the desired velocity based on the processed emg signals and calibration
MaikOvermars 17:1f93c83e211f 207 if (process_emg_0 >= 0.16) { des_vx = vxmax; }
MaikOvermars 17:1f93c83e211f 208 else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
MaikOvermars 17:1f93c83e211f 209 else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
MaikOvermars 17:1f93c83e211f 210 else { des_vx = 0; }
MaikOvermars 17:1f93c83e211f 211
MaikOvermars 17:1f93c83e211f 212 if (process_emg_1 >= 0.16) { des_vy = vymax; }
MaikOvermars 17:1f93c83e211f 213 else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
MaikOvermars 17:1f93c83e211f 214 else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
MaikOvermars 17:1f93c83e211f 215 else { des_vy = 0; }
MaikOvermars 17:1f93c83e211f 216
MaikOvermars 0:4cb1de41d049 217 if (button.read() == true) { current_state = demo; }
MaikOvermars 0:4cb1de41d049 218
bjonkheer 29:d1e8eb135e6c 219
MaikOvermars 0:4cb1de41d049 220 break;
MaikOvermars 0:4cb1de41d049 221
MaikOvermars 0:4cb1de41d049 222 case demo: //moving according to a specified trajectory
MaikOvermars 0:4cb1de41d049 223
MaikOvermars 17:1f93c83e211f 224 if (button.read() == true) { current_state = operational; }
MaikOvermars 0:4cb1de41d049 225
MaikOvermars 0:4cb1de41d049 226 break;
MaikOvermars 0:4cb1de41d049 227
MaikOvermars 0:4cb1de41d049 228 case failure: //no way to get out
MaikOvermars 0:4cb1de41d049 229 u1 = 0.0f;
MaikOvermars 17:1f93c83e211f 230 u2 = 0.0f;
bjonkheer 29:d1e8eb135e6c 231 led_R = 0;
bjonkheer 29:d1e8eb135e6c 232 led_G = 1;
bjonkheer 29:d1e8eb135e6c 233 led_B = 1;
MaikOvermars 0:4cb1de41d049 234 break;
MaikOvermars 0:4cb1de41d049 235 }
MaikOvermars 0:4cb1de41d049 236 }
MaikOvermars 0:4cb1de41d049 237
MaikOvermars 0:4cb1de41d049 238 void motor_controller()
MaikOvermars 0:4cb1de41d049 239 {
MaikOvermars 0:4cb1de41d049 240 if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
MaikOvermars 16:0280a604cf7e 241 inversekinematics_function(x,y,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
MaikOvermars 16:0280a604cf7e 242 e1 = qref1 - q1; //tracking error (q_ref - q_meas)
MaikOvermars 16:0280a604cf7e 243 e2 = qref2 - q2;
MaikOvermars 17:1f93c83e211f 244 PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
MaikOvermars 0:4cb1de41d049 245 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
MaikOvermars 0:4cb1de41d049 246 }
MaikOvermars 0:4cb1de41d049 247
MaikOvermars 0:4cb1de41d049 248
MaikOvermars 0:4cb1de41d049 249 void loop_function() {
MaikOvermars 0:4cb1de41d049 250 measure_all(); //measure all signals
MaikOvermars 0:4cb1de41d049 251 state_machine(); //Do relevant state dependent things
MaikOvermars 0:4cb1de41d049 252 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
MaikOvermars 0:4cb1de41d049 253 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
MaikOvermars 0:4cb1de41d049 254 }
MaikOvermars 0:4cb1de41d049 255
MaikOvermars 0:4cb1de41d049 256
MaikOvermars 0:4cb1de41d049 257 int main()
MaikOvermars 0:4cb1de41d049 258 {
MaikOvermars 0:4cb1de41d049 259 pc.baud(115200);
MaikOvermars 0:4cb1de41d049 260 motor1_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 261 motor2_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 262 current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
MaikOvermars 0:4cb1de41d049 263 u1 = 0.0f; //initial output to motors is 0.
MaikOvermars 10:7339dca7d604 264 u2 = 0.0f;
MaikOvermars 25:734a26538711 265 bqc0.add(&bq0high).add(&bq0notch); // filter cascade for emg
MaikOvermars 25:734a26538711 266 bqc1.add(&bq1high).add(&bq1notch); // filter cascade for emg
MaikOvermars 17:1f93c83e211f 267 loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
MaikOvermars 0:4cb1de41d049 268
MaikOvermars 0:4cb1de41d049 269 while (true) { } //Do nothing here (timing purposes)
MaikOvermars 0:4cb1de41d049 270 }