Revision 0:59335354afee, committed 2019-10-30

Comitter:

PatrickZieverink

Date:

Wed Oct 30 15:27:06 2019 +0000

Commit message:

Werkend model zonder boundaries 16:27;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tomlankhorst/code/HIDScope/#d23c6edecc49

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/MODSERIAL/#d2a5e26fd658

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,586 @@
+/*
+To-do:
+    calibration with reverse kinematics
+    EMG calibration upper bound
+
+    Homing
+    Turning the magnet on/off (reading magnet status?)
+    Gravity compensation (if necessary)
+    PID values (too large)
+    Boundaries (after verification of the PID values)
+*/
+#include "mbed.h"
+#include "MODSERIAL.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(5);                  //HIDScope instance
+DigitalOut motor0_direction(D7);    //rotation motor 1 on shield (always D6)
+FastPWM motor0_pwm(D6);             //pwm 1 on shield (always D7)
+DigitalOut motor1_direction(D4);    //rotation motor 2 on shield (always D4)
+FastPWM motor1_pwm(D5);             //pwm 2 on shield (always D5)
+Ticker loop_ticker;                 //used in main()
+Ticker scope_ticker;
+InterruptIn but1(SW2);              //button on mbed board
+InterruptIn but2(D9);               //debounced button on biorobotics shield
+InterruptIn but3(D8);               //button 1 on bio shield
+AnalogIn EMG0_sig(A0);
+AnalogIn EMG0_ref(A1);
+AnalogIn EMG1_sig(A2);
+AnalogIn EMG1_ref(A3);
+
+//Joystick test
+
+float direction_switch;
+
+void check_failure();
+int schmitt_trigger(float i);
+
+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_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
+BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
+BiQuad bq2_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
+BiQuad bq2_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
+
+//variables
+enum States {s_idle, s_cali_EMG_low, s_cali_EMG_high, 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_cycle[2];            //pwm of 1st motor
+    bool direction[2];               //direction of 1st motor
+    int default_direction[2];
+    bool magnet;                    //state of the magnet
+} actuator;
+
+struct EMG_params {
+    float max[2];  //params of the emg, tbd during calibration
+    float min[2];
+} EMG;
+
+struct PID_struct {
+    float P[2];
+    float I[2];
+    float D[2];
+    float I_counter[2];
+} PID;
+
+float dt = 0.001;
+float theta[2]; //theta0 = rot, theta1 = lin
+int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
+float EMG_raw[2][2];
+float EMG_filtered[2];
+int enc_value[2];
+float current_error[2] = {0.0, 0.0};
+float errors[2];
+float last_error[2] = {0.0, 0.0};
+float action[2];
+bool state_changed = false; //used to see if the state is "starting"
+volatile bool button1_pressed = false;
+volatile bool button2_pressed = false;
+volatile bool button3_pressed = false;
+volatile bool failure_occurred = false;
+bool EMG_low_has_been_calibrated;
+bool EMG_high_has_been_calibrated;
+float filter_value[2];
+int speed[2];
+int past_speed[2] = {0, 0};
+float velocity_desired[2];
+float theta_desired[2];
+const int EMG_cali_amount = 1000;
+float past_EMG_values[2][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_EMG_low;
+        button1_pressed = false;
+    }
+    errors[0] = 0.0;
+    errors[1] = 0.0;
+}
+
+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;
+    }
+    errors[0] = 0.0;
+    errors[1] = 0.0;
+    PID.I_counter[0] = 0.0;
+    PID.I_counter[1] = 0.0;
+}
+void cali_EMG_low()
+/*
+    Calibration of the EMG. Values determined during calibration should be
+    added to the EMG_params instance.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started low value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
+        state_changed = false;
+    }
+    if (past_EMG_count < EMG_cali_amount) {
+        past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
+        past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
+        past_EMG_count++;
+    } else if (!EMG_low_has_been_calibrated) { //calibration has concluded
+        pc.printf("Low value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
+        float sum[2] = {0.0, 0.0};
+        float mean[2] = {0.0, 0.0};
+        for(int c = 0; c<2; c++) {
+            for(int i = 0; i<EMG_cali_amount; i++) {
+                sum[c] += past_EMG_values[c][i];
+            }
+            mean[c] = sum[c]/(float)EMG_cali_amount;
+            EMG.min[c] = mean[c];
+        }
+
+        //calibration done
+        pc.printf("Low EMG values: %f %f\r\n", EMG.min[0], EMG.min[1]);
+        EMG_low_has_been_calibrated = true;
+    }
+    errors[0] = 0.0;
+    errors[1] = 0.0;
+    PID.I_counter[0] = 0.0;
+    PID.I_counter[1] = 0.0;
+    if (button1_pressed && EMG_low_has_been_calibrated) {// move to high value calibration
+        button1_pressed = false;
+        state = s_cali_EMG_high;
+        state_changed = true;
+    }
+}
+
+void cali_EMG_high()
+/*
+    Calibration of the EMG. Values determined during calibration should be
+    added to the EMG_params instance.
+*/
+{
+    if (state_changed) {
+        pc.printf("Started high value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
+        state_changed = false;
+        past_EMG_count = 0;
+    }
+    if (past_EMG_count < EMG_cali_amount) {
+        past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
+        past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
+        past_EMG_count++;
+    } else { //calibration has concluded
+        pc.printf("High value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
+        float max[2] = {0.0, 0.0};
+        for(int c = 0; c<2; c++) {
+            for(int i = 0; i<EMG_cali_amount; i++) {
+                if (past_EMG_values[c][i] > max[c]) {
+                    max[c] = past_EMG_values[c][i];
+                }
+            }
+            EMG.max[c] = max[c];
+        }
+
+        //calibration done, moving to cali_enc
+        pc.printf("High EMG values: %f %f\r\n", EMG.max[0], EMG.max[1]);
+        EMG_high_has_been_calibrated = true;
+        state_changed = true;
+        state = s_cali_enc;
+    }
+    errors[0] = 0.0;
+    errors[1] = 0.0;
+    PID.I_counter[0] = 0.0;
+    PID.I_counter[1] = 0.0;
+}
+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\r\n");
+        enc_zero[0] = enc_value[0];
+        enc_zero[1] = enc_value[1];//+130990; //the magic number!
+        button1_pressed = false;
+        state = s_moving_magnet_off;
+        state_changed = true;
+    }
+    errors[0] = 1.0e-5*speed[0];
+    errors[1] = 1.0e-5*speed[1];
+}
+
+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;
+    }
+    theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
+    errors[0] = theta_desired[0] - theta[0];
+    errors[1] = theta_desired[1] - theta[1];
+    if (button2_pressed) {
+        pc.printf("positions: (rad, m): %f %f\r\n"
+                  "Errors:              %f %f\r\n"
+                  "Previous actions:    %f %f\r\n"
+                  "Vx, Vy:              %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1], velocity_desired[0],velocity_desired[1]);
+        button2_pressed = false;
+    }
+}
+
+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;
+    }
+    theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
+    errors[0] = theta_desired[0] - theta[0];
+    errors[1] = theta_desired[1] - theta[0];
+}
+void homing()
+
+//    Dropping the chip and moving towards the rest position.
+
+{
+    if (state_changed) {
+        pc.printf("Started homing\r\n");
+        state_changed = false;
+    }
+    theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
+    theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
+    errors[0] = theta_desired[0] - theta[0];
+    errors[1] = theta_desired[1] - theta[0];
+}
+
+void measure_signals()
+{
+    //only one emg input, reference and plus
+    EMG_raw[0][0] = EMG0_sig.read();
+    EMG_raw[0][1] = EMG0_ref.read();
+    EMG_raw[1][0] = EMG1_sig.read();
+    EMG_raw[1][1] = EMG1_ref.read();
+    filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
+    filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
+
+    enc_value[0] = enc1.getPulses();
+    enc_value[1] = enc2.getPulses();
+
+    for(int c = 0; c < 2; c++) {
+        if (EMG_high_has_been_calibrated && EMG_low_has_been_calibrated) {
+            EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
+        } else {
+            EMG_filtered[c] = filter_value[c];
+        }
+        enc_value[c] -= enc_zero[c];
+        theta[c] = (float)(enc_value[c])/(float)(8400)*2*PI;    //Revoluties        Theta 0 en 1 zijn de gemeten waardes hier!
+    }
+    
+    if (EMG_high_has_been_calibrated && EMG_low_has_been_calibrated){
+        if (EMG_filtered[0] > EMG_filtered[1]){
+            EMG_filtered[1] = 0;
+        }
+        else {
+            EMG_filtered[0] = 0;
+        }  
+    }
+    
+    if(button3_pressed) {
+        direction_switch = -1.0*direction_switch;
+        button3_pressed = false;
+    } 
+    
+    
+    theta[1] = theta[1]*(5.05*0.008/2.0/PI)+0.63;
+    theta[0] = theta[0]*-1.0;
+    
+    float y = theta[1]*sin(theta[0]);         // quick y calculation
+
+    for(int c = 0; c<2; c++) {
+        
+        speed[c] = schmitt_trigger(EMG_filtered[c]);
+        if (speed[c] == -1) {
+            speed[c] = past_speed[c];
+        }
+        past_speed[c] = speed[c];
+        if (speed[c] == 0) {
+            velocity_desired[c] = 0.00f*direction_switch;
+        }
+        if (speed[c] == 1) {
+            velocity_desired[c] = 0.01f*direction_switch;
+        }
+        if (speed[c] == 2) {
+            velocity_desired[c] = 0.02f*direction_switch;
+        }
+         
+        // Joystick beweging, comment dan de EMG zooi!
+        /*
+        if ( c == 0){
+            velocity_desired[c] = (vrx.read()*100-50)/50*0.02;
+            if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
+                velocity_desired[c] = 0;
+            }
+        }
+        if ( c == 1){
+            velocity_desired[c] = (vry.read()*100-50)/50*0.02;
+            if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
+                velocity_desired[c] = 0;
+            }
+        }
+        */
+    }
+}
+
+
+void motor_controller()   //s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure
+{
+    float P_action[2];
+    float I_action[2];
+    float D_action[2];
+    float mg;
+    float duty_mg;
+    float velocity_estimate[2];
+    for (int c = 0; c<2; c++) {
+
+        //P action
+        P_action[c] = PID.P[c] * errors[c];
+
+        //I part
+        PID.I_counter[c] += errors[c]*dt;
+        if (PID.I_counter[c] > 0.05) {
+            PID.I_counter[c] = 0.05;
+        }
+        if (PID.I_counter[c] < -0.05) {
+            PID.I_counter[c] = -0.05;
+        }
+        I_action[c] = PID.I_counter[c] * PID.I[c];
+
+        //D part
+        velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
+        D_action[c] = velocity_estimate[c] * PID.D[c];
+
+        action[c] = (P_action[c] + I_action[c] + D_action[c])/dt;
+        last_error[c] = errors[c];
+    }
+    
+    if (theta[0]> 50){
+        mg =  0;
+    } else { mg = (theta[1]-0.125)*9.81*0.200*cos(theta[0]*1.75); }        // zwaartekracht compensatie
+    duty_mg = mg/10.0;
+    action[0] += duty_mg;
+
+    for (int c = 0; c<2; c++) {
+        actuator.direction[c] = (action[c] > 0); //1 if action is positive, 0 otherwise
+        actuator.duty_cycle[c] = fabs(action[c]);
+        if (actuator.duty_cycle[c] > 1.0f) {
+            actuator.duty_cycle[c] = 1.0f;
+        }
+        if (actuator.duty_cycle[c] < 0.0f) {
+            actuator.duty_cycle[c] = 0.0f;
+        }
+    }
+}
+
+void output()
+{
+    for (int c = 0; c <2; c++) {
+        if (actuator.default_direction[c] == false) {
+            if (actuator.direction[c] == 1) {
+                actuator.direction[c] = 0;
+            } else {
+                actuator.direction[c] = 1;
+            }
+        } else {
+            if (actuator.direction[c] == 1) {
+                actuator.direction[c] = 1;
+            } else {
+                actuator.direction[c] = 0;
+            }
+        }
+    }
+
+    motor0_direction = actuator.direction[0];
+    motor1_direction = actuator.direction[1];
+    motor0_pwm.write(actuator.duty_cycle[0]);
+    motor1_pwm.write(actuator.duty_cycle[1]);
+
+    scope.set(0, EMG_filtered[0]);
+    scope.set(1, EMG_filtered[1]);
+    scope.set(2, actuator.duty_cycle[0]);
+    scope.set(3, actuator.duty_cycle[1]);
+    scope.set(4, speed[0]);
+    scope.set(5, speed[1]);
+    /*
+    scope.set(0, actuator.duty_cycle[1]);
+    scope.set(1, theta[1]);
+    scope.set(2, theta_desired[1]);
+    scope.set(3, actuator.duty_cycle[0]);
+    scope.set(4, theta[0]);
+    scope.set(5, errors[0]);
+    */
+}
+
+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_low:
+            cali_EMG_low();
+            break;
+        case s_cali_EMG_high:
+            cali_EMG_high();
+            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()
+{
+    if (failure_occurred) {
+        state = s_failure;
+        state_changed = true;
+    }
+}
+
+void but1_interrupt()
+{
+    if(!but2.read() || !but3.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    button1_pressed = true;
+    pc.printf("Button 1 pressed!\n\r");
+}
+
+void but2_interrupt()
+{
+    if(!but1.read() || !but3.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    button2_pressed = true;
+    pc.printf("Button 2 pressed!\n\r");
+}
+
+void but3_interrupt()
+{
+    if(!but1.read() || !but2.read()) {
+        failure_occurred = true;
+    }
+    button3_pressed = true;
+    
+    pc.printf("Button 3 pressed!\n\r");
+}
+
+int schmitt_trigger(float i)
+{
+    int speed;
+    speed = -1; //default value, this means the state should not change
+    if (i >  0.000f && i < 0.125f) {
+        speed = 0;
+    }
+    if (i >  0.250f && i < 0.375f) {
+        speed = 1;
+    }
+    if (i >  0.500f && i < 1.000f) {
+        speed = 2;
+    }
+    return speed;
+}
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Executing main()... \r\n");
+    state = s_idle;                         // Hij slaat nu dus de calibratie over!
+
+    motor0_pwm.period(1.0f/160000.0f);      // 1/frequency van waarop hij draait
+    motor1_pwm.period(1.0f/160000.0f);      // 1/frequency van waarop hij draait
+
+    actuator.direction[0] = 0;
+    actuator.direction[1] = 0;
+
+    actuator.default_direction[0] = true;
+    actuator.default_direction[1] = false;          // dit is gedubbelcheckt!
+
+    direction_switch = 1.0;
+
+    PID.P[0] = 1.75;
+    PID.P[1] = 0.5;
+    PID.I[0] = 0.0;
+    PID.I[1] = 0.0;
+    PID.D[0] = 0.005;
+    PID.D[1] = 0.0;
+    PID.I_counter[0] = 0.0;
+    PID.I_counter[1] = 0.0;
+
+    theta_desired[0] = 0.0;
+    theta_desired[1] = 0.63;
+
+    actuator.magnet = false;
+    EMG.max[0] = 0.01;
+    EMG.max[1] = 0.01;
+
+    but1.fall(&but1_interrupt);
+    but2.fall(&but2_interrupt);
+    but3.fall(&but3_interrupt);
+    scope_ticker.attach(&scope, &HIDScope::send, 0.05);
+    loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
+    pc.printf("Main_loop is running\n\r");
+    while (true) {
+        wait(0.1f);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 30 15:27:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/d1b4690b3f8b
\ No newline at end of file