Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- joostbonekamp
- Date:
- 2019-10-07
- Revision:
- 15:9a1f34bc9958
- Parent:
- 14:4cf17b10e504
- Child:
- 16:696e9cbcc823
File content as of revision 15:9a1f34bc9958:
#include "mbed.h" #include "MODSERIAL.h" #include "FastPWM.h" #include "QEI.h" #define PI 3.14159265 Serial pc(USBTX, USBRX); //connect to pc 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() 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 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken //variables enum States {idle, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, 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 } actuators; struct EMG_params { float idk; //params of the emg, tbd during calibration } emg_values; int enc_zero; //the zero position of the encoders, to be determined from the //encoder calibration //variables used throughout the program bool state_changed = false; //used to see if the state is "starting" volatile bool but1_pressed = false; volatile bool but2_pressed = false; float pot_1; //used to keep track of the potentiometer values float pot_2; void do_nothing() /* Idle state. Used in the beginning, before the calibration states. */ {} 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() /* Calibratioin 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; } } void cali_enc() /* Calibration of the encoder. The encoder should be moved to the lowest position for the linear stage and the most upright postition for the rotating stage. */ { if (state_changed) { pc.printf("Started encoder calibration\r\n"); state_changed = false; } } 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; } return; } 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; } // the funtions needed to run the program void measure_signals() { return; } void do_nothing() { actuator.duty_cycle1 = 0; actuator.duty_cycle2 = 0; //state guard if (but1_pressed) { //this moves the program from the idle to cw state current_state = cw; state_changed = true; //to show next state it can initialize pc.printf("Changed state from idle to cw\r\n"); but1_pressed = false; //reset button 1 } } void output() { return; } void state_machine() { //run current state switch (current_state) { case idle: do_nothing(); break; case failure: failure(); break; case cali_EMG: cali_EMG(); break; case cali_ENC: cali_encoder(); break; case moving_magnet_on: moving_magnet_on(); break; case moving_magnet_off: moving_magnet_off(); break; case 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 but1_interrupt () { but1_pressed = true; pc.printf("Button 1 pressed \n\r"); } void but2_interrupt () { but2_pressed = true; pc.printf("Button 2 pressed \n\r"); } int main() { pc.baud(115200); pc.printf("Executing main()... \r\n"); current_state = idle; motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait actuator.dir1 = 0; actuator.dir2 = 0; actuator.magnet = false; but1.fall(&but1_interrupt); but2.fall(&but2_interrupt); loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz pc.printf("Main_loop is running\n\r"); while (true) {} }