Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 20:a6a5bdd7d118
- Parent:
- 19:1fd39a2afc30
- Child:
- 21:456acc79726c
--- a/main.cpp Thu Oct 24 09:25:27 2019 +0000
+++ b/main.cpp Thu Oct 24 11:46:17 2019 +0000
@@ -24,32 +24,74 @@
AnalogIn EMG_calf_raw (A2);
Ticker loop_ticker;
-<<<<<<< working copy
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;
+HIDScope scope(3);
+
+float mean_EMG_biceps_right;
+float mean_EMG_biceps_left;
+float mean_EMG_calf;
+float normalized_EMG_biceps_right;
+float filtered_EMG_biceps_right;
+float normalized_EMG_biceps_left;
+float filtered_EMG_biceps_left;
+float normalized_EMG_calf;
+float filtered_EMG_calf;
+
+// Definities voor eerste BiQuadChain (High-pass en Notch)
+BiQuadChain bqc;
+BiQuad bq1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
+BiQuad bq2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
+
+// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
+// Definieer (twee Low-pass -> vierde orde verkrijgen):
+BiQuadChain bqc2;
+BiQuad bq3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bq4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+
void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled
- // hierdoor het EMG signaal en het haalt er een filter overheen
+ // hierdoor het EMG signaal en het haalt er een filter overheen.
+ // Het signaal kan tevens via de HIDScope worden gevolgd.
+ // Tenslotte wordt er een stuk script gerund, wanneer de robot
+ // zich in de kalibratie toestand bevindt.
{
- 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_1=bqc.step(EMG_biceps_right_raw.read());
+ float filtered_EMG_biceps_left_1=bqc.step(EMG_biceps_left_raw.read());
+ float filtered_EMG_calf_1=bqc.step(EMG_calf_raw.read());
+
+ float filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
+ float filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
+ float filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
+
+ float filtered_EMG_biceps_right=bqc2.step(filtered_EMG_biceps_right_abs);
+ float filtered_EMG_biceps_left=bqc2.step(filtered_EMG_biceps_left_abs);
+ float filtered_EMG_calf=bqc2.step(filtered_EMG_calf_abs);
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)
+ //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
+//}
+
+ if (calib) // In de kalibratie staat treedt deze loop in werking. De spier wordt
+ // dan maximaal aangespannen (gedurende 5 seconden). De EMG waarden
+ // worden bij elkaar opgeteld, waarna het gemiddelde wordt bepaald.
{
if (i_calib < 500)
{
@@ -60,30 +102,14 @@
}
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;
+ float mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
+ float mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
+ float 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
-}
-=======
->>>>>>> destination
-
// Emergency
void emergency()
{
@@ -113,7 +139,6 @@
motor1_dir.write(1);
pc.printf("Motoren aan functie\r\n");
}
-<<<<<<< working copy
// EMG kalibreren
void emg_calibration()
@@ -130,8 +155,6 @@
// MOGELIJK NIET MEER NODIG???
}
-=======
->>>>>>> destination
// Finite state machine programming (calibration servo motor?)
enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
@@ -185,21 +208,21 @@
if (stateChanged)
{
// Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
-<<<<<<< working copy
+
calib = true;
- emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
+ 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");
}
-=======
+
currentState = Homing;
stateChanged = true;
pc.printf("Moving to Homing state\r\n");
}
->>>>>>> destination
+
if (Emergency_button_pressed.read() == false)
{
emergency();
@@ -232,26 +255,54 @@
// 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;
-
+ float normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
+ float normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
+ float normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
+
+ if (normalized_EMG_biceps_right >= 0.3)
+ {
+ motor1.write(0.5);
+ motor2.write(0);
+ }
+ if (normalized_EMG_biceps_right < 0.3)
+ {
+ motor1.write(0);
+ motor2.write(0);
+ }
+ if (normalized_EMG_biceps_left >= 0.3)
+ {
+ motor2.write(0.9);
+ motor1.write(0);
+ }
+ if (normalized_EMG_biceps_left < 0.3)
+ {
+ motor2.write(0);
+ motor1.write(0);
+ }
+
+ 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();
@@ -263,6 +314,9 @@
int main(void)
{
pc.printf("Opstarten\r\n");
+ bqc.add(&bq1).add(&bq2);
+ bqc2.add(&bq3).add(&bq4);
+ emgSampleTicker.attach(&emgSampleFilter, 0.01f);
loop_ticker.attach(&ProcessStateMachine, 5.0f);
while(true)
{ /* do nothing */}