Diff: main.cpp

Revision:

22:afd521069446

Parent:

1:b862262a9d14

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Mon Oct 21 08:20:32 2019 +0000
@@ -1,23 +1,396 @@
+/*
+To-do:
+    Add reference generator
+    fully implement schmitt trigger 
+    Homing 
+    Turning the magnet on/off
+    Inverse kinematics
+    Gravity compensation
+    PID values
+    General program layout
+    better names for EMG input
+*/
+
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "FastPWM.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#define PI 3.14159265
+
+Serial pc(USBTX, USBRX);            //connect to pc
+HIDScope scope(1);                  //HIDScope instance
+DigitalOut motor1_direction(D4);    //rotation motor 1 on shield (always D6)
+FastPWM motor1_pwm(D5);             //pwm 1 on shield (always D7)
+DigitalOut motor2_direction(D7);    //rotation motor 2 on shield (always D4)
+FastPWM motor2_pwm(D6);             //pwm 2 on shield (always D5)
+Ticker loop_ticker;                 //used in main()
+Ticker scope_ticker;
+AnalogIn Pot1(A1);                  //pot 1 on biorobotics shield
+AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
+InterruptIn but1(D10);              //debounced button on biorobotics shield
+InterruptIn but2(D9);               //debounced button on biorobotics shield
+AnalogIn EMG1(A2);
+AnalogIn EMG2(A3);
+
+void check_failure();
+
+QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
+QEI enc2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
+
+BiQuad bq1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
+BiQuad bq2 (0.000198358203463849, 0.000396716406927699, 0.000198358203463849, -1.96262073248799, 0.963423352820821);
+
+//variables
+enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
+States state;                       //using the States enum
+struct actuator_state {
+    float duty_cycle1;              //pwm of 1st motor
+    float duty_cycle2;              //pwm of 2nd motor
+    int dir1;                       //direction of 1st motor
+    int dir2;                       //direction of 2nd motor
+    bool magnet;                    //state of the magnet
+} actuator;
+
+struct EMG_params {
+    float max;  //params of the emg, tbd during calibration
+    float min;
+} EMG_values;
+
+struct PID {
+    float P;
+    float I;
+    float D;
+    float I_counter;
+};
+PID PID1;
+PID PID2;
+
+float dt = 0.001;
+float theta;
+float L;
+int enc1_zero = 0;//the zero position of the encoders, to be determined from the
+int enc2_zero = 0;//encoder calibration
+int EMG1_filtered;
+int EMG2_filtered;
+int enc1_value;
+int enc2_value;
+float error1 = 0.0;
+float error2 = 0.0;
+float last_error1 = 0.0;
+float last_error2 = 0.0;
+float action1;
+float action2;
+bool state_changed = false; //used to see if the state is "starting"
+volatile bool but1_pressed = false;
+volatile bool but2_pressed = false;
+volatile bool failure_occurred = false;
+float pot_1;                //used to keep track of the potentiometer values
+float pot_2;
+bool EMG_has_been_calibrated;
+bool button1_pressed;
+bool button2_pressed;
+const int EMG_cali_amount = 1000;
+float past_EMG_values[EMG_cali_amount];
+int past_EMG_count = 0;
+
+void do_nothing()
+
+/*
+    Idle state. Used in the beginning, before the calibration states.
+*/
+{
+    if (button1_pressed) {
+        state_changed = true;
+        state = s_cali_enc;
+        button1_pressed = false;
+    }
+}
+
+void failure()
+/*
+    Failure mode. This should execute when button 2 is pressed during operation.
+*/
+{
+    if (state_changed) {
+        pc.printf("Something went wrong!\r\n");
+        state_changed = false;
+    }
+}
+
+void cali_EMG()
+/*
+    Calibration of the EMG. Values determined during calibration should be
+    added to the EMG_params instance.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started EMG calibration\r\n");
+        state_changed = false;
+    }
+    if (past_EMG_count < EMG_cali_amount) {
+        past_EMG_values[past_EMG_count] = EMG1_filtered;
+        past_EMG_count++;
+    }
+    else { //calibration is has concluded
+        float sum = 0.0;
+        for(int i = 0; i<EMG_cali_amount; i++) {
+            sum += past_EMG_values[i];
+        }
+        float mean = sum/(float)EMG_cali_amount;
+        EMG_values.min = mean;
+        //calibration done, moving to cali_enc
+        pc.printf("Calibration of the EMG is done, lower bound = %f", mean);
+        EMG_has_been_calibrated = true;
+        state_changed = true;
+        state = s_cali_enc;
+    }
+}
+
+void cali_enc()
+/*
+    Calibration of the encoder. The encoder should be moved to the lowest
+    position for the linear stage and the horizontal postition for the
+    rotating stage.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started encoder calibration\r\n");
+        state_changed = false;
+    }
+    if (button1_pressed) {
+        pc.printf("Encoder has been calibrated");
+        enc1_zero = enc1_value;
+        enc2_zero = enc2_value;
+        button1_pressed = false;
+        state = s_moving_magnet_off;
+        state_changed = true;
+
+    }
+}
+
+void moving_magnet_off()
+/*
+    Moving with the magnet disabled. This is the part from the home position
+    towards the storage of chips.
+*/
+{
+    if (state_changed) {
+        pc.printf("Moving without magnet\r\n");
+        state_changed = false;
+    }
+}
 
