![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 54:a14acf0d1683
- Parent:
- 53:35282a3e0cad
- Child:
- 55:a4e63ef1587c
diff -r 35282a3e0cad -r a14acf0d1683 main.cpp --- a/main.cpp Thu Oct 31 21:11:16 2019 +0000 +++ b/main.cpp Thu Oct 31 21:17:07 2019 +0000 @@ -557,7 +557,27 @@ } } +/* +------------------------------ EMG SUBSTATE MACHINE ------------------------------ +*/ +void emg_state_machine() +{ + switch(emg_curr_state) { + case emg_wait: + do_emg_wait(); + break; + case emg_cal_MVC: + do_emg_cal(); + break; + case emg_cal_rest: + do_emg_cal(); + break; + case emg_operation: + do_emg_operation(); + break; + } +} /* ------------------------------ MOTOR GLOBAL VARIABLES & CONSTANTS ------------------------------ @@ -781,16 +801,6 @@ motor2Direction = motordir2; } -void changeservo() -{ - servo_button1= extButton1.read(); - if (servo_button1 == 1) { - myservo.SetPosition(2000); - } else { - myservo.SetPosition(1000); - } -} - void motorKillPower() { motor1Power.write(0.0f); @@ -845,26 +855,20 @@ } /* ------------------------------- OPERATION GLOBAL VARIABLES & CONSTANTS ------------------------------ +------------------------------ MOTOR SUBSTATE MACHINE ------------------------------ */ - -/* ------------------------------- OPERATION GLOBAL FUNCTIONS ------------------------------ -*/ -void toggleServo() +void motor_state_machine() { - if ( operation_showcard == true ) { - myservo.SetPosition(2000); - operation_showcard = !operation_showcard; - } - - else { - myservo.SetPosition(1000); - operation_showcard = !operation_showcard; + switch(motor_curr_state) { + case motor_encoder_set: + do_motor_encoder_set(); + break; + case motor_finish: + do_motor_finish(); + break; } } - /* ------------------------------ OPERATION SUBSTATE FUNCTIONS ------------------------------ */ @@ -889,11 +893,6 @@ motorKillPower(); // Disables motor outputs - if ( switch2_pressed == true) { - switch2_pressed = false; - toggleServo(); - } - // State transition guard if ( button1_pressed == true ) { button1_pressed = false; @@ -928,11 +927,6 @@ motorControl(); RKI(); - if ( switch2_pressed == true) { - switch2_pressed = false; - toggleServo(); - } - // State transition guard if ( button1_pressed == true ) { button1_pressed = false; @@ -947,44 +941,6 @@ } /* ------------------------------- EMG SUBSTATE MACHINE ------------------------------ -*/ - -void emg_state_machine() -{ - switch(emg_curr_state) { - case emg_wait: - do_emg_wait(); - break; - case emg_cal_MVC: - do_emg_cal(); - break; - case emg_cal_rest: - do_emg_cal(); - break; - case emg_operation: - do_emg_operation(); - break; - } -} - -/* ------------------------------- MOTOR SUBSTATE MACHINE ------------------------------ -*/ - -void motor_state_machine() -{ - switch(motor_curr_state) { - case motor_encoder_set: - do_motor_encoder_set(); - break; - case motor_finish: - do_motor_finish(); - break; - } -} - -/* ------------------------------ OPERATION SUBSTATE MACHINE ------------------------------ */ @@ -1302,7 +1258,7 @@ } /* ------------------------------- READ SAMPLES ------------------------------ +------------------------------ OTHER MAIN LOOP FUNCTIONS ------------------------------ */ void sampleSignals() { @@ -1350,6 +1306,16 @@ countsPrev2 = counts2af; } +void changeservo() +{ + servo_button1 = extButton1.read(); + if (servo_button1 == 1) { + myservo.SetPosition(2000); + } else { + myservo.SetPosition(1000); + } +} + /* ------------------------------ GLOBAL PROGRAM LOOP ------------------------------ */ @@ -1413,7 +1379,6 @@ //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg); //pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f); //pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f); - //pc.printf("Xe: %f Ye: %f\r\n",xe,ye); wait(0.5f); }