Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 28:7c7508bdb21f
- Parent:
- 23:4572750a5c59
- Child:
- 29:8e0a7c33e4e7
--- a/main.cpp Mon Oct 28 18:55:22 2019 +0000
+++ b/main.cpp Tue Oct 29 14:14:15 2019 +0000
@@ -16,7 +16,7 @@
// BENODIGD VOOR PROCESS STATE MACHINE
enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
-states currentState = Motors_off;
+states currentState = Motors_off;
bool stateChanged = true; // Make sure the initialization of first state is executed
// INPUTS
@@ -29,10 +29,10 @@
AnalogIn EMG_calf_raw (A2);
QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
-QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);
+QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);
// OUTPUTS
-PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
+PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
PwmOut motor2(D5); // samen kunnen gaan met de servo motor
DigitalOut motor1_dir(D7);
@@ -52,39 +52,39 @@
// DEFINITIES VOOR FILTERS
// BICEPS-RECHTS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqcbr;
-BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
+// Definities voor eerste BiQuadChain (High-pass en Notch)
+BiQuadChain bqcbr;
+BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
BiQuad bqbr2(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):
+// Definieer (twee Low-pass -> vierde orde verkrijgen):
BiQuadChain bqcbr2;
BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
// BICEPS-LINKS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqcbl;
-BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
+// Definities voor eerste BiQuadChain (High-pass en Notch)
+BiQuadChain bqcbl;
+BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
BiQuad bqbl2(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):
+// Definieer (twee Low-pass -> vierde orde verkrijgen):
BiQuadChain bqcbl2;
BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
// KUIT
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqck;
-BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
+// Definities voor eerste BiQuadChain (High-pass en Notch)
+BiQuadChain bqck;
+BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
BiQuad bqk2(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):
+// Definieer (twee Low-pass -> vierde orde verkrijgen):
BiQuadChain bqck2;
BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-// VARIABELEN VOOR EMG + FILTEREN
+// VARIABELEN VOOR EMG + FILTEREN
double filtered_EMG_biceps_right;
double filtered_EMG_biceps_left;
double filtered_EMG_calf;
@@ -97,7 +97,7 @@
double filtered_EMG_biceps_left_abs;
double filtered_EMG_calf_abs;
-double filtered_EMG_biceps_right_total;
+double filtered_EMG_biceps_right_total;
double filtered_EMG_biceps_left_total;
double filtered_EMG_calf_total;
@@ -105,7 +105,7 @@
HIDScope scope(3);
// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
-bool calib = false;
+bool calib = false;
static int i_calib = 0;
double mean_EMG_biceps_right;
@@ -120,337 +120,270 @@
// VOIDS
// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
-// Enige optie is resetten, dan wordt het script opnieuw opgestart.
+// Enige optie is resetten, dan wordt het script opnieuw opgestart.
void emergency()
- {
- loop_ticker.detach();
- motor1.write(0);
- motor2.write(0);
- pc.printf("Ik ga exploderen!!!\r\n");
- }
+{
+ loop_ticker.detach();
+ motor1.write(0);
+ motor2.write(0);
+ pc.printf("Ik ga exploderen!!!\r\n");
+}
-// Motoren uitzetten
+// Motoren uitzetten
void motors_off()
- {
- motor1.write(0);
- motor2.write(0);
- pc.printf("Motoren uit functie\r\n");
- }
-
+{
+ 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");
- }
+{
+ motor1.write(0.9);
+ motor1_dir.write(1);
+ motor2.write(0.1);
+ motor2_dir.write(1);
+ pc.printf("Motoren aan functie\r\n");
+}
// Finite state machine programming (calibration servo motor?)
-void ProcessStateMachine(void)
-{
+void ProcessStateMachine(void)
+{
// Berekenen van de motorhoeken (in radialen)
- counts1 = Encoder1.getPulses();
- counts2 = Encoder2.getPulses();
+ counts1 = Encoder1.getPulses();
+ counts2 = Encoder2.getPulses();
theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
-
+
// Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
- // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
+ // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
-
+
// Vervolgens wordt de absolute waarde hiervan genomen
filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
-
+
// Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
// over het signaal gedaan
filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
-
- // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
- scope.set(0, filtered_EMG_biceps_right);
- scope.set(1, normalized_EMG_biceps_right);
- scope.set(2, filtered_EMG_calf);
+
+ // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
+ scope.set(0, normalized_EMG_biceps_right);
+ scope.set(1, normalized_EMG_biceps_left);
+ scope.set(2, normalized_EMG_calf);
scope.send();
-
- // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
- // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
+
+ // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
+ // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
// 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
- // waarna het gemiddelde wordt bepaald.
- if (calib)
- {
- if (i_calib == 0)
- {
- filtered_EMG_biceps_right_total=0;
- filtered_EMG_biceps_left_total=0;
- filtered_EMG_calf_total=0;
- }
- if (i_calib <= 2500)
- {
- filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
- filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
- filtered_EMG_calf_total+=filtered_EMG_calf;
- i_calib++;
- }
- if (i_calib > 2500)
- {
- mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
- mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
- mean_EMG_calf=filtered_EMG_calf_total/2500.0;
- pc.printf("Ontspan spieren\r\n");
- pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
- calib = false;
- }
- }
-
+ // waarna het gemiddelde wordt bepaald.
+ if (calib) {
+ if (i_calib == 0) {
+ filtered_EMG_biceps_right_total=0;
+ filtered_EMG_biceps_left_total=0;
+ filtered_EMG_calf_total=0;
+ }
+ if (i_calib <= 2500) {
+ filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
+ filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
+ filtered_EMG_calf_total+=filtered_EMG_calf;
+ i_calib++;
+ }
+ if (i_calib > 2500) {
+ mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
+ mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
+ mean_EMG_calf=filtered_EMG_calf_total/2500.0;
+ pc.printf("Ontspan spieren\r\n");
+ pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
+ calib = false;
+ }
+ }
+
// Genormaliseerde EMG's berekenen
normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
-
- switch (currentState)
- {
+
+ // Finite state machine
+ switch (currentState) {
case Motors_off:
-
- if (stateChanged)
- {
+
+ 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
- {
+ }
+ 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
- {
+ if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
emergency();
- }
+ }
break;
-
+
case Calib_motor:
-
- if (stateChanged && Motor_calib_button_pressed.read() == false)
- {
+
+ if (stateChanged && Motor_calib_button_pressed.read() == false) {
theta_h_1_rad = 0;
theta_h_2_rad = 0;
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");
- }
- if (Emergency_button_pressed.read() == false)
- {
+ }
+ if (Emergency_button_pressed.read() == false) {
emergency();
- }
+ }
break;
-
- case Calib_EMG:
-
- if (stateChanged)
- {
- motors_off();
- i_calib = 0;
- calib = true;
- pc.printf("Span spieren aan\r\n");
- stateChanged = false;
- }
-
- if (i_calib > 2500)
- {
- calib = false;
- currentState = Homing;
- stateChanged = true;
- pc.printf("Moving to Homing state\r\n");
- }
-
- if (Emergency_button_pressed.read() == false)
- {
+
+ case Calib_EMG:
+
+ if (stateChanged) {
+ motors_off();
+ i_calib = 0;
+ calib = true;
+ pc.printf("Span spieren aan\r\n");
+ stateChanged = false;
+ }
+
+ if (i_calib > 2500) {
+ calib = false;
+ currentState = Homing;
+ stateChanged = true;
+ pc.printf("Moving to Homing state\r\n");
+ }
+
+ if (Emergency_button_pressed.read() == false) {
emergency();
- }
- break;
-
- case Homing: // NOG NAAR KIJKEN
-
- if (stateChanged)
- {
- // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
+ }
+ break;
+
+ case Homing: // NOG NAAR KIJKEN
+
+ if (stateChanged) {
+ // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
// (inclusief de end-effector) in de juiste home positie wordt gezet
motors_on();
- currentState = Operation_mode;
- stateChanged = true;
- motors_off();
+
+ stateChanged = false;
+ motors_off();
pc.printf("Moving to operation mode \r\n");
}
- if (Emergency_button_pressed.read() == false)
- {
+ if (Emergency_button_pressed.read() == false) {
emergency();
- }
- break;
+ }
+
+ //...
- case Operation_mode: // Overgaan tot emergency wanneer referentie niet
- // overeenkomt met werkelijkheid
+ if (homing done)
+ {
+ currentState = Operation_mode;
+ stateChanged = true;
+ }
+ break;
+
+ case Operation_mode: // Overgaan tot emergency wanneer referentie niet
+ // overeenkomt met werkelijkheid
+
// pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
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 (normalized_EMG_biceps_right >= 0.3)
- {
- motor1.write(0.5);
- motor1_dir.write(1);
- motor2.write(0);
- motor2_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- motor1.write(0.1);
- motor1_dir = !motor1_dir;
- }
- if (normalized_EMG_biceps_left >= 0.3)
- {
- motor2.write(0.9);
- motor2_dir.write(1);
- motor1.write(0);
- motor1_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- motor2.write(0.1);
- motor2_dir = !motor2_dir;
- }
- }
- }
- if (normalized_EMG_biceps_right < 0.3)
- {
- motor1.write(0);
- motor2.write(0);
- if (normalized_EMG_calf >= 0.3)
- {
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- }
- if (normalized_EMG_biceps_left >= 0.3)
- {
- motor2.write(0.9);
- motor2_dir.write(1);
- motor1.write(0);
- motor1_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- }
- }
- }
- if (normalized_EMG_biceps_left >= 0.3)
- {
- motor2.write(0.9);
- motor2_dir.write(1);
- motor1.write(0);
- motor1_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- }
- if (normalized_EMG_biceps_right >= 0.3)
- {
- motor1.write(0.5);
- motor1_dir.write(1);
- motor2.write(0);
- motor2_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- }
- }
- }
- if (normalized_EMG_biceps_left < 0.3)
- {
- motor2.write(0);
- motor1.write(0);
- if (normalized_EMG_biceps_right >= 0.3)
- {
- motor1.write(0.5);
- motor1_dir.write(1);
- motor2.write(0);
- motor2_dir.write(1);
- if (normalized_EMG_calf >= 0.3)
- {
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- }
- }
- }
+
+ // 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 (normalized_EMG_biceps_right >= 0.3) {
+ motor1.write(0.3);
+ motor2.write(0.3);
+ motor1_dir.write(0);
+ motor2_dir.write(0);
+ //if (normalized_EMG_calf >= 0.3)
+ //{
+ //motor1.write(0.1);
+ //motor1_dir = !motor1_dir;
+ //}
+
+ } else if (normalized_EMG_biceps_left >= 0.3) {
+ motor2.write(0.9);
+ motor1.write(0.9);
+ motor1_dir.write(1);
+ motor2_dir.write(1);
+ //if (normalized_EMG_calf >= 0.3)
+ //{
+ // motor1_dir = !motor1_dir;
+ // pc.printf("Richting zou om moeten draaien");
+ // motor2_dir = !motor2_dir;
+ //}
+ } else {
+ motor1.write(0);
+ motor2.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();
- }
+ //if (normalized_EMG_calf >= 0.3)
+ //{
+ // motor1_dir = !motor1_dir;
+ // pc.printf("Richting zou om moeten draaien");
+ // motor2_dir = !motor2_dir;
+ //}
+
+ }
+
+ 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(25);
- // else
- // {
- // currentState = Homing;
- // stateChanged = true;
- // pc.printf("Terug naar de state Homing\r\n");
- // }
+ // 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");
-
- // Chain voor rechter biceps
- bqcbr.add(&bqbr1).add(&bqbr2);
- bqcbr2.add(&bqbr3).add(&bqbr4);
- // Chain voor linker biceps
- bqcbl.add(&bqbl1).add(&bqbl2);
- bqcbl2.add(&bqbl3).add(&bqbl4);
- // Chain voor kuit
- bqck.add(&bqk1).add(&bqk2);
- bqck2.add(&bqk3).add(&bqk4);
+{
+ pc.printf("Opstarten\r\n");
- loop_ticker.attach(&ProcessStateMachine, 0.002f);
-
- while(true)
- {
- // wait(0.2);
- /* do nothing */
- }
- }
\ No newline at end of file
+ // Chain voor rechter biceps
+ bqcbr.add(&bqbr1).add(&bqbr2);
+ bqcbr2.add(&bqbr3).add(&bqbr4);
+ // Chain voor linker biceps
+ bqcbl.add(&bqbl1).add(&bqbl2);
+ bqcbl2.add(&bqbl3).add(&bqbl4);
+ // Chain voor kuit
+ bqck.add(&bqk1).add(&bqk2);
+ bqck2.add(&bqk3).add(&bqk4);
+
+ loop_ticker.attach(&ProcessStateMachine, 0.002f);
+
+ while(true) {
+ // wait(0.2);
+ /* do nothing */
+ }
+}
\ No newline at end of file