Maik Overmars / 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

Fork of Robot_Software by Bram Jonkheer

main.cpp@0:ef81b9f14f58, 2018-10-19 (annotated)

Committer:
bjonkheer
Date:
Fri Oct 19 07:50:39 2018 +0000
Revision:
0:ef81b9f14f58
Child:
1:ce487c9929dd
Set-up of the complete software for the robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjonkheer 0:ef81b9f14f58 1 #include "mbed.h"
bjonkheer 0:ef81b9f14f58 2 #include "MODSERIAL.h"
bjonkheer 0:ef81b9f14f58 3 #include "QEI.h"
bjonkheer 0:ef81b9f14f58 4 #include "HIDScope.h"
bjonkheer 0:ef81b9f14f58 5 #include "BiQuad.h"
bjonkheer 0:ef81b9f14f58 6 #include "PID_controller.h"
bjonkheer 0:ef81b9f14f58 7 #include "Servo.h"
bjonkheer 0:ef81b9f14f58 8
bjonkheer 0:ef81b9f14f58 9 //Define objects
bjonkheer 0:ef81b9f14f58 10 AnalogIn emg0( A0 );
bjonkheer 0:ef81b9f14f58 11 AnalogIn emg1( A1 );
bjonkheer 0:ef81b9f14f58 12 HIDScope scope(2);
bjonkheer 0:ef81b9f14f58 13 PwmOut motor1_pwm(D5);
bjonkheer 0:ef81b9f14f58 14 PwmOut motor2_pwm(D7);
bjonkheer 0:ef81b9f14f58 15 DigitalOut motor2_dir(D6);
bjonkheer 0:ef81b9f14f58 16 DigitalOut directionpin(D4);
bjonkheer 0:ef81b9f14f58 17 AnalogIn potmeter1(A2);
bjonkheer 0:ef81b9f14f58 18 AnalogIn potmeter2(A3);
bjonkheer 0:ef81b9f14f58 19 DigitalIn button(D0);
bjonkheer 0:ef81b9f14f58 20 Ticker Sample;
bjonkheer 0:ef81b9f14f58 21 Timer t;
bjonkheer 0:ef81b9f14f58 22
bjonkheer 0:ef81b9f14f58 23 enum States {failure, wait, calib_enc, calib_emg, operational, demo}; //All possible robot states
bjonkheer 0:ef81b9f14f58 24
bjonkheer 0:ef81b9f14f58 25 //Global variables/objects
bjonkheer 0:ef81b9f14f58 26 States current_state;
bjonkheer 0:ef81b9f14f58 27 Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
bjonkheer 0:ef81b9f14f58 28 float u, emg_signal_raw_1, processed_emg_1, robot_end_point, motor1pwm, motor1dir, motor2pwm, motor2dir, motor_angle, motor_counts; //will be set by the motor_controller function
bjonkheer 0:ef81b9f14f58 29
bjonkheer 0:ef81b9f14f58 30 void measure_all()
bjonkheer 0:ef81b9f14f58 31 {
bjonkheer 0:ef81b9f14f58 32 motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
bjonkheer 0:ef81b9f14f58 33 robot_end_point = forwardkinematics_function(); //motor_angle is global, this function ne
bjonkheer 0:ef81b9f14f58 34 emg_signal_raw_1 = emg1.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
bjonkheer 0:ef81b9f14f58 35 emg_signal_raw_2 = emg2.read();
bjonkheer 0:ef81b9f14f58 36 processed_emg_1 = processing_chain_emg_1(); // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
bjonkheer 0:ef81b9f14f58 37 processed_emg_2 = processing_chain_emg_2();
bjonkheer 0:ef81b9f14f58 38 }
bjonkheer 0:ef81b9f14f58 39
bjonkheer 0:ef81b9f14f58 40 void output_all() {
bjonkheer 0:ef81b9f14f58 41 motor1_pwm = fabs(u1);
bjonkheer 0:ef81b9f14f58 42 motor1_dir = u1 > 0.5f;
bjonkheer 0:ef81b9f14f58 43 motor2_pwm = fabs(u2);
bjonkheer 0:ef81b9f14f58 44 motor2_dir = u2 > 0.5f;
bjonkheer 0:ef81b9f14f58 45 output_counter++;
bjonkheer 0:ef81b9f14f58 46 if (output_counter == 100) {serial.printf(“Something something... %f”,u); output_counter = 0;} //Print to screen at 10 Hz with MODSERIAL
bjonkheer 0:ef81b9f14f58 47 }
bjonkheer 0:ef81b9f14f58 48
bjonkheer 0:ef81b9f14f58 49 void state_machine() {
bjonkheer 0:ef81b9f14f58 50 switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
bjonkheer 0:ef81b9f14f58 51 case wait: //Nothing useful here, maybe a blinking LED for fun and communication to the user
bjonkheer 0:ef81b9f14f58 52 if (calib_button.read()==true)
bjonkheer 0:ef81b9f14f58 53 {
bjonkheer 0:ef81b9f14f58 54 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
bjonkheer 0:ef81b9f14f58 55 }
bjonkheer 0:ef81b9f14f58 56 break; //to avoid falling through to the next state, although this can sometimes be very useful.
bjonkheer 0:ef81b9f14f58 57
bjonkheer 0:ef81b9f14f58 58 case calib_enc:
bjonkheer 0:ef81b9f14f58 59 if (state_changed==true)
bjonkheer 0:ef81b9f14f58 60 {
bjonkheer 0:ef81b9f14f58 61 state_time.reset();
bjonkheer 0:ef81b9f14f58 62 state_timer.start();
bjonkheer 0:ef81b9f14f58 63 state_changed = false;
bjonkheer 0:ef81b9f14f58 64 }
bjonkheer 0:ef81b9f14f58 65 u = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
bjonkheer 0:ef81b9f14f58 66 if (fabs(motor1.velocity()) < 0.1f && state_timer.read() > 5.0f) {
bjonkheer 0:ef81b9f14f58 67 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
bjonkheer 0:ef81b9f14f58 68 state_changed = true;
bjonkheer 0:ef81b9f14f58 69 }
bjonkheer 0:ef81b9f14f58 70 break;
bjonkheer 0:ef81b9f14f58 71
bjonkheer 0:ef81b9f14f58 72 case calib_emg: //calibrate emg-signals
bjonkheer 0:ef81b9f14f58 73
bjonkheer 0:ef81b9f14f58 74
bjonkheer 0:ef81b9f14f58 75 case operational: //interpreting emg-signals to move the end effector
bjonkheer 0:ef81b9f14f58 76 if (state_changed==true) { ... }
bjonkheer 0:ef81b9f14f58 77 end_effector_reference_position = some_function(processed_emg);
bjonkheer 0:ef81b9f14f58 78 if (... some state tr. guard ...) { ... }
bjonkheer 0:ef81b9f14f58 79
bjonkheer 0:ef81b9f14f58 80 break;
bjonkheer 0:ef81b9f14f58 81
bjonkheer 0:ef81b9f14f58 82 case demo: //moving according to a specified trajectory
bjonkheer 0:ef81b9f14f58 83
bjonkheer 0:ef81b9f14f58 84 break;
bjonkheer 0:ef81b9f14f58 85
bjonkheer 0:ef81b9f14f58 86 case failure: //no way to get out
bjonkheer 0:ef81b9f14f58 87 u = 0.0f;
bjonkheer 0:ef81b9f14f58 88 break;
bjonkheer 0:ef81b9f14f58 89 }
bjonkheer 0:ef81b9f14f58 90
bjonkheer 0:ef81b9f14f58 91 void motor_controller()
bjonkheer 0:ef81b9f14f58 92 {
bjonkheer 0:ef81b9f14f58 93 if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
bjonkheer 0:ef81b9f14f58 94 q_ref = inverse_kinematics(end_effector_reference_position); //many different states can modify your reference position, so just do the inverse kinematics once, here
bjonkheer 0:ef81b9f14f58 95 e = q_ref – q_meas; //tracking error
bjonkheer 0:ef81b9f14f58 96 u = PID_controller(e); //feedback controller or with possibly fancy controller additions...;
bjonkheer 0:ef81b9f14f58 97 } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
bjonkheer 0:ef81b9f14f58 98 }
bjonkheer 0:ef81b9f14f58 99
bjonkheer 0:ef81b9f14f58 100
bjonkheer 0:ef81b9f14f58 101 void loop_function() {
bjonkheer 0:ef81b9f14f58 102 measure_all(); //measure all signals
bjonkheer 0:ef81b9f14f58 103 state_machine(); //Do relevant state dependent things
bjonkheer 0:ef81b9f14f58 104 motor_controller(); //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
bjonkheer 0:ef81b9f14f58 105 output_all(); //Output relevant signals, messages, screen outputs, LEDs etc.
bjonkheer 0:ef81b9f14f58 106 }
bjonkheer 0:ef81b9f14f58 107
bjonkheer 0:ef81b9f14f58 108 int main()
bjonkheer 0:ef81b9f14f58 109 {
bjonkheer 0:ef81b9f14f58 110 pc.baud(115200);
bjonkheer 0:ef81b9f14f58 111 pwmpin.period_us(60);
bjonkheer 0:ef81b9f14f58 112 current_state = wait; //we start in state ‘wait’ and current_state can be accessed by all functions
bjonkheer 0:ef81b9f14f58 113 u = 0.0f; //initial output to motors is 0.
bjonkheer 0:ef81b9f14f58 114 loop_ticker.attach(&loop_function, 0.001f); //Run the function loop_function 1000 times per second
bjonkheer 0:ef81b9f14f58 115
bjonkheer 0:ef81b9f14f58 116 float counts_per_rotation = 32;
bjonkheer 0:ef81b9f14f58 117
bjonkheer 0:ef81b9f14f58 118 while (true) { } //Do nothing here (timing purposes)
bjonkheer 0:ef81b9f14f58 119 }