Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- joostbonekamp
- Date:
- 2019-10-22
- Revision:
- 23:9eeac9d1ecbe
- Parent:
- 21:bea7c8a08e1d
- Child:
- 24:710d7d99b915
File content as of revision 23:9eeac9d1ecbe:
/* 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 "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(2); //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; InterruptIn but1(SW2); //debounced button on biorobotics shield InterruptIn but2(D9); //debounced button on biorobotics shield AnalogIn EMG1(A0); AnalogIn EMG2(A1); 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 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027); BiQuad bq2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901); //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 float EMG1_filtered; float 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; bool EMG_has_been_calibrated; bool button1_pressed; bool button2_pressed; float filter_value1; int past_speed = 0; void do_nothing() /* Idle state. Used in the beginning, before the calibration states. */ { if (button1_pressed) { state_changed = true; state = s_cali_EMG; 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; } } const int EMG_cali_amount = 1000; float past_EMG_values[EMG_cali_amount]; int past_EMG_count = 0; 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\nTime is %i\r\n", us_ticker_read()); state_changed = false; } if (past_EMG_count < EMG_cali_amount) { past_EMG_values[past_EMG_count] = EMG1_filtered; past_EMG_count++; } else { //calibration has concluded pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read()); float highest = 0.0; 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("Min value: %f\r\n", EMG_values.min); pc.printf("Length: %f\r\n", (float)EMG_cali_amount); pc.printf("Calibration of the EMG is done, lower bound = %f\r\n", 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; } } 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; } float EMG1_raw; float EMG2_raw; void measure_signals() { //only one emg input, reference and plus EMG1_raw = EMG1.read(); EMG2_raw = EMG2.read(); 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 motor_controller() { int speed = schmitt_trigger(EMG1_filtered); if (speed == -1) {speed = past_speed;} past_speed = speed; 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); scope.set(1, past_speed); } 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; } button1_pressed = true; pc.printf("Button 1 pressed \n\r"); } void but2_interrupt() { if(but1.read()) {//both buttons are pressed failure_occurred = true; } button2_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.000 && i < 0.125) {speed = 0;} if (i > 0.250 && i < 0.375) {speed = 1;} if (i > 0.500 && i < 1.000) {speed = 2;} return speed; } int main() { pc.baud(115200); 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; EMG_values.max = 0.01; 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) { wait(0.1f); } }