Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 0:59335354afee
--- /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