Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp.orig
- Revision:
- 19:1fd39a2afc30
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp.orig Thu Oct 24 09:25:27 2019 +0000 @@ -0,0 +1,259 @@ +#include "mbed.h" +#include "HIDScope.h" +#include "QEI.h" +#include "MODSERIAL.h" +#include "BiQuad.h" +#include "FastPWM.h" +#include <math.h> +#include "Servo.h" + +// Definieer objecten +Serial pc(USBTX, USBRX); + +PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet +PwmOut motor2(D5); // samen kunnen gaan met de servo motor + +DigitalOut motor1_dir(D7); +DigitalOut motor2_dir(D4); + +DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken! +DigitalIn Emergency_button_pressed(D2); + +AnalogIn EMG_biceps_right_raw (A0); +AnalogIn EMG_biceps_left_raw (A1); +AnalogIn EMG_calf_raw (A2); + +Ticker loop_ticker; +Ticker HIDScope_ticker; +Ticker emgSampleTicker; + +HIDScope scope(3); + +BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd +BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass +BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass +BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab + +bool calib = false; // MOGELIJK GAAT HET HIER FOUT +int i_calib = 0; + +void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled + // hierdoor het EMG signaal en het haalt er een filter overheen +{ + float filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read()); + float filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read()); + float filtered_EMG_calf=bqc.step(EMG_calf_raw.read()); + + float filtered_EMG_biceps_right_total=filtered_EMG_biceps_right; + float filtered_EMG_biceps_left_total=filtered_EMG_biceps_left; + float filtered_EMG_calf_total=filtered_EMG_calf; + + if (calib) + { + if (i_calib < 500) + { + filtered_EMG_biceps_right_total=filtered_EMG_biceps_right+filtered_EMG_biceps_right_total; + filtered_EMG_biceps_left_total=filtered_EMG_biceps_left+filtered_EMG_biceps_left_total; + filtered_EMG_calf_total=filtered_EMG_calf+filtered_EMG_calf_total; + i_calib++; + } + if (i_calib >= 500) + { + mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500; + mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500; + mean_EMG_calf=filtered_EMG_calf_total/500; + calib = false; + } + } +} + +void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope + // Wordt eveneens gerund dmv een ticker +{ + /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ + scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read? + scope.set(1, filtered_EMG_biceps_left() ); + scope.set(2, filtered_EMG_calf() ); + /* 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(); + // Eventueel nog een ledje laten branden +} + +// Emergency +void emergency() + { + loop_ticker.detach(); + motor1.write(0); + motor2.write(0); + pc.printf("Ik ga exploderen!!!\r\n"); + // Alles moet uitgaan (evt. een rood LEDje laten branden), moet + // opnieuw worden opgestart. Mogelijk kan dit door de ticker te + // detachen + } + +// Motoren uitzetten +void motors_off() + { + motor1.write(0); + motor2.write(0); + pc.printf("Motoren uit functie\r\n"); + } + +// Motoren aanzetten +void motors_on() + { + motor1.write(0.9); + motor1_dir.write(1); + motor2.write(0.1); + motor1_dir.write(1); + pc.printf("Motoren aan functie\r\n"); + } + +// EMG kalibreren +void emg_calibration() + { + // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal + // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie + // hiervan kan als drempel worden gebruikt voor beweging + + // *Tijd instellen* + // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019 + + // Ledje van kleur laten veranderen + + // MOGELIJK NIET MEER NODIG??? + + } + +// Finite state machine programming (calibration servo motor?) +enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode}; + +states currentState = Motors_off; +bool stateChanged = true; // Make sure the initialization of first state is executed + +void ProcessStateMachine(void) +{ + switch (currentState) + { + case Motors_off: + + if (stateChanged) + { + motors_off(); // functie waarbij motoren uitgaan + stateChanged = false; + pc.printf("Motors off state\r\n"); + } + if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false + { + motors_on(); + currentState = Calib_motor; + stateChanged = true; + pc.printf("Moving to Calib_motor state\r\n"); + } + if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false + { + emergency(); + } + break; + + case Calib_motor: + + if (stateChanged) + { + // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald + currentState = Calib_EMG; + stateChanged = true; + pc.printf("Moving to Calib_EMG state\r\n"); + } + if (Emergency_button_pressed.read() == false) + { + emergency(); + } + break; + + case Calib_EMG: + + motors_off(); + if (stateChanged) + { + // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald + calib = true; + emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen? + if (i_calib >= 500) // of wait(10);? + { + currentState = Homing; + stateChanged = true; + pc.printf("Moving to Homing state\r\n"); + } + if (Emergency_button_pressed.read() == false) + { + emergency(); + } + break; + + case Homing: + + motors_on(); + if (stateChanged) + { + // Ervoor zorgen dat de motoren zo bewegen dat de robotarm + // (inclusief de end-effector) in de juiste home positie wordt gezet + currentState = Operation_mode; + stateChanged = true; + pc.printf("Moving to operation mode \r\n"); + } + if (Emergency_button_pressed.read() == false) + { + emergency(); + } + break; + + case Operation_mode: // Overgaan tot emergency wanneer referentie niet + // overeenkomt met werkelijkheid + + if (stateChanged) + + // Hier moet een functie worden aangeroepen die ervoor zorgt dat + // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, + // zodat de robotarm kan bewegen + + if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false + { + motors_off(); + currentState = Motors_off; + stateChanged = true; + pc.printf("Terug naar de state Motors_off\r\n"); + } + if (Emergency_button_pressed.read() == false) + { + emergency(); + } + // wait(5); + else + { + currentState = Homing; + stateChanged = true; + pc.printf("Terug naar de state Homing\r\n"); + } + break; + + default: + // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety! + motors_off(); + pc.printf("Unknown or uninplemented state reached!\r\n"); + + } +} + +int main(void) + { + pc.printf("Opstarten\r\n"); + bqc.add(&bq1).add(&bq2).add(&bq3); + emgSampleTicker.attach(&emgSampleFilter, 0.01f); + HIDScope_ticker.attach(&sendHIDScope, 0.01f); + loop_ticker.attach(&ProcessStateMachine, 5.0f); + while(true) + { /* do nothing */} + } \ No newline at end of file