Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- joostbonekamp
- Date:
- 2019-10-25
- Revision:
- 26:3456b03d5bce
- Parent:
- 25:e1577c9e8c7e
- Child:
- 27:4fa916e1f6a9
File content as of revision 26:3456b03d5bce:
/* To-do: Homing Turning the magnet on/off (reading magnet status?) Gravity compensation (if necessary) PID values 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(4); //HIDScope instance DigitalOut motor0_direction(D4); //rotation motor 1 on shield (always D6) FastPWM motor0_pwm(D5); //pwm 1 on shield (always D7) DigitalOut motor1_direction(D7); //rotation motor 2 on shield (always D4) FastPWM motor1_pwm(D6); //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 AnalogIn EMG1_sig(A0); AnalogIn EMG1_ref(A1); AnalogIn EMG2_sig(A2); AnalogIn EMG2_ref(A3); 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, 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 int 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 failure_occurred = false; bool EMG_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; 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() /* 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[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("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, moving to cali_enc pc.printf("Min values: %f %f\r\n", EMG.min[0], EMG.min[1]); pc.printf("Length: %f\r\n", (float)EMG_cali_amount); EMG_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]; button1_pressed = false; state = s_moving_magnet_off; state_changed = true; } theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]); theta_desired[1] = theta[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 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[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]); theta_desired[1] = theta[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]; if (button2_pressed) { pc.printf("positions: (rad, m): %f %f\r\nErrors: %f %f\r\n", theta[0], theta[1], errors[0], errors[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[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]); theta_desired[1] = theta[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[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]); theta_desired[1] = theta[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] = EMG1_sig.read(); EMG_raw[0][1] = EMG1_ref.read(); EMG_raw[1][0] = EMG2_sig.read(); EMG_raw[1][1] = EMG2_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 (filter_value[c] > EMG.max[c]) { EMG.max[c] = filter_value[c]; } if (EMG_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); } theta[1] = theta[1]*(5.05*0.008*2*PI)+0.63; 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; } if (speed[c] == 1){ velocity_desired[c] = 0.01; } if (speed[c] == 2){ velocity_desired[c] = 0.02; } } } 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 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; 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]; 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.0) {actuator.duty_cycle[c] = 1.0;} if (actuator.duty_cycle[c] < 0.0) {actuator.duty_cycle[c] = 0.0;} } } void output() { motor0_direction = actuator.direction[0]*actuator.default_direction[0]; motor1_direction = actuator.direction[1]*actuator.default_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, past_speed[0]); //scope.set(2, EMG_filtered[1]); //scope.set(3, past_speed[1]); } 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() { if (failure_occurred) { 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.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; motor0_pwm.period(1/160000); // 1/frequency van waarop hij draait motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait actuator.direction[0] = 0; actuator.direction[1] = 0; actuator.default_direction[0] = -1; actuator.default_direction[1] = 1; PID.P[0] = 0.0; PID.P[1] = 0.0; PID.I[0] = 0.0; PID.I[1] = 0.0; PID.D[0] = 0.0; PID.D[1] = 0.0; PID.I_counter[0] = 0.0; PID.I_counter[1] = 0.0; actuator.magnet = false; EMG.max[0] = 0.01; EMG.max[1] = 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); } }