Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 15:9a1f34bc9958
- Parent:
- 14:4cf17b10e504
- Child:
- 16:696e9cbcc823
--- a/main.cpp Mon Oct 07 13:36:38 2019 +0000 +++ b/main.cpp Mon Oct 07 13:38:54 2019 +0000 @@ -143,181 +143,79 @@ } -<<<<<<< working copy -void kinematics1() // + +void output() { - double length_1; - volatile double theta; - volatile double length_2; + return; +} - class H_matrix //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is) - { - public: - int h1_1_1 = 1; - int h1_1_2 = 0; - float h1_1_3 = sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden - int h1_2_1 = 0; - int h1_2_2 = 1; - float h1_2_3 = cos(theta*PI/180)*(length_1+length_2); - int h1_3_1 = 0; - int h1_3_2 = 0; - int h1_3_3 = 1; - } - H_matrix H; - - class Position_vector1 //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm) - { - public: - float p1_1_1 - float p1_2_1 - float p1_3_1 - } - class Position_vector2 - { - public: - float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1 - float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1 - float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1 +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 motor_controller() + +void main_loop() { - - motor1_direction = actuator.dir1; // Zet de richting goed - motor1_pwm.write(actuator.duty_cycle1); // Zet het op de snelheid van actuator.speed1 - - >>>>>>> merge rev - - <<<<<<< working copy - - void kinematics2() { // - double length_1; - volatile double theta; - volatile double length_2; - - class H_matrix2 //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is) - { - public: - int h2_1_1 = 1; - int h2_1_2 = 0; - float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden - int h2_2_1 = 0; - int h2_2_2 = 1; - float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2); - int h2_3_1 = 0; - int h2_3_2 = 0; - int h2_3_3 = 1; - } - H_matrix H; - - class Position_vector3 //positie vector gezien vanuit het onderste draaipunt - { - public: - float p3_1_1 - float p3_2_1 - float p3_3_1 - } - class Position_vector4 - { - public: - float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1 - float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1 - float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1 - } - H_matrix2 H2 - } - - void rotate_cw() { - if (state_changed) { - pc.printf("State changed to CW\r\n"); - state_changed = false; //reset this so it wont print next loop - } - motor.dir1 = 1; //todo: check if this is actually clockwise - motor.dir2 = 1; //todo: check if this is actually clockwise - - motor.pwm1 = x_input; //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden - motor.pwm2 = y_input; // ook nog niet echt de y dus - //state guard - if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state - current_state = ccw; - state_changed = true; - but1_pressed = false; //reset this - } - - ======= - motor2_direction = actuator.dir2; - motor2_pwm.write(actuator.duty_cycle2); - return; - >>>>>>> merge rev - } - - 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(); - } + 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 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; +void but2_interrupt () +{ + but2_pressed = true; + pc.printf("Button 2 pressed \n\r"); +} - motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait - motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait +int main() +{ + pc.baud(115200); + pc.printf("Executing main()... \r\n"); + current_state = idle; - actuator.dir1 = 0; - actuator.dir2 = 0; + motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait + motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait - actuator.magnet = false; + 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) {} - } \ No newline at end of file + 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) {} +} \ No newline at end of file