Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Revision 2:25d2d6a5e212, committed 2019-12-10
- Comitter:
- Renate
- Date:
- Tue Dec 10 21:05:59 2019 +0000
- Parent:
- 1:b862262a9d14
- Commit message:
- Werkend script mbv Laurens
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r b862262a9d14 -r 25d2d6a5e212 main.cpp
--- a/main.cpp Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp Tue Dec 10 21:05:59 2019 +0000
@@ -1,23 +1,487 @@
#include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#define M_PI 3.14159265358979323846 /* pi */
+#include <math.h>
+// PC CONNECTIONS
+Serial pc(USBTX, USBRX);
+HIDScope scope(6);
+// DIGITAL OUT
+
+
+
+
+
+// DIGITAL IN
+DigitalIn Power_button_pressed(D1); // Definiëren van alle buttons, we gebruiken hiervoor geen InterruptIn, maar DigitalIn.
+InterruptIn Emergency_button_pressed(D2);
+DigitalIn Motor_calib_button_pressed(SW2);
+DigitalIn servo_button_pressed(SW3);
+
+// ANALOG IN
+AnalogIn EMG_biceps_right_raw (A0); // Definiëren van de ruwe EMG-signalen die binnenkomen via AnalogIn.
+AnalogIn EMG_biceps_left_raw (A1); // We gebruiken signalen van de kuit en de linker en rechter biceps.
+AnalogIn EMG_calf_raw (A2);
+
+// ENCODERS
+QEI Encoder1(D13, D12, NC, 64); // Definities voor de encoders op motor 1 (Encoder1) en 2 (Encoder2). Hiervoor wordt de QEI library gebruikt
+QEI Encoder2(D10, D9, NC, 64); // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie.
+// MOTORS
+const float PWMfreq = 18000.0;
+FastPWM motor1(D6); // Definities voor de motorsnelheden door middel van PwmOut. Er kan een getal tussen 0 en 1 worden ingevoerd.
+FastPWM motor2(D5);
+
+DigitalOut motor1_dir(D7); // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere.
+DigitalOut motor2_dir(D4); // Het aantal counts per omwenteling is gelijk aan 64.
+
+
+// TICKERS
+Ticker loop_ticker; // Ticker aanmaken die ervoor zorgt dat de ProcessStateMachine met een frequentie vsn 500 Hz kan worden aangeroepen.
+const float main_loop_freq = 500.0; // Main loop frequency.
+// BENODIGD VOOR PROCESS STATE MACHINE
+enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Demo, Operation_mode}; // Alle states definiëren.
+states currentState = Motors_off; // State waarin wordt begonnen definiëren.
+volatile bool stateChanged = true; // Toevoegen zodat de initialisatie van de eerste state plaatsvindt.
+
+
+// VARIABELEN
+// Motor encoders
+volatile float offset1 = 0.0;
+volatile float offset2 = 0.0;
+volatile float counts1;
+volatile float counts2;
+volatile float theta_h_1_rad; // Actuele motorhoek in radialen (motor 1).
+volatile float theta_h_2_rad; // Actuele motorhoek in radialen (motor 2).
+volatile float q1; // Hoek joint 1 die volgt uit de actuele motorhoeken.
+volatile float q2;
+// VARIABELEN VOOR EMG + FILTEREN
+volatile float filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
+volatile float filtered_EMG_biceps_left_1;
+volatile float filtered_EMG_calf_1;
+
+volatile float filtered_EMG_biceps_right_abs; // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen.
+volatile float filtered_EMG_biceps_left_abs;
+volatile float filtered_EMG_calf_abs;
+
+volatile float filtered_EMG_biceps_right; // Definities voor de gefilterde EMG-signalen, na de tweede filter keten.
+volatile float filtered_EMG_biceps_left;
+volatile float filtered_EMG_calf;
+
+// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
+volatile bool calib = false; // Benodigde bool en 'runner' (i_calib) voor het uitvoeren van de EMG-kalbratie.
+volatile int i_calib = 0;
+
+volatile float filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
+volatile float filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
+volatile float filtered_EMG_calf_total;
+
+volatile float mean_EMG_biceps_right; // Global variables definiëren voor het gemiddelde maximale EMG-signaal, verkregen d.m.v. de EMG-kalibratie.
+volatile float mean_EMG_biceps_left;
+volatile float mean_EMG_calf;
+// CONTROLLER
+// VARIABELEN VOOR OPERATION MODE (RKI)
+volatile float vx; // Geeft de definitie voor de 'desired velocity' in x-richting.
+volatile float vy; // Geeft de definitie voor de 'desired velocity' in y-richting.
+
+volatile float Inverse_jacobian[2][2]; // Matrixen opstellen voor de inverse jacobian en de gewenste snelheden (vx en vy).
+volatile float desired_velocity[2][1];
+volatile float Jacobian_double_prime[2][2];
+volatile float Joint_velocity[2][1] = {{0.0}, {0.0}}; // Nulmatrix opstellen voor de joint snelheden, beginwaarden.
+
+const float delta_t = 0.002; // Tijdsverschil dat wordt gebruikt om de joint velocities te integreren, is gelijk aan de duur van een 'ticker ronde'.
+
+volatile float Joint_1_position = 0.0; // Begindefinities opstellen voor de joint en motor posities.
+volatile float Joint_2_position = 0.0;
+
+volatile float Joint_1_position_prev = 0.0;
+volatile float Joint_2_position_prev = 0.0;
+
+volatile float Motor_1_position = 0.0;
+volatile float Motor_2_position = 0.0;
+
+volatile float error_M1; // Definiëren van de motorerrors, het verschil tussen de daadwerkelijke hoek en de gewenste hoek.
+volatile float error_M2;
+
+// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
+const float Kp = 25.0; // Kp en Ki waarden voor de PI-controller definiëren.
+const float Ki = 0.1; // Zijn theoretisch getest met goede resultaten, in de praktijk konden geen goede tests worden verricht, doordat de
+ // robot oncontroleerbaar gedrag vertoonde.
+volatile float theta_k_1 = 0.0; // Begin definities opstellen voor de motorhoeken na de proportional part.
+volatile float theta_k_2 = 0.0;
+
+volatile float error_integral_1 = 0.0; // Begin definities opstellen voor de integralen van de errors.
+volatile float error_integral_2 = 0.0;
+
+volatile float u_i_1; // Uitkomst variabelen definiëren voor de vermenigvuldiging van de error integraal met Ki.
+volatile float u_i_2;
+
+volatile float theta_t_1; // Variabelen opstellen voor de sommatie van theta_k en u_i.
+volatile float theta_t_2;
+
+volatile float abs_theta_t_1; // Variabele opstellen voor de absolute waarde van theta_t.
+volatile float abs_theta_t_2;
+// DEMO MODE
+volatile int i_demo;
+// EMG FILTERS
+// BICEPS-RECHTS
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
+BiQuadChain bqcbr;
+BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
+BiQuadChain bqcbr2;
+BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+
+// BICEPS-LINKS
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
+BiQuadChain bqcbl;
+BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
+BiQuadChain bqcbl2;
+BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+
+// KUIT
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
+BiQuadChain bqck;
+BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
+BiQuadChain bqck2;
+BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+// Functions
+void motors_off() // Functie waarbij beide motoren worden uitgezet.
+{
+ motor1.write(0);
+ motor2.write(0);
+ pc.printf("Motoren uit functie\r\n");
+}
+
+void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
+{ // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
+ loop_ticker.detach(); // het script opnieuw moet worden opgestart.
+ motor1.write(0.0);
+ motor2.write(0.0);
+ pc.printf("Ik ga exploderen!!!\r\n");
+}
+void EMG_calibration() // Functie die wordt uitgevoerd tijdens de EMG kalibratie. Wordt geinitialiseerd door de bool calib,
+{ // die alleen waar is tijdens de EMG kalibratie. Er wordt gebruikgemaakt van een runner i_calib, zodat
+ if (i_calib == 0) { // EMG-waarden tijdens maximale spierspanning gedurende 5 seconden bij elkaar op worden geteld. Het gemiddelde
+ filtered_EMG_biceps_right_total=0; // hiervan kan worden bepaald door te delen door het aantal samples dat genomen is in dit interval, namelijk 2500.
+ 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;
+ }
+}
+void Inverse_Kinematics() // Functie die de inverse kinematica uitvoert. De inverse Jacobian is afkosmtig uit eerdere berekeningen in Matlab.
+{ // The desired velocity komt voort uit de operation mode, waar hier een waarde aan wordt toegekend.
+ // Defining joint velocities based on calculations of matlab // Outputs van deze functie zijn motorposities, die later worden gebruikt bij het berekenen van een positie error.
+ Jacobian_double_prime[0][0] = cos(q1-3.141592653589793/3.0)*(-1.7E+1/4.0E+1)+(cos(q1+q2-atan(sqrt(3.0)*(8.5E+1/1.37E+2)-4.5E+1/1.37E+2))*1.7093510851226E+2)/4.0E+2;
+ Jacobian_double_prime[0][1] = (cos(q1+q2-atan(sqrt(3.0)*(8.5E+1/1.37E+2)-4.5E+1/1.37E+2))*1.7093510851226E+2)/4.0E+2;
+ Jacobian_double_prime[1][0] = cos(q1+3.141592653589793/6.0)*(1.7E+1/4.0E+1)+(sin(q1+q2-atan(sqrt(3.0)*(8.5E+1/1.37E+2)-4.5E+1/1.37E+2))*2.682835056795381E+3*(sqrt(2.0)*1.7E+1-sqrt(6.0)*3.0))/1.048E+5;
+ Jacobian_double_prime[1][1] = (sin(q1+q2-atan(sqrt(3.0)*(8.5E+1/1.37E+2)-4.5E+1/1.37E+2))*2.682835056795381E+3*(sqrt(2.0)*1.7E+1-sqrt(6.0)*3.0))/1.048E+5;
+ float detJ = Jacobian_double_prime[0][0]*Jacobian_double_prime[1][1] - Jacobian_double_prime[0][1]*Jacobian_double_prime[1][0];
+ Inverse_jacobian[0][0] = (1.0/float(detJ))*Jacobian_double_prime[1][1];
+ Inverse_jacobian[0][1] = -(1.0/float(detJ))*Jacobian_double_prime[0][1];
+ Inverse_jacobian[1][0] = -(1.0/float(detJ))*Jacobian_double_prime[1][0];
+ Inverse_jacobian[1][1] = (1.0/float(detJ))*Jacobian_double_prime[0][0];
+ if (fabs(detJ)<0.01)
+ {
+ Joint_velocity[0][0] = 0.0;
+ Joint_velocity[1][0] = 0.0;
+ }
+ else
+ {
+ Joint_velocity[0][0] = Inverse_jacobian[0][0]*vx + Inverse_jacobian[0][1]*vy;
+ Joint_velocity[1][0] = Inverse_jacobian[1][0]*vx + Inverse_jacobian[1][1]*vy;
+ }
+ scope.set(4,Joint_velocity[0][0]);
+ scope.set(5,Joint_velocity[1][0]);
+ // Integration
+ Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
+ Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
+
+ Joint_1_position_prev = Joint_1_position;
+ Joint_2_position_prev = Joint_2_position;
-DigitalOut led(LED_RED);
+ Motor_1_position = Joint_1_position;
+ Motor_2_position = Joint_1_position + Joint_2_position;
+}
+void Read_all_inputs()
+{
+ // Berekenen van de motorhoeken (in radialen)
+ counts1 = Encoder1.getPulses(); // Verkrijgen van het aantal counts uit de encoders
+ counts2 = Encoder2.getPulses();
+ theta_h_1_rad = ((2.0*M_PI)*(counts1-offset1))/(32.0*131.25); // Van het gemeten aantal counts wordt de offset afgetrokken, zodat het aantal counts in de referentie positie gelijk is aan 0.
+ theta_h_2_rad = ((2.0*M_PI)*(counts2-offset2))/(32.0*131.25); // Vermenigvuldiging met de conversion factor zorgt ervoor dat het aantal counts wordt omgezet naar de huidige motorhoeken.
+
+ // Calculating joint angles based on motor angles (current encoder values)
+ q1 = theta_h_1_rad; // Relatieve hoek joint 1 in radialen.
+ q2 = theta_h_2_rad - theta_h_1_rad; // Relatieve hoek joint 2 in radialen.
+
+ // 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'
+ 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=fabs(filtered_EMG_biceps_right_1);
+ filtered_EMG_biceps_left_abs=fabs(filtered_EMG_biceps_left_1);
+ filtered_EMG_calf_abs=fabs(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);
+}
+void Homing_function() // Functie die ervoor zorgt dat homing wordt uitgevoerd. De motoren worden met een lage snelheid bewogen, tot de motorhoeken
+{
+ float home = 0.0; // weer in de referentie positie staan (0 in ons geval, doordat we compenseren met een offset voor encoder counts). Wanneer
+ if (theta_h_1_rad != home) { // deze positie is bereikt, wordt de snelheid op nul gezet. In de ProcessStateMachine wordt overgegaan naar de volgende state,
+ if (theta_h_1_rad < home) { // wanneer dit voor beide motoren is gebeurd.
+ motor1.write(0.6f);
+ motor1_dir = 0;
+ } else {
+ motor1.write(0.6f);
+ motor1_dir = 1;
+ }
+ }
+ if (fabs(theta_h_1_rad-home)<0.02) {
+ motor1.write(0);
+ }
+ if (theta_h_2_rad != home) {
+ if (theta_h_2_rad < home) {
+ motor2.write(0.6f);
+ motor2_dir = 0;
+ } else {
+ motor2.write(0.6f);
+ motor2_dir = 1;
+ }
+ }
+ if (fabs(theta_h_2_rad-home) < 0.02) {
+ motor2.write(0.0f);
+ }
+ if (fabs(theta_h_2_rad-home) < 0.02 && fabs(theta_h_1_rad-home)<0.02)
+ {
+ motor2.write(0.0f);
+ motor1.write(0.0f);
+ stateChanged = true;
+ }
+}
+void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
+{ // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
+// Proportional part: // voor de motorsnelheden gebruikt.
+ theta_k_1= Kp * error_M1;
+ theta_k_2= Kp * error_M2;
+
+// Integral part
+ error_integral_1 = error_integral_1+ error_M1*delta_t;
+ error_integral_2 = error_integral_2+ error_M2*delta_t;
+ u_i_1= Ki * error_integral_1;
+ u_i_2= Ki * error_integral_2;
+
+// Sum all parts and return it
+ theta_t_1= theta_k_1 + u_i_1;
+ theta_t_2= theta_k_2 + u_i_2;
+}
+void Define_motor_dir() // Deze functie bepaalt de output richtingen die aan de motoren worden gegeven tijdens de operation mode.
+{
+ if (theta_t_1 >= 0) motor1_dir = 0;
+ else motor1_dir = 1;
+ if (theta_t_2 >= 0) motor2_dir = 0;
+ else motor2_dir = 1;
+}
+
+void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt
+{ // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste
+ Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
+ // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
+ error_M1 = Motor_1_position - theta_h_1_rad; // aangeroepen die ervoor zorgt dat de motoren in de juiste richting gaan draaien.
+ error_M2 = Motor_2_position - theta_h_2_rad;
+
+ PI_controller();
-MODSERIAL pc(USBTX, USBRX);
+ abs_theta_t_1 = fabs(theta_t_1);
+ abs_theta_t_2 = fabs(theta_t_2);
+ Define_motor_dir();
+ motor1.write(abs_theta_t_1);
+ motor2.write(abs_theta_t_2);
+}
+void UpdateScope()
+{
+ // 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.set(0,theta_h_1_rad);
+ scope.set(1,theta_h_2_rad);
+ scope.set(2,theta_t_1);
+ scope.set(3,theta_t_2);
+ //scope.set(4,error_M1);
+ //scope.set(5,error_M2);
+ scope.send();
+}
+void ProcessStateMachine()
+{
+ if (calib) {
+ EMG_calibration();
+ }
+ switch(currentState)
+ {
+ case Motors_off:
+ if (stateChanged) {
+ motors_off(); // Functie waarbij motoren uitgaan.
+ stateChanged = false;
+ pc.printf("Motors off state\r\n");
+ }
+ if (Emergency_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
+ emergency(); // Bij het indrukken van de emergency button, wordt overgegaan op de emergency functie.
+ }
+ if (Power_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
+ currentState = Calib_motor; // Er wordt doorgegaan naar de volgende state, wanneer de power button wordt ingedrukt.
+ stateChanged = true;
+ pc.printf("Moving to Calib_motor state\r\n");
+ }
+ break;
+ case Calib_motor:
+ 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;
+ }
+
+ if (Emergency_button_pressed.read() == false) {
+ emergency();
+ }
+ if (Motor_calib_button_pressed.read() == false) {
+ offset1 = counts1;
+ offset2 = counts2;
+ pc.printf("\r\n Offset1: %f, Offset2: %f\r\n",offset1,offset2);
+ currentState = Calib_EMG;
+ stateChanged = true;
+ pc.printf("Moving to Calib_EMG state\r\n");
+ }
+
+ break;
+ case Calib_EMG:
+ 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;
+ }
+
+ if (Emergency_button_pressed.read() == false) {
+ emergency();
+ }
+
+ if (i_calib > 2500) {
+ calib = false;
+ currentState = Homing;
+ stateChanged = true;
+ pc.printf("Moving to Homing state\r\n");
+ }
+ break;
+ case Homing:
+ if (stateChanged)
+ {
+ stateChanged = false;
+ }
+ Homing_function();
+ if (stateChanged) {
+ currentState = Demo; // overgegaan naar de volgende state. De eerste keer dat de ProcessStateMachine in de homing state belandt, is dit al het geval, // waardoor gelijk doorgegaan kan worden naar de operation mode.
+ i_demo = 0.0;
+ }
+ break;
+ case Demo:
+
+ if (stateChanged)
+ {
+ stateChanged = false;
+ }
+ if (i_demo <= 1*main_loop_freq) {
+ vx = 0;
+ vy = 0;
+ }
+ else if (i_demo <= 5*main_loop_freq) {
+ vx = 0.05;
+ vy = 0;
+ }
+ else if (i_demo <= 7*main_loop_freq) {
+ vx = 0;
+ vy = 0.05;
+ }
+ else if (i_demo <= 8*main_loop_freq) {
+ vx = 0.0;
+ vy = -0.02;
+ } else {
+ vx = 0;
+ vy = 0;
+ }
+
+ Controlling_system();
+ i_demo++;
+ break;
+ case Operation_mode:
+
+ break;
+
+ }
+}
+
+// State machine
+
+
+void main_loop()
+{
+ Read_all_inputs();
+ ProcessStateMachine();
+ UpdateScope();
+ //SetPrevious();
+}
+
int main()
{
pc.baud(115200);
pc.printf("\r\nStarting...\r\n\r\n");
-
+ Emergency_button_pressed.fall(&emergency);
+ loop_ticker.attach(&main_loop,1.0/float(main_loop_freq));
+ Power_button_pressed.mode(PullUp);
+ Motor_calib_button_pressed.mode(PullUp);
+ servo_button_pressed.mode(PullUp);
+ bqcbr.add(&bqbr1).add(&bqbr2); // high-pass filter, gevolgd door een notch filter. Na het nemen van de absolute waarde van het bewerkte EMG-signaal
+ bqcbr2.add(&bqbr3).add(&bqbr4); // moet het tweede gedeelte van de filterketen worden toegepast. Hiervoor moeten twee low-pass filters aan elkaar
+ // Chain voor linker biceps // geketend worden.
+ bqcbl.add(&bqbl1).add(&bqbl2);
+ bqcbl2.add(&bqbl3).add(&bqbl4);
+ // Chain voor kuit
+ bqck.add(&bqk1).add(&bqk2);
+ bqck2.add(&bqk3).add(&bqk4);
+ motor1.period(1.0/float(PWMfreq));
+ motor2.period(1.0/float(PWMfreq));
while (true) {
-
- led = !led;
-
- wait_ms(500);
}
}