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@36:dc0571d14e30, 2018-10-31 (annotated)

Committer:
SvenD97
Date:
Wed Oct 31 11:50:28 2018 +0000
Revision:
36:dc0571d14e30
Parent:
35:6110d0b5513b
Child:
37:5ddfd9e6cdb2
Got rid of hidscope and modserial stuff;

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 10:7339dca7d604 10 // emg inputs
MaikOvermars 0:4cb1de41d049 11 AnalogIn emg0( A0 );
MaikOvermars 0:4cb1de41d049 12 AnalogIn emg1( A1 );
MaikOvermars 0:4cb1de41d049 13
MaikOvermars 10:7339dca7d604 14 // motor ouptuts
MaikOvermars 0:4cb1de41d049 15 PwmOut motor1_pwm(D5);
MaikOvermars 0:4cb1de41d049 16 DigitalOut motor1_dir(D4);
MaikOvermars 0:4cb1de41d049 17 PwmOut motor2_pwm(D7);
MaikOvermars 0:4cb1de41d049 18 DigitalOut motor2_dir(D6);
MaikOvermars 0:4cb1de41d049 19
SvenD97 14:4744cc6c90f4 20 // defining encoders
MaikOvermars 16:0280a604cf7e 21 QEI motor_1_encoder(D12,D13,NC,32);
MaikOvermars 16:0280a604cf7e 22 QEI motor_2_encoder(D10,D11,NC,32);
SvenD97 14:4744cc6c90f4 23
MaikOvermars 17:1f93c83e211f 24 // other objects
MaikOvermars 0:4cb1de41d049 25 AnalogIn potmeter1(A2);
MaikOvermars 0:4cb1de41d049 26 AnalogIn potmeter2(A3);
MaikOvermars 0:4cb1de41d049 27 DigitalIn button(D0);
SvenD97 31:393a7ec1d396 28 DigitalOut led_R(LED_RED);
SvenD97 31:393a7ec1d396 29 DigitalOut led_B(LED_BLUE);
SvenD97 31:393a7ec1d396 30 DigitalOut led_G(LED_GREEN);
MaikOvermars 0:4cb1de41d049 31
SvenD97 14:4744cc6c90f4 32 // tickers and timers
MaikOvermars 16:0280a604cf7e 33 Ticker loop_ticker;
MaikOvermars 0:4cb1de41d049 34 Timer state_timer;
SvenD97 27:eee900e0a47d 35 Timer emg_timer;
MaikOvermars 0:4cb1de41d049 36
SvenD97 34:7d672cd04486 37 // Timeouts and related variables
SvenD97 34:7d672cd04486 38 Timeout make_button_active;
SvenD97 34:7d672cd04486 39 bool button_suppressed = false;
SvenD97 34:7d672cd04486 40
SvenD97 34:7d672cd04486 41 // States
SvenD97 31:393a7ec1d396 42 enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
SvenD97 27:eee900e0a47d 43 enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
MaikOvermars 0:4cb1de41d049 44
MaikOvermars 0:4cb1de41d049 45 //Global variables/objects
MaikOvermars 0:4cb1de41d049 46 States current_state;
SvenD97 31:393a7ec1d396 47 Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
MaikOvermars 16:0280a604cf7e 48
SvenD97 35:6110d0b5513b 49 double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2; //will be set by the motor_controller function
SvenD97 35:6110d0b5513b 50 double u1 = 0, u2= 0;
MaikOvermars 17:1f93c83e211f 51 double vxmax = 1.0, vymax = 1.0;
SvenD97 27:eee900e0a47d 52 double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
SvenD97 27:eee900e0a47d 53
SvenD97 31:393a7ec1d396 54 // Variables for emg
SvenD97 31:393a7ec1d396 55 double raw_emg_0, process_emg_0;
SvenD97 31:393a7ec1d396 56 double raw_emg_1, process_emg_1;
SvenD97 31:393a7ec1d396 57 double raw_emg_2, process_emg_2;
SvenD97 31:393a7ec1d396 58 double raw_emg_3, process_emg_3;
SvenD97 31:393a7ec1d396 59
SvenD97 31:393a7ec1d396 60
SvenD97 33:eb77ed8d167c 61 // Variables for calibration
SvenD97 35:6110d0b5513b 62 double calib_q1 = 3.1415926535f;
SvenD97 35:6110d0b5513b 63 double calib_q2 = 1.5f*3.1415926535f;
SvenD97 33:eb77ed8d167c 64 double off_set_q1 = 0; // This variable is used to determine the off set from our definition from q1 and q2.
SvenD97 33:eb77ed8d167c 65 double off_set_q2 = 0;
SvenD97 33:eb77ed8d167c 66
SvenD97 30:7a66951a0122 67 // Variables defined for the homing state
SvenD97 35:6110d0b5513b 68 double q1_homing = 0.5f*3.1415926535f, q2_homing = 3.1415926535f;
SvenD97 30:7a66951a0122 69 double beta = 5;
SvenD97 31:393a7ec1d396 70 double k_hom = 2;
SvenD97 31:393a7ec1d396 71
SvenD97 31:393a7ec1d396 72 // For the state calib_enc
SvenD97 31:393a7ec1d396 73 double q1old;
SvenD97 31:393a7ec1d396 74 double q2old;
SvenD97 30:7a66951a0122 75
SvenD97 27:eee900e0a47d 76 // Meaning of process_emg_0 and such
SvenD97 27:eee900e0a47d 77 // - process_emg_0 is right biceps
SvenD97 27:eee900e0a47d 78 // - process_emg_1 is right triceps
SvenD97 27:eee900e0a47d 79 // - process_emg_2 is left biceps
SvenD97 27:eee900e0a47d 80 // - process_emg_3 is left triceps
bjonkheer 23:7d83af123c43 81
MaikOvermars 0:4cb1de41d049 82 int counts_per_rotation = 32;
MaikOvermars 0:4cb1de41d049 83 bool state_changed = false;
SvenD97 8:bba05e863b68 84 const double T = 0.001;
bjonkheer 23:7d83af123c43 85
SvenD97 30:7a66951a0122 86 // Resolution of the encoder at the output axle
SvenD97 30:7a66951a0122 87 double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians
SvenD97 30:7a66951a0122 88
MaikOvermars 17:1f93c83e211f 89 // Functions
MaikOvermars 0:4cb1de41d049 90 void measure_all()
MaikOvermars 0:4cb1de41d049 91 {
bjonkheer 23:7d83af123c43 92
SvenD97 33:eb77ed8d167c 93 q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
SvenD97 33:eb77ed8d167c 94 q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q2;
SvenD97 33:eb77ed8d167c 95 forwardkinematics_function(q1,q2,x,y);
MaikOvermars 17:1f93c83e211f 96 raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
MaikOvermars 17:1f93c83e211f 97 raw_emg_1 = emg1.read();
MaikOvermars 17:1f93c83e211f 98 processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1); // processes the emg signals
MaikOvermars 0:4cb1de41d049 99 }
MaikOvermars 0:4cb1de41d049 100
MaikOvermars 0:4cb1de41d049 101 void output_all() {
MaikOvermars 0:4cb1de41d049 102 motor1_pwm = fabs(u1);
MaikOvermars 16:0280a604cf7e 103 motor1_dir = u1 > 0.0f;
MaikOvermars 0:4cb1de41d049 104 motor2_pwm = fabs(u2);
MaikOvermars 16:0280a604cf7e 105 motor2_dir = u2 > 0.0f;
MaikOvermars 0:4cb1de41d049 106 static int output_counter = 0;
MaikOvermars 0:4cb1de41d049 107 output_counter++;
MaikOvermars 0:4cb1de41d049 108 }
MaikOvermars 0:4cb1de41d049 109
SvenD97 34:7d672cd04486 110 void unsuppressing_button(){
SvenD97 34:7d672cd04486 111 button_suppressed = false;
SvenD97 34:7d672cd04486 112 }
SvenD97 34:7d672cd04486 113
MaikOvermars 0:4cb1de41d049 114 void state_machine() {
MaikOvermars 0:4cb1de41d049 115 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
MaikOvermars 0:4cb1de41d049 116 case waiting: //Nothing useful here, maybe a blinking LED for fun and communication to the user
MaikOvermars 0:4cb1de41d049 117 if (button.read()==true)
MaikOvermars 0:4cb1de41d049 118 {
MaikOvermars 0:4cb1de41d049 119 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
SvenD97 31:393a7ec1d396 120 state_changed = true;
MaikOvermars 0:4cb1de41d049 121 }
bjonkheer 29:d1e8eb135e6c 122
MaikOvermars 0:4cb1de41d049 123 break; //to avoid falling through to the next state, although this can sometimes be very useful.
MaikOvermars 0:4cb1de41d049 124
MaikOvermars 0:4cb1de41d049 125 case calib_enc:
bjonkheer 29:d1e8eb135e6c 126
MaikOvermars 0:4cb1de41d049 127 if (state_changed==true)
MaikOvermars 0:4cb1de41d049 128 {
MaikOvermars 0:4cb1de41d049 129 state_timer.reset();
MaikOvermars 0:4cb1de41d049 130 state_timer.start();
MaikOvermars 0:4cb1de41d049 131 state_changed = false;
bjonkheer 29:d1e8eb135e6c 132 n = 0;
bjonkheer 29:d1e8eb135e6c 133 led_G = 0;
bjonkheer 29:d1e8eb135e6c 134 led_B = 1;
bjonkheer 29:d1e8eb135e6c 135 led_R = 1;
bjonkheer 29:d1e8eb135e6c 136 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 137 u2 = 0.55f;
bjonkheer 29:d1e8eb135e6c 138 q1old = 0;
bjonkheer 29:d1e8eb135e6c 139 q2old = 0;
MaikOvermars 0:4cb1de41d049 140 }
bjonkheer 29:d1e8eb135e6c 141
SvenD97 31:393a7ec1d396 142 if (q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f)
bjonkheer 29:d1e8eb135e6c 143 {
MaikOvermars 0:4cb1de41d049 144 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
SvenD97 31:393a7ec1d396 145 current_emg_calibration_state = calib_right_bicep;
MaikOvermars 0:4cb1de41d049 146 state_changed = true;
SvenD97 33:eb77ed8d167c 147 off_set_q1 = calib_q1 - q1;
SvenD97 33:eb77ed8d167c 148 off_set_q2 = calib_q2 - q2;
SvenD97 35:6110d0b5513b 149 u1 = 0;
SvenD97 35:6110d0b5513b 150 u2 = 0;
bjonkheer 29:d1e8eb135e6c 151 }
bjonkheer 29:d1e8eb135e6c 152 q1old = q1;
bjonkheer 29:d1e8eb135e6c 153 q2old = q2;
bjonkheer 29:d1e8eb135e6c 154
bjonkheer 29:d1e8eb135e6c 155 n++;
bjonkheer 29:d1e8eb135e6c 156 if (n%1000 == 0)
bjonkheer 29:d1e8eb135e6c 157 {
bjonkheer 29:d1e8eb135e6c 158 led_G = !led_G;
MaikOvermars 0:4cb1de41d049 159 }
bjonkheer 29:d1e8eb135e6c 160
MaikOvermars 0:4cb1de41d049 161 break;
MaikOvermars 0:4cb1de41d049 162
MaikOvermars 0:4cb1de41d049 163 case calib_emg: //calibrate emg-signals
SvenD97 27:eee900e0a47d 164 if (state_changed == true){
SvenD97 27:eee900e0a47d 165 state_timer.reset();
SvenD97 27:eee900e0a47d 166 state_timer.start();
SvenD97 27:eee900e0a47d 167 emg_timer.reset();
SvenD97 27:eee900e0a47d 168 emg_timer.start();
SvenD97 27:eee900e0a47d 169 state_changed = false;
SvenD97 27:eee900e0a47d 170 }
SvenD97 27:eee900e0a47d 171
SvenD97 31:393a7ec1d396 172 switch(current_emg_calibration_state){
SvenD97 27:eee900e0a47d 173 case calib_right_bicep:
SvenD97 27:eee900e0a47d 174 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 175 if (process_emg_0 > right_bicep_max){
SvenD97 27:eee900e0a47d 176 right_bicep_max = process_emg_0;
SvenD97 27:eee900e0a47d 177 }
SvenD97 27:eee900e0a47d 178 }
SvenD97 30:7a66951a0122 179 else if (process_emg_0 < 0.1*right_bicep_max){
SvenD97 27:eee900e0a47d 180 current_emg_calibration_state = calib_right_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_right_tricep:
SvenD97 27:eee900e0a47d 186 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 187 if (process_emg_1 > right_tricep_max){
SvenD97 27:eee900e0a47d 188 right_tricep_max = process_emg_1;
SvenD97 27:eee900e0a47d 189 }
SvenD97 27:eee900e0a47d 190 }
SvenD97 30:7a66951a0122 191 else if (process_emg_1 < 0.1*right_tricep_max){
SvenD97 30:7a66951a0122 192 current_emg_calibration_state = calib_left_bicep;
SvenD97 30:7a66951a0122 193 emg_timer.reset();
SvenD97 30:7a66951a0122 194 emg_timer.start();
SvenD97 30:7a66951a0122 195 }
SvenD97 27:eee900e0a47d 196 break;
SvenD97 31:393a7ec1d396 197 case calib_left_bicep:
SvenD97 30:7a66951a0122 198 if(emg_timer < 5.0f){
SvenD97 30:7a66951a0122 199 if (process_emg_2 > left_bicep_max){
SvenD97 30:7a66951a0122 200 left_bicep_max = process_emg_2;
SvenD97 27:eee900e0a47d 201 }
SvenD97 30:7a66951a0122 202 }
SvenD97 30:7a66951a0122 203 else if (process_emg_2 < 0.1*left_bicep_max){
SvenD97 30:7a66951a0122 204 current_emg_calibration_state = calib_left_tricep;
SvenD97 30:7a66951a0122 205 emg_timer.reset();
SvenD97 30:7a66951a0122 206 emg_timer.start();
SvenD97 30:7a66951a0122 207 }
SvenD97 27:eee900e0a47d 208 break;
SvenD97 27:eee900e0a47d 209 case calib_left_tricep:
SvenD97 27:eee900e0a47d 210 if(emg_timer < 5.0f){
SvenD97 27:eee900e0a47d 211 if (process_emg_3 > left_tricep_max){
SvenD97 27:eee900e0a47d 212 left_tricep_max = process_emg_3;
SvenD97 27:eee900e0a47d 213 }
SvenD97 27:eee900e0a47d 214 }
SvenD97 30:7a66951a0122 215 else if (process_emg_3 < 0.1*left_tricep_max){
SvenD97 30:7a66951a0122 216 current_emg_calibration_state = not_in_calib_emg;
SvenD97 30:7a66951a0122 217 current_state = homing;
SvenD97 30:7a66951a0122 218 state_changed = true;
SvenD97 30:7a66951a0122 219 emg_timer.reset();
SvenD97 30:7a66951a0122 220 }
SvenD97 27:eee900e0a47d 221 break;
SvenD97 27:eee900e0a47d 222 default:
SvenD97 27:eee900e0a47d 223 current_state = failure;
SvenD97 27:eee900e0a47d 224 state_changed = true;
SvenD97 31:393a7ec1d396 225 }
SvenD97 31:393a7ec1d396 226
SvenD97 31:393a7ec1d396 227 break;
SvenD97 27:eee900e0a47d 228
SvenD97 30:7a66951a0122 229 case homing:
SvenD97 30:7a66951a0122 230 if (state_changed == true){
SvenD97 30:7a66951a0122 231 state_timer.reset();
SvenD97 30:7a66951a0122 232 state_timer.start();
SvenD97 30:7a66951a0122 233 qref1 = q1; //NOT SURE IF WE NEED THIS. I do not think so, but just to be sure.
SvenD97 30:7a66951a0122 234 qref2 = q2;
SvenD97 30:7a66951a0122 235 }
SvenD97 33:eb77ed8d167c 236 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 237 des_vy = min(beta, k_hom*(q2 - q2_homing));
SvenD97 30:7a66951a0122 238
SvenD97 31:393a7ec1d396 239 // The value of 3.0 and 2*resolution can be changed
SvenD97 31:393a7ec1d396 240 if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
SvenD97 31:393a7ec1d396 241 if (state_timer > 3.0f){
SvenD97 31:393a7ec1d396 242 current_state = operational;
SvenD97 31:393a7ec1d396 243 state_changed = true;
SvenD97 33:eb77ed8d167c 244 des_vx = 0; // Not sure if needed but added it anyways.
SvenD97 33:eb77ed8d167c 245 des_vy = 0;
SvenD97 30:7a66951a0122 246 }
SvenD97 30:7a66951a0122 247 }
SvenD97 31:393a7ec1d396 248 else{
SvenD97 31:393a7ec1d396 249 state_timer.reset();
SvenD97 31:393a7ec1d396 250 }
SvenD97 31:393a7ec1d396 251 break;
MaikOvermars 0:4cb1de41d049 252
MaikOvermars 0:4cb1de41d049 253 case operational: //interpreting emg-signals to move the end effector
SvenD97 30:7a66951a0122 254 if (state_changed == true){
SvenD97 30:7a66951a0122 255 state_changed = false;
SvenD97 30:7a66951a0122 256 }
SvenD97 30:7a66951a0122 257
bjonkheer 23:7d83af123c43 258
MaikOvermars 17:1f93c83e211f 259 // here we have to determine the desired velocity based on the processed emg signals and calibration
MaikOvermars 17:1f93c83e211f 260 if (process_emg_0 >= 0.16) { des_vx = vxmax; }
MaikOvermars 17:1f93c83e211f 261 else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
MaikOvermars 17:1f93c83e211f 262 else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
MaikOvermars 17:1f93c83e211f 263 else { des_vx = 0; }
MaikOvermars 17:1f93c83e211f 264
MaikOvermars 17:1f93c83e211f 265 if (process_emg_1 >= 0.16) { des_vy = vymax; }
MaikOvermars 17:1f93c83e211f 266 else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
MaikOvermars 17:1f93c83e211f 267 else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
MaikOvermars 17:1f93c83e211f 268 else { des_vy = 0; }
MaikOvermars 17:1f93c83e211f 269
SvenD97 34:7d672cd04486 270 if (button.read() == true && button_suppressed == false ) {
SvenD97 30:7a66951a0122 271 current_state = demo;
SvenD97 30:7a66951a0122 272 state_changed = true;
SvenD97 34:7d672cd04486 273 button_suppressed = true;
SvenD97 34:7d672cd04486 274
SvenD97 34:7d672cd04486 275 make_button_active.attach(&unsuppressing_button,0.5);
SvenD97 34:7d672cd04486 276
SvenD97 34:7d672cd04486 277 }
bjonkheer 29:d1e8eb135e6c 278
MaikOvermars 0:4cb1de41d049 279 break;
MaikOvermars 0:4cb1de41d049 280
MaikOvermars 0:4cb1de41d049 281 case demo: //moving according to a specified trajectory
SvenD97 33:eb77ed8d167c 282 // We want to draw a square. Hence, first move to a certain point and then start moving a square.
SvenD97 30:7a66951a0122 283 if (state_changed == true){
SvenD97 30:7a66951a0122 284 state_changed = false;
SvenD97 30:7a66951a0122 285 }
SvenD97 30:7a66951a0122 286
SvenD97 34:7d672cd04486 287 if (button.read() == true && button_suppressed == false ) {
SvenD97 30:7a66951a0122 288 current_state = operational;
SvenD97 30:7a66951a0122 289 state_changed = true;
SvenD97 34:7d672cd04486 290
SvenD97 34:7d672cd04486 291 button_suppressed = true;
SvenD97 34:7d672cd04486 292
SvenD97 34:7d672cd04486 293 make_button_active.attach(&unsuppressing_button,0.5);
SvenD97 30:7a66951a0122 294 }
MaikOvermars 0:4cb1de41d049 295
MaikOvermars 0:4cb1de41d049 296 break;
MaikOvermars 0:4cb1de41d049 297
MaikOvermars 0:4cb1de41d049 298 case failure: //no way to get out
MaikOvermars 0:4cb1de41d049 299 u1 = 0.0f;
MaikOvermars 17:1f93c83e211f 300 u2 = 0.0f;
bjonkheer 29:d1e8eb135e6c 301 led_R = 0;
bjonkheer 29:d1e8eb135e6c 302 led_G = 1;
bjonkheer 29:d1e8eb135e6c 303 led_B = 1;
MaikOvermars 0:4cb1de41d049 304 break;
MaikOvermars 0:4cb1de41d049 305 }
MaikOvermars 0:4cb1de41d049 306 }
MaikOvermars 0:4cb1de41d049 307
MaikOvermars 0:4cb1de41d049 308 void motor_controller()
MaikOvermars 0:4cb1de41d049 309 {
MaikOvermars 0:4cb1de41d049 310 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 311 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 312 e1 = qref1 - q1; //tracking error (q_ref - q_meas)
MaikOvermars 16:0280a604cf7e 313 e2 = qref2 - q2;
MaikOvermars 17:1f93c83e211f 314 PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
MaikOvermars 0:4cb1de41d049 315 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
MaikOvermars 0:4cb1de41d049 316 }
MaikOvermars 0:4cb1de41d049 317
MaikOvermars 0:4cb1de41d049 318
MaikOvermars 0:4cb1de41d049 319 void loop_function() {
MaikOvermars 0:4cb1de41d049 320 measure_all(); //measure all signals
MaikOvermars 0:4cb1de41d049 321 state_machine(); //Do relevant state dependent things
MaikOvermars 0:4cb1de41d049 322 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
MaikOvermars 0:4cb1de41d049 323 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
MaikOvermars 0:4cb1de41d049 324 }
MaikOvermars 0:4cb1de41d049 325
MaikOvermars 0:4cb1de41d049 326
MaikOvermars 0:4cb1de41d049 327 int main()
MaikOvermars 0:4cb1de41d049 328 {
MaikOvermars 0:4cb1de41d049 329 motor1_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 330 motor2_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 331 current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
MaikOvermars 0:4cb1de41d049 332 u1 = 0.0f; //initial output to motors is 0.
MaikOvermars 10:7339dca7d604 333 u2 = 0.0f;
MaikOvermars 25:734a26538711 334 bqc0.add(&bq0high).add(&bq0notch); // filter cascade for emg
MaikOvermars 25:734a26538711 335 bqc1.add(&bq1high).add(&bq1notch); // filter cascade for emg
MaikOvermars 17:1f93c83e211f 336 loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
SvenD97 32:2596cc9156ec 337 led_R = 1;
SvenD97 32:2596cc9156ec 338 led_B = 1;
SvenD97 32:2596cc9156ec 339 led_G = 1;
SvenD97 32:2596cc9156ec 340
MaikOvermars 0:4cb1de41d049 341 while (true) { } //Do nothing here (timing purposes)
MaikOvermars 0:4cb1de41d049 342 }