-DigitalOut led(LED_RED);
+void moving_magnet_on()
+/*
+    Moving with the magnet enabled. This is the part of the movement from the
+    chip holder to the top of the playing board.
+*/
+{
+    if (state_changed) {
+        pc.printf("Moving with magnet\r\n");
+        state_changed = false;
+    }
+    return;
+}
+void homing()
+/*
+    Dropping the chip and moving towards the rest position.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started homing");
+        state_changed = false;
+    }
+    return;
+}
+
+void measure_signals()
+{
+    //only one emg input, reference and plus
+    float EMG1_raw = EMG1.read();
+    float EMG2_raw = EMG2.read();
+    float filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
+    
+    if (filter_value1 > EMG_values.max) {
+        EMG_values.max = filter_value1;
+    }
+    if (EMG_has_been_calibrated) {
+        EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min);
+    }
+    else {
+        EMG1_filtered = filter_value1;
+    }
+        
+    enc1_value = enc1.getPulses();
+    enc2_value = enc2.getPulses();
+    enc1_value -= enc1_zero;
+    enc2_value -= enc2_zero;
+    theta = (float)(enc1_value)/(float)(8400*2*PI);
+    L     = (float)(enc2_value)/(float)(8400*2*PI);
+    
+
+}
+
+void sample()
+{
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    float emg0_value = EMG1.read(); 
+    float emg1_value = EMG2.read();
+    
+    //double filter_value = bqc.step(emg1_value);
+    float filter_value = fabs(bq2.step(fabs(bq1.step(emg0_value - emg1_value))));
+    if (filter_value > EMG_values.max) {
+        EMG_values.max = filter_value;
+        }
+    if (EMG_values.min > filter_value) {
+        EMG_values.min = filter_value;
+        }
+    
+    filter_value = filter_value-EMG_values.min;
+    filter_value = filter_value/(EMG_values.max-EMG_values.min);
+    
+    scope.set(0, EMG1.read() );
+    scope.set(1, EMG2.read() );
+    scope.set(2, filter_value);
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    *  Ensure that enough channels are available (HIDScope scope( 2 ))
+    *  Finally, send all channels to the PC at once */
+    scope.send();
+    /* To indicate that the function is working, the LED is toggled */
+}
 
-MODSERIAL pc(USBTX, USBRX);
+void motor_controller() {
+    float error1, error2;
+    //P part of the controller
+    float P_action1 = PID1.P * error1;
+    float P_action2 = PID2.P * error2;
+    
+    //I part
+    PID1.I_counter += error1;
+    PID2.I_counter += error2;
+    float I_action1 = PID1.I_counter * PID1.I;
+    float I_action2 = PID2.I_counter * PID2.I;
+    
+    //D part
+    float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
+    float velocity_estimate_2 = (error2-last_error2)/dt;
+    float D_action1 = velocity_estimate_1 * PID1.D;
+    float D_action2 = velocity_estimate_2 * PID2.D;
+    
+    action1 = P_action1 + I_action1 + D_action1;
+    action2 = P_action2 + I_action2 + D_action2;
+        
+    last_error1 = error1;
+    last_error2 = error2;
+}
+
+void output()
+{
+    motor1_direction = actuator.dir1;
+    motor2_direction = actuator.dir2;
+    motor1_pwm.write(actuator.duty_cycle1);
+    motor2_pwm.write(actuator.duty_cycle2);
+    
+    scope.set(0, EMG1_filtered);
+}
+
+void state_machine()
+{
+    check_failure(); //check for an error in the last loop before state machine
+    //run current state
+    switch (state) {
+        case s_idle:
+            do_nothing();
+            break;
+        case s_failure:
+            failure();
+            break;
+        case s_cali_EMG:
+            cali_EMG();
+            break;
+        case s_cali_enc:
+            cali_enc();
+            break;
+        case s_moving_magnet_on:
+            moving_magnet_on();
+            break;
+        case s_moving_magnet_off:
+            moving_magnet_off();
+            break;
+        case s_homing:
+            homing();
+            break;
+    }
+}
+
+void main_loop()
+{
+    measure_signals();
+    state_machine();
+    motor_controller();
+    output();
+}
+
+//Helper functions, not directly called by the main_loop functions or
+//state machines
+void check_failure()
+{
+    //state = s_failure;
+    //state_changed = true;
+}
+
+void but1_interrupt()
+{
+    if(but2.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    but1_pressed = true;
+    pc.printf("Button 1 pressed \n\r");
+}
+
+void but2_interrupt()
+{
+    if(but1.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    but2_pressed = true;
+    pc.printf("Button 2 pressed \n\r");
+}
+
+int schmitt_trigger(float i) 
+{
+    int speed;
+    speed = -1; //default value, this means the state should not change
+    if (i >  0/14 && i <  2/14) {speed = 0;}
+    if (i >  3/14 && i <  5/14) {speed = 1;}
+    if (i >  6/14 && i <  8/14) {speed = 2;}
+    if (i >  9/14 && i < 11/14) {speed = 3;}
+    if (i > 12/14 && i < 14/14) {speed = 4;}    
+    return speed;
+}
 
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
-    
+    pc.printf("Executing main()... \r\n");
+    state = s_idle;
+
+    motor2_pwm.period(1/160000);   // 1/frequency van waarop hij draait
+    motor1_pwm.period(1/160000);   // 1/frequency van waarop hij draait
+
+    actuator.dir1 = 0;
+    actuator.dir2 = 0;
+
+    actuator.magnet = false;
+
+    but1.fall(&but1_interrupt);
+    but2.fall(&but2_interrupt);
+    scope_ticker.attach(&scope, &HIDScope::send, 0.02);
+    loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
+    pc.printf("Main_loop is running\n\r");
     while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
+        wait(0.1f);
     }
-}
+}
\ No newline at end of file