Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

main.cpp@12:3c47c7b1d1d7, 2018-10-23 (annotated)

Committer:
SvenD97
Date:
Tue Oct 23 10:47:19 2018 +0000
Branch:
bla
Revision:
12:3c47c7b1d1d7
Parent:
11:c8251a1362b7 
Added some variables;

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 0:4cb1de41d049 8
MaikOvermars 0:4cb1de41d049 9 //Define objects
MaikOvermars 0:4cb1de41d049 10 MODSERIAL pc(USBTX, USBRX);
MaikOvermars 0:4cb1de41d049 11 HIDScope scope(2);
MaikOvermars 0:4cb1de41d049 12
MaikOvermars 10:7339dca7d604 13 // emg inputs
MaikOvermars 0:4cb1de41d049 14 AnalogIn emg0( A0 );
MaikOvermars 0:4cb1de41d049 15 AnalogIn emg1( A1 );
MaikOvermars 0:4cb1de41d049 16
MaikOvermars 10:7339dca7d604 17 // motor ouptuts
MaikOvermars 0:4cb1de41d049 18 PwmOut motor1_pwm(D5);
MaikOvermars 0:4cb1de41d049 19 DigitalOut motor1_dir(D4);
MaikOvermars 0:4cb1de41d049 20 PwmOut motor2_pwm(D7);
MaikOvermars 0:4cb1de41d049 21 DigitalOut motor2_dir(D6);
MaikOvermars 0:4cb1de41d049 22
MaikOvermars 0:4cb1de41d049 23 AnalogIn potmeter1(A2);
MaikOvermars 0:4cb1de41d049 24 AnalogIn potmeter2(A3);
MaikOvermars 0:4cb1de41d049 25 DigitalIn button(D0);
MaikOvermars 0:4cb1de41d049 26
MaikOvermars 0:4cb1de41d049 27 Ticker Sample;
MaikOvermars 0:4cb1de41d049 28 Timer state_timer;
MaikOvermars 0:4cb1de41d049 29
MaikOvermars 0:4cb1de41d049 30 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
MaikOvermars 0:4cb1de41d049 31
MaikOvermars 0:4cb1de41d049 32 //Global variables/objects
MaikOvermars 0:4cb1de41d049 33 States current_state;
MaikOvermars 0:4cb1de41d049 34 Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
SvenD97 12:3c47c7b1d1d7 35 double x,y, qref1, qref2 ,e, e1, e2, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, q1, q2, motor_counts, q_ref; //will be set by the motor_controller function
MaikOvermars 0:4cb1de41d049 36 int counts_per_rotation = 32;
MaikOvermars 0:4cb1de41d049 37 bool state_changed = false;
MaikOvermars 10:7339dca7d604 38 double samplingfreq = 1000;
SvenD97 5:0dd66c757f24 39 double x; // Making the position (x,y) of the end effector global
SvenD97 5:0dd66c757f24 40 double y;
SvenD97 8:bba05e863b68 41 const double T = 0.001;
MaikOvermars 0:4cb1de41d049 42
MaikOvermars 0:4cb1de41d049 43 float processing_chain_emg(int num) {
MaikOvermars 0:4cb1de41d049 44 return 6.0;
MaikOvermars 0:4cb1de41d049 45 }
MaikOvermars 0:4cb1de41d049 46
MaikOvermars 0:4cb1de41d049 47 void measure_all()
MaikOvermars 0:4cb1de41d049 48 {
MaikOvermars 0:4cb1de41d049 49 motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
MaikOvermars 0:4cb1de41d049 50 robot_end_point = forwardkinematics_function(motor_angle); //motor_angle is global, this function ne
MaikOvermars 0:4cb1de41d049 51 emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
MaikOvermars 0:4cb1de41d049 52 emg_signal_raw_1 = emg1.read();
MaikOvermars 0:4cb1de41d049 53 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 54 processed_emg_1 = processing_chain_emg(1);
MaikOvermars 0:4cb1de41d049 55 }
MaikOvermars 0:4cb1de41d049 56
MaikOvermars 0:4cb1de41d049 57 void output_all() {
MaikOvermars 0:4cb1de41d049 58 motor1_pwm = fabs(u1);
MaikOvermars 0:4cb1de41d049 59 motor1_dir = u1 > 0.5f;
MaikOvermars 0:4cb1de41d049 60 motor2_pwm = fabs(u2);
MaikOvermars 0:4cb1de41d049 61 motor2_dir = u2 > 0.5f;
MaikOvermars 0:4cb1de41d049 62 static int output_counter = 0;
MaikOvermars 0:4cb1de41d049 63 output_counter++;
MaikOvermars 0:4cb1de41d049 64 if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;} //Print to screen at 10 Hz with MODSERIAL
MaikOvermars 0:4cb1de41d049 65 }
MaikOvermars 0:4cb1de41d049 66
MaikOvermars 0:4cb1de41d049 67 void state_machine() {
MaikOvermars 0:4cb1de41d049 68 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
MaikOvermars 0:4cb1de41d049 69 case waiting: //Nothing useful here, maybe a blinking LED for fun and communication to the user
MaikOvermars 0:4cb1de41d049 70 if (button.read()==true)
MaikOvermars 0:4cb1de41d049 71 {
MaikOvermars 0:4cb1de41d049 72 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
SvenD97 9:8e1112874c12 73 // no state_changed action?
MaikOvermars 0:4cb1de41d049 74 }
MaikOvermars 0:4cb1de41d049 75 break; //to avoid falling through to the next state, although this can sometimes be very useful.
MaikOvermars 0:4cb1de41d049 76
MaikOvermars 0:4cb1de41d049 77 case calib_enc:
MaikOvermars 0:4cb1de41d049 78 if (state_changed==true)
MaikOvermars 0:4cb1de41d049 79 {
MaikOvermars 0:4cb1de41d049 80 state_timer.reset();
MaikOvermars 0:4cb1de41d049 81 state_timer.start();
MaikOvermars 0:4cb1de41d049 82 state_changed = false;
MaikOvermars 0:4cb1de41d049 83 }
MaikOvermars 0:4cb1de41d049 84 u1 = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
MaikOvermars 0:4cb1de41d049 85 // fabs(motor1.velocity()) < 0.1f &&
MaikOvermars 0:4cb1de41d049 86 if (state_timer.read() > 5.0f) {
MaikOvermars 0:4cb1de41d049 87 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
MaikOvermars 0:4cb1de41d049 88 state_changed = true;
MaikOvermars 0:4cb1de41d049 89 }
MaikOvermars 0:4cb1de41d049 90 break;
MaikOvermars 0:4cb1de41d049 91
MaikOvermars 0:4cb1de41d049 92 case calib_emg: //calibrate emg-signals
MaikOvermars 0:4cb1de41d049 93
MaikOvermars 0:4cb1de41d049 94 break;
MaikOvermars 0:4cb1de41d049 95
MaikOvermars 0:4cb1de41d049 96 case operational: //interpreting emg-signals to move the end effector
MaikOvermars 0:4cb1de41d049 97 if (state_changed==true) { int x = 5; }
MaikOvermars 0:4cb1de41d049 98 // example
MaikOvermars 0:4cb1de41d049 99 reference_end_point = robot_end_point + processed_emg_0;
MaikOvermars 0:4cb1de41d049 100 if (button.read() == true) { current_state = demo; }
MaikOvermars 0:4cb1de41d049 101
MaikOvermars 0:4cb1de41d049 102 break;
MaikOvermars 0:4cb1de41d049 103
MaikOvermars 0:4cb1de41d049 104 case demo: //moving according to a specified trajectory
MaikOvermars 0:4cb1de41d049 105
MaikOvermars 0:4cb1de41d049 106 if (button.read() == true) { current_state = demo; }
MaikOvermars 0:4cb1de41d049 107
MaikOvermars 0:4cb1de41d049 108 break;
MaikOvermars 0:4cb1de41d049 109
MaikOvermars 0:4cb1de41d049 110 case failure: //no way to get out
MaikOvermars 0:4cb1de41d049 111 u1 = 0.0f;
MaikOvermars 0:4cb1de41d049 112 break;
MaikOvermars 0:4cb1de41d049 113 }
MaikOvermars 0:4cb1de41d049 114 }
MaikOvermars 0:4cb1de41d049 115
MaikOvermars 0:4cb1de41d049 116 void motor_controller()
MaikOvermars 0:4cb1de41d049 117 {
MaikOvermars 0:4cb1de41d049 118 if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
MaikOvermars 10:7339dca7d604 119 q_ref += inversekinematics_function(reference_end_point)/samplingfreq; //many different states can modify your reference position, so just do the inverse kinematics once, here
MaikOvermars 10:7339dca7d604 120 e1 = q_ref - motor_angle; //tracking error (q_ref - q_meas)
MaikOvermars 10:7339dca7d604 121 e2 = q_ref - motor_angle;
MaikOvermars 10:7339dca7d604 122 PID_controller(e1,e2,u1,u2); //feedback controller or with possibly fancy controller additions...; pass by reference
MaikOvermars 0:4cb1de41d049 123 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
MaikOvermars 0:4cb1de41d049 124 }
MaikOvermars 0:4cb1de41d049 125
MaikOvermars 0:4cb1de41d049 126
MaikOvermars 0:4cb1de41d049 127 void loop_function() {
MaikOvermars 0:4cb1de41d049 128 measure_all(); //measure all signals
MaikOvermars 0:4cb1de41d049 129 state_machine(); //Do relevant state dependent things
MaikOvermars 0:4cb1de41d049 130 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
MaikOvermars 0:4cb1de41d049 131 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
MaikOvermars 0:4cb1de41d049 132 }
MaikOvermars 0:4cb1de41d049 133
MaikOvermars 0:4cb1de41d049 134
MaikOvermars 0:4cb1de41d049 135 int main()
MaikOvermars 0:4cb1de41d049 136 {
MaikOvermars 0:4cb1de41d049 137 pc.baud(115200);
MaikOvermars 0:4cb1de41d049 138 motor1_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 139 motor2_pwm.period_us(60);
MaikOvermars 0:4cb1de41d049 140 current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
MaikOvermars 0:4cb1de41d049 141 u1 = 0.0f; //initial output to motors is 0.
MaikOvermars 10:7339dca7d604 142 u2 = 0.0f;
MaikOvermars 10:7339dca7d604 143 loop_ticker.attach(&loop_function, 1/samplingfreq); //Run the function loop_function 1000 times per second
MaikOvermars 0:4cb1de41d049 144
MaikOvermars 0:4cb1de41d049 145 while (true) { } //Do nothing here (timing purposes)
MaikOvermars 0:4cb1de41d049 146 }