Diff: main.cpp

Revision:

0:ef81b9f14f58

Child:

1:ce487c9929dd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 19 07:50:39 2018 +0000
@@ -0,0 +1,119 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "PID_controller.h"
+#include "Servo.h"
+
+//Define objects
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
+HIDScope    scope(2);
+PwmOut      motor1_pwm(D5);
+PwmOut      motor2_pwm(D7);
+DigitalOut  motor2_dir(D6);
+DigitalOut  directionpin(D4);
+AnalogIn    potmeter1(A2);
+AnalogIn    potmeter2(A3);
+DigitalIn   button(D0);
+Ticker Sample;
+Timer t;
+
+enum States {failure, wait, calib_enc, calib_emg, operational, demo}; //All possible robot states
+
+//Global variables/objects
+States current_state;
+Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
+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
+
+void measure_all() 
+{
+    motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
+    robot_end_point = forwardkinematics_function();  //motor_angle is global, this function ne
+    emg_signal_raw_1 = emg1.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
+    emg_signal_raw_2 = emg2.read();
+    processed_emg_1 = processing_chain_emg_1();  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
+    processed_emg_2 = processing_chain_emg_2();
+}
+
+void output_all() {
+    motor1_pwm = fabs(u1);
+    motor1_dir = u1 > 0.5f;
+    motor2_pwm = fabs(u2);
+    motor2_dir = u2 > 0.5f;
+    output_counter++;
+    if (output_counter == 100) {serial.printf(“Something something... %f”,u); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
+}
+
+void state_machine() {
+    switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
+        case wait:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
+            if (calib_button.read()==true) 
+            {
+                current_state = calib_enc; //the NEXT loop we will be in calib_enc state
+            }
+            break; //to avoid falling through to the next state, although this can sometimes be very useful.
+        
+        case calib_enc:
+            if (state_changed==true) 
+            {
+                state_time.reset();
+                state_timer.start();
+                state_changed = false;
+            }
+            u = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
+            if (fabs(motor1.velocity()) < 0.1f && state_timer.read() > 5.0f) {
+                current_state = calib_emg; //the NEXT loop we will be in calib_emg state
+                state_changed = true;
+            }
+            break;
+            
+        case calib_emg:     //calibrate emg-signals
+            
+        
+        case operational:       //interpreting emg-signals to move the end effector
+            if (state_changed==true) { ... }
+            end_effector_reference_position = some_function(processed_emg);
+            if (... some state tr. guard ...) { ... }
+            
+            break;
+            
+        case demo: //moving according to a specified trajectory
+            
+            break;
+        
+        case failure: //no way to get out
+            u = 0.0f;
+            break;
+}
+
+void motor_controller() 
+{
+    if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
+        q_ref = inverse_kinematics(end_effector_reference_position); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        e = q_ref – q_meas; //tracking error
+        u = PID_controller(e); //feedback controller or with possibly fancy controller additions...;
+        } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
+}
+
+
+void loop_function() {
+    measure_all();              //measure all signals
+    state_machine();            //Do relevant state dependent things
+    motor_controller();         //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
+    output_all();               //Output relevant signals, messages, screen outputs, LEDs etc.
+}
+
+int main() 
+{
+    pc.baud(115200);
+    pwmpin.period_us(60);
+    current_state = wait; //we start in state ‘wait’ and current_state can be accessed by all functions
+    u = 0.0f; //initial output to motors is 0.
+    loop_ticker.attach(&loop_function, 0.001f); //Run the function loop_function 1000 times per second
+    
+    float counts_per_rotation = 32;
+    
+    while (true) { }  //Do nothing here (timing purposes)
+}