Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 39:37744ca57a58
- Parent:
- 38:fb163733c147
- Child:
- 40:b26d19d52d40
--- a/main.cpp Tue Nov 05 12:00:00 2019 +0000 +++ b/main.cpp Tue Nov 05 12:25:04 2019 +0000 @@ -394,10 +394,10 @@ } break; - case Calib_motor: // In deze state wordt de robot arm in de juiste referentie (home) positie geplaatst. - - if (stateChanged) { - pc.printf("Zet motoren handmatig in home positie\r\n"); + case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst. + // Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als + if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts + pc.printf("Zet motoren handmatig in home positie\r\n"); // op dat moment, wordt ervoor gezorgd dat de motorhoeken in de referentiepositie gelijk zijn aan nul. stateChanged = false; } @@ -407,18 +407,17 @@ if (Motor_calib_button_pressed.read() == false) { offset1 = counts1; offset2 = counts2; - pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad); currentState = Calib_EMG; stateChanged = true; pc.printf("Moving to Calib_EMG state\r\n"); } break; - case Calib_EMG: - - if (stateChanged) { - i_calib = 0; - calib = true; + case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib) + // samen met een teller (i_calib) in werking gezet, die ervoor zorgt dat de eerder beschreven EMG-kalibratie + if (stateChanged) { // wordt doorlopen. De spieren worden tijdens de kalibratie gedurende 5 seconden maximaal aangespannen. + i_calib = 0; // Na deze 5 seconden wordt doorgegaan naar de volgende state. De output van deze kalibratie is een gemiddelde + calib = true; // waarde van het EMG-signaal tijdens het maximaal aanspannen. pc.printf("Span spieren aan\r\n"); stateChanged = false; } @@ -435,12 +434,10 @@ } break; - case Homing: - - if (stateChanged) { - // Ervoor zorgen dat de motoren zo bewegen dat de robotarm - // (inclusief de end-effector) in de juiste home positie wordt gezet - stateChanged = false; + case Homing: // In deze state wordt de homing uitgevoerd. De motoren draaien hier op een langzame snelheid, todat de gewenste referentiepositie + // wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt + if (stateChanged) { // overgegaan naar de volgende state. De eerste keer dat de ProcessStateMachine in de homing state belandt, is dit al het geval, + stateChanged = false; // waardoor gelijk doorgegaan kan worden naar de operation mode. } if (Emergency_button_pressed.read() == false) { emergency(); @@ -457,9 +454,9 @@ case Operation_mode: - if (stateChanged) { - motors_off(); - Joint_1_position = 0; + if (stateChanged) { // Aan het begin van de operation mode, wordt aan allerlei variabelen een beginwaarde van 0 toegekend. Dit wordt een keer gedaan + motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is + Joint_1_position = 0; // naar een andere state Joint_2_position = 0; Joint_1_position_prev = Joint_1_position; Joint_2_position_prev = Joint_2_position;