Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
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