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@22:31065a83d9e8, 2018-10-29 (annotated)

Committer:
bjonkheer
Date:
Mon Oct 29 12:21:26 2018 +0000
Revision:
22:31065a83d9e8
Parent:
20:31876566d70f
Parent:
17:1f93c83e211f
Child:
23:7d83af123c43
Fixed merge

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
MaikOvermars 0:4cb1de41d049 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);
MaikOvermars 0:4cb1de41d049 32
SvenD97 14:4744cc6c90f4 33 // tickers and timers
MaikOvermars 16:0280a604cf7e 34 Ticker loop_ticker;
MaikOvermars 0:4cb1de41d049 35 Timer state_timer;
MaikOvermars 0:4cb1de41d049 36
MaikOvermars 0:4cb1de41d049 37 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
MaikOvermars 0:4cb1de41d049 38
MaikOvermars 0:4cb1de41d049 39 //Global variables/objects
MaikOvermars 0:4cb1de41d049 40 States current_state;
bjonkheer 22:31065a83d9e8 41 <<<<<<< local
MaikOvermars 0:4cb1de41d049 42 Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
bjonkheer 20:31876566d70f 43 float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_x, robot_end_y, reference_end_x, reference_end_y, motor_angle_1, motor_angle_2, motor_counts_1, motor_counts_2, q_ref_1, q_ref_2; //will be set by the motor_controller function
bjonkheer 22:31065a83d9e8 44 =======
MaikOvermars 16:0280a604cf7e 45
MaikOvermars 17:1f93c83e211f 46 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 47 double vxmax = 1.0, vymax = 1.0;
bjonkheer 22:31065a83d9e8 48 >>>>>>> other
MaikOvermars 0:4cb1de41d049 49 int counts_per_rotation = 32;
MaikOvermars 0:4cb1de41d049 50 bool state_changed = false;
bjonkheer 22:31065a83d9e8 51 <<<<<<< local
SvenD97 5:0dd66c757f24 52 double x; // Making the position (x,y) of the end effector global
SvenD97 5:0dd66c757f24 53 double y;
bjonkheer 22:31065a83d9e8 54 =======
SvenD97 8:bba05e863b68 55 const double T = 0.001;
bjonkheer 22:31065a83d9e8 56 >>>>>>> other
MaikOvermars 0:4cb1de41d049 57
MaikOvermars 17:1f93c83e211f 58 // Functions
MaikOvermars 0:4cb1de41d049 59 void measure_all()
MaikOvermars 0:4cb1de41d049 60 {
bjonkheer 22:31065a83d9e8 61 <<<<<<< local
bjonkheer 19:e1e18746d98d 62 motor_angle_1 = motor_counts_1*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
bjonkheer 19:e1e18746d98d 63 motor_angle_2 = motor_counts_2*2.0f*3.1415926535f/counts_per_rotation;
bjonkheer 20:31876566d70f 64 forwardkinematics_function(motor_angle_1,motor_angle_2); //motor_angle is global, this function ne
bjonkheer 20:31876566d70f 65
MaikOvermars 0:4cb1de41d049 66 emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
MaikOvermars 0:4cb1de41d049 67 emg_signal_raw_1 = emg1.read();
MaikOvermars 0:4cb1de41d049 68 processed_emg_0 = processing_chain_emg(0); // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
MaikOvermars 0:4cb1de41d049 69 processed_emg_1 = processing_chain_emg(1);
bjonkheer 22:31065a83d9e8 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
bjonkheer 22:31065a83d9e8 77 >>>>>>> other
MaikOvermars 0:4cb1de41d049 78 }
MaikOvermars 0:4cb1de41d049 79
MaikOvermars 0:4cb1de41d049 80 void output_all() {
MaikOvermars 0:4cb1de41d049 81 motor1_pwm = fabs(u1);
MaikOvermars 16:0280a604cf7e 82 motor1_dir = u1 > 0.0f;
MaikOvermars 0:4cb1de41d049 83 motor2_pwm = fabs(u2);
MaikOvermars 16:0280a604cf7e 84 motor2_dir = u2 > 0.0f;
MaikOvermars 0:4cb1de41d049 85 static int output_counter = 0;
MaikOvermars 0:4cb1de41d049 86 output_counter++;
MaikOvermars 0:4cb1de41d049 87 if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;} //Print to screen at 10 Hz with MODSERIAL
MaikOvermars 0:4cb1de41d049 88 }
MaikOvermars 0:4cb1de41d049 89
MaikOvermars 0:4cb1de41d049 90 void state_machine() {
MaikOvermars 0:4cb1de41d049 91 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
MaikOvermars 0:4cb1de41d049 92 case waiting: //Nothing useful here, maybe a blinking LED for fun and communication to the user
MaikOvermars 0:4cb1de41d049 93 if (button.read()==true)
MaikOvermars 0:4cb1de41d049 94 {
MaikOvermars 0:4cb1de41d049 95 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
SvenD97 9:8e1112874c12 96 // no state_changed action?
MaikOvermars 0:4cb1de41d049 97 }
MaikOvermars 0:4cb1de41d049 98 break; //to avoid falling through to the next state, although this can sometimes be very useful.
MaikOvermars 0:4cb1de41d049 99
MaikOvermars 0:4cb1de41d049 100 case calib_enc:
MaikOvermars 0:4cb1de41d049 101 if (state_changed==true)
MaikOvermars 0:4cb1de41d049 102 {
MaikOvermars 0:4cb1de41d049 103 state_timer.reset();
MaikOvermars 0:4cb1de41d049 104 state_timer.start();
MaikOvermars 0:4cb1de41d049 105 state_changed = false;
MaikOvermars 0:4cb1de41d049 106 }
bjonkheer 18:ea605c49afee 107 u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
MaikOvermars 0:4cb1de41d049 108 // fabs(motor1.velocity()) < 0.1f &&
MaikOvermars 0:4cb1de41d049 109 if (state_timer.read() > 5.0f) {
MaikOvermars 0:4cb1de41d049 110 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
MaikOvermars 0:4cb1de41d049 111 state_changed = true;
MaikOvermars 0:4cb1de41d049 112 }
MaikOvermars 0:4cb1de41d049 113 break;
MaikOvermars 0:4cb1de41d049 114
MaikOvermars 0:4cb1de41d049 115 case calib_emg: //calibrate emg-signals
MaikOvermars 17:1f93c83e211f 116 current_state = operational;
MaikOvermars 0:4cb1de41d049 117 break;
MaikOvermars 0:4cb1de41d049 118
MaikOvermars 0:4cb1de41d049 119 case operational: //interpreting emg-signals to move the end effector
MaikOvermars 0:4cb1de41d049 120 if (state_changed==true) { int x = 5; }
MaikOvermars 0:4cb1de41d049 121 // example
bjonkheer 20:31876566d70f 122 reference_end_x = robot_end_x + processed_emg_0;
bjonkheer 20:31876566d70f 123 reference_end_y = robot_end_y + processed_emg_0;
MaikOvermars 17:1f93c83e211f 124
bjonkheer 22:31065a83d9e8 125 <<<<<<< local
bjonkheer 22:31065a83d9e8 126 =======
MaikOvermars 17:1f93c83e211f 127 // here we have to determine the desired velocity based on the processed emg signals and calibration
MaikOvermars 17:1f93c83e211f 128 if (process_emg_0 >= 0.16) { des_vx = vxmax; }
MaikOvermars 17:1f93c83e211f 129 else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
MaikOvermars 17:1f93c83e211f 130 else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
MaikOvermars 17:1f93c83e211f 131 else { des_vx = 0; }
MaikOvermars 17:1f93c83e211f 132
MaikOvermars 17:1f93c83e211f 133 if (process_emg_1 >= 0.16) { des_vy = vymax; }
MaikOvermars 17:1f93c83e211f 134 else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
MaikOvermars 17:1f93c83e211f 135 else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
MaikOvermars 17:1f93c83e211f 136 else { des_vy = 0; }
MaikOvermars 17:1f93c83e211f 137
bjonkheer 22:31065a83d9e8 138 >>>>>>> other
MaikOvermars 0:4cb1de41d049 139 if (button.read() == true) { current_state = demo; }
MaikOvermars 0:4cb1de41d049 140
MaikOvermars 0:4cb1de41d049 141 break;
MaikOvermars 0:4cb1de41d049 142
MaikOvermars 0:4cb1de41d049 143 case demo: //moving according to a specified trajectory
MaikOvermars 0:4cb1de41d049 144
MaikOvermars 17:1f93c83e211f 145 if (button.read() == true) { current_state = operational; }
MaikOvermars 0:4cb1de41d049 146
MaikOvermars 0:4cb1de41d049 147 break;
MaikOvermars 0:4cb1de41d049 148
MaikOvermars 0:4cb1de41d049 149 case failure: //no way to get out
MaikOvermars 0:4cb1de41d049 150 u1 = 0.0f;
MaikOvermars 17:1f93c83e211f 151 u2 = 0.0f;
MaikOvermars 0:4cb1de41d049 152 break;
MaikOvermars 0:4cb1de41d049 153 }
MaikOvermars 0:4cb1de41d049 154 }
MaikOvermars 0:4cb1de41d049 155
MaikOvermars 0:4cb1de41d049 156 void motor_controller()
MaikOvermars 0:4cb1de41d049 157 {
MaikOvermars 0:4cb1de41d049 158 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 159 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 160 e1 = qref1 - q1; //tracking error (q_ref - q_meas)
MaikOvermars 16:0280a604cf7e 161 e2 = qref2 - q2;
MaikOvermars 17:1f93c83e211f 162 PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
MaikOvermars 0:4cb1de41d049 163 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
MaikOvermars 0:4cb1de41d049 164 }
MaikOvermars 0:4cb1de41d049 165
MaikOvermars 0:4cb1de41d049 166
MaikOvermars 0:4cb1de41d049 167 void loop_function() {
MaikOvermars 0:4cb1de41d049 168 measure_all(); //measure all signals
MaikOvermars 0:4cb1de41d049 169 state_machine(); //Do relevant state dependent things
MaikOvermars 0:4cb1de41d049 170 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
MaikOvermars 0:4cb1de41d049 171 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
MaikOvermars 0:4cb1de41d049 172 }
MaikOvermars 0:4cb1de41d049 173
MaikOvermars 0:4cb1de41d049 174
MaikOvermars 0:4cb1de41d049 175 int main()
MaikOvermars 0:4cb1de41d049 176 {
MaikOvermars 0:4cb1de41d049 177 pc.baud(115200);
MaikOvermars 0:4cb1de41d049 178 motor1_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 179 motor2_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 180 current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
MaikOvermars 0:4cb1de41d049 181 u1 = 0.0f; //initial output to motors is 0.
MaikOvermars 10:7339dca7d604 182 u2 = 0.0f;
MaikOvermars 17:1f93c83e211f 183 bqc.add(&bqhigh).add(&bqnotch); // filter cascade for emg
MaikOvermars 17:1f93c83e211f 184 loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
MaikOvermars 0:4cb1de41d049 185
MaikOvermars 0:4cb1de41d049 186 while (true) { } //Do nothing here (timing purposes)
MaikOvermars 0:4cb1de41d049 187 }