Marijke Zondag
..
Fork of
statemachineopzet
by 
Onno Derkman
Diff: main.cpp
- Revision:
- 1:1541f49d8680
- Parent:
- 0:64c100cd152a
- Child:
- 2:afa78538ef33
--- a/main.cpp Wed Oct 31 10:47:46 2018 +0000
+++ b/main.cpp Fri Nov 02 11:44:30 2018 +0000
@@ -1,80 +1,581 @@
#include "mbed.h"
#include "MODSERIAL.h"
+#include "BiQuad.h"
+//#include "HIDScope.h"
+#include <math.h>
+#include "QEI.h"
-DigitalOut ledr(LED1);
-DigitalOut ledg(LED2);
-DigitalOut ledb(LED3);
-DigitalIn button1(SW2);
-DigitalIn button2(SW3);
-MODSERIAL pc(USBTX, USBRX);
-enum states{WAITING, EMG_CAL, BCM, FAIL};
+//ATTENTION: set mBed to version 151
+// set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
+// set MODSERIAL to version 44
+// set HIDScope to version 7
+// set biquadFilter to version 7
+
+AnalogIn emg0_in (A0); //First raw EMG signal input
+AnalogIn emg1_in (A1); //Second raw EMG signal input
+AnalogIn emg2_in (A2); //Third raw EMG signal input
+
+DigitalIn button1 (SW2);
+DigitalIn button2 (SW3);
+InterruptIn button3 (D10);
+InterruptIn button4 (D11);
+
+DigitalOut directionpin1 (D4);
+DigitalOut directionpin2 (D7);
+
+PwmOut pwmpin1 (D5);
+PwmOut pwmpin2 (D6);
+
+DigitalOut ledr (LED_RED);
+DigitalOut ledb (LED_BLUE);
+DigitalOut ledg (LED_GREEN);
+
+MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
+
+QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING);
+QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING);
+
+//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
+
+//Tickers
+Ticker RKI_tick;
+Ticker DC_tick;
+Ticker movag_tick;
+Ticker emg_tick;
+Ticker print_tick;
+
+//Global variables
+const float T = 0.002f; //Ticker period EMG, engine control
+const float T2 = 0.2f; //Ticker print function
+
+//State machine
+enum states{WAITING, EMG_CAL, CHOOSE, RKI, DC, HOMING, FAIL};
states CurrentState = WAITING;
bool StateChanged = true;
-volatile int x = 0;
bool failure = false;
+bool homing = false;
+bool backRKI = false;
+bool backDC = false;
+int start_v_des_calculate_qref = 0;
+int start_direct_control = 0;
+
+
+//EMG filter
+double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
+double emg0_raw, emg1_raw, emg2_raw;
+double emg0_filt_x, emg1_filt_x, emg2_filt_x;
+const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
+double sum, sum1, sum2, sum3; //variables used to sum elements in array
+double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
+double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
+
+//Calibration variables
+int x = -1; //Start switch, colour LED is blue.
+int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
+const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
+double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
+double Mean0,Mean1,Mean2; //average of maximum tightening
+double Threshold0, Threshold1, Threshold2;
+
+//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
+BiQuadChain emg0filter;
+BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
+
+BiQuadChain emg1filter;
+BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+
+BiQuadChain emg2filter;
+BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
+
+//Variables PID controller
+double PI = 3.14159;
+double Kp1 = 20.0; //Motor 1
+double Ki1 = 1.02;
+double Kd1 = 1.0;
+double encoder_radians1=0;
+double err_integral1 = 0;
+double err_prev1, err_prev2;
+double err1, err2;
+BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass
+
+double Kp2 = 20.0; //Motor 2
+double Ki2 = 1.02;
+double Kd2 = 1.0;
+double encoder_radians2=0;
+double err_integral2 = 0;
+double u1, u2;
+BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
+
+// Inverse Kinematica variables
+//const double L1 = 0.208; // Hoogte van tafel tot joint 1
+//const double L2 = 0.288; // Hoogte van tafel tot joint 2
+const double L3 = 0.212; // Lengte van de arm
+//const double L4 = 0.089; // Afstand van achterkant base tot joint 1
+//const double L5 = 0.030; // Afstand van joint 1 naar joint 2
+const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
+
+// Variërende variabelen inverse kinematics:
+double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
+double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
+
+//double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
+//double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
+
+double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
+
+double q1_ii=0.0; // Reference signal for motorangle q1ref
+double q2_ii=0.0; // Reference signal for motorangle q2ref
+
+double q1_motor;
+double q2_motor;
+
+//---------------------------------Functions-----------------------------------------------//
+
+//---------------------------------EMG calibration + filter--------------------------------//
+void switch_to_calibrate()
+{
+ x++; //Every time function gets called, x increases. Every button press --> new calibration state.
+ //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
+
+ if(x==0) //If x = 0, led is red
+ {
+ ledr = 0;
+ ledb = 1;
+ ledg = 1;
+ }
+ else if (x==1) //If x = 1, led is blue
+ {
+ ledr = 1;
+ ledb = 0;
+ ledg = 1;
+ }
+ else if (x==2) //If x = 2, led is green
+ {
+ ledr = 1;
+ ledb = 1;
+ ledg = 0;
+ }
+ else //If x = 3 or 4, led is white
+ {
+ ledr = 0;
+ ledb = 0;
+ ledg = 0;
+ }
+
+ if(x==4) //Reset back to x = -1
+ {
+ x = -1;
+ emg_cal=0; //reset, motors off
+ }
+}
+
+
+void calibrate(void)
+{
+ switch(x)
+ {
+ case 0: //If calibration state 0:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
+ {
+ StoreCal0[j] = emg0_filt;
+ sum+=StoreCal0[j];
+ wait(0.001f); //Does there need to be a wait?
+ }
+ Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
+ Threshold0 = Mean0*0.8; //Threshold calculation calve = 0.8*mean
+ break; //Stop. Threshold is calculated, we will use this further in the code
+ }
+ case 1: //If calibration state 1:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
+ {
+ StoreCal1[j] = emg1_filt;
+ sum+=StoreCal1[j];
+ wait(0.001f);
+ }
+ Mean1 = sum/sizeCal;
+ Threshold1 = Mean1/2;
+ break;
+ }
+ case 2: //If calibration state 2:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
+ {
+ StoreCal2[j] = emg2_filt;
+ sum+=StoreCal2[j];
+ wait(0.001f);
+ }
+ Mean2 = sum/sizeCal;
+ Threshold2 = Mean2/2;
+ break;
+ }
+ case 3: //EMG is calibrated, robot can be set to Home position.
+ {
+ emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
+
+ wait(0.001f);
+ break;
+ }
+ default: //Ensures nothing happens if x is not 0,1 or 2.
+ {
+ break;
+ }
+ }
+}
+
+void EMGFilter0()
+{
+ emg0_raw = emg0_in.read(); //give name to raw EMG0 data
+ emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
+ emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
+}
+
+void EMGFilter1()
+{
+ emg1_raw = emg1_in.read(); //give name to raw EMG1 data
+ emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
+ emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
+}
+
+void EMGFilter2()
+{
+ emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
+ emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
+ emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
+}
+
+void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
+{
+ for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
+ {
+ StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
+ StoreArray1[i] = StoreArray1[i-1];
+ StoreArray2[i] = StoreArray2[i-1];
+ }
+
+ StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
+ StoreArray1[0] = emg1_filt;
+ StoreArray2[0] = emg2_filt;
+
+ sum1 = 0.0;
+ sum2 = 0.0;
+ sum3 = 0.0;
+
+ for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
+ {
+ sum1 += StoreArray0[a];
+ sum2 += StoreArray1[a];
+ sum3 += StoreArray2[a];
+ }
+
+ movAg0 = sum1/windowsize; //calculates an average in the array
+ movAg1 = sum2/windowsize;
+ movAg2 = sum3/windowsize;
+ //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
+}
+
+void emg_filtered() //Call all filter functions
+{
+ EMGFilter0();
+ EMGFilter1();
+ EMGFilter2();
+}
+
+
+//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
+void PID_control1()
+{
+ //pc.printf("ik doe het, PDI \n\r");
+
+ // Proportional part:
+ double u_k1 = Kp1 * err1;
+
+ //Integral part
+ err_integral1 = err_integral1 + err1 * T;
+ double u_i1 = Ki1 * err_integral1;
+
+ // Derivative part
+ double err_derivative1 = (err1 - err_prev1)/T;
+ double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);
+ double u_d1 = Kd1 * filtered_err_derivative1;
+ err_prev1 = err1;
+
+
+ // Sum all parts and return it
+ u1 = u_k1 + u_i1 + u_d1;
+}
+
+void PID_control2()
+{
+ //pc.printf("ik doe het, PDI \n\r");
+
+ // Proportional part:
+ double u_k2 = Kp2 * err2;
+
+ //Integral part
+ err_integral2 = err_integral2 + err2 * T;
+ double u_i2 = Ki2 * err_integral2;
+
+ // Derivative part
+ double err_derivative2 = (err2 - err_prev2)/T;
+ double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
+ double u_d2 = Kd2 * filtered_err_derivative2;
+ err_prev2 = err2;
+
+
+ // Sum all parts and return it
+ u2 = u_k2 + u_i2 + u_d2;
+}
+
+void engine_control1() //Engine 1 is translational engine, connected with left side pins
+{
+ //pc.printf("ik doe het, engine control 1\n\r");
+ encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
+ //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
+ //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
+ err1 = q1_motor - encoder_radians1;
+ //pc.printf("err1 = %f\n\r",err1);
+ PID_control1(); //PID controller function call
+
+ //pc.printf("u1 = %f\n\r",u1);
+
+ //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
+ //{
+ pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin1.write(u1<0);
+ //}
+ //else
+ // {
+ // pwmpin1 = 0;
+ // }
+}
+
+void engine_control2() //Engine 2 is rotational engine, connected with right side wires
+{
+ encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
+ //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
+ //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
+ err2 = q2_motor - encoder_radians2;
+ //pc.printf("err2 = %f\n\r",err2);
+ PID_control2(); //PID controller function call
+ //pc.printf("u2 = %f\n\r",u2);
+
+ //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
+ // {
+ pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(u2>0);
+ // }
+ //else
+ // {
+ // pwmpin2 = 0;
+ // }
+}
+
+//------------------ Inversed Kinematics --------------------------------//
+
+void inverse_kinematics()
+{
+ //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+
+ //Lq1 = q1ref*r_trans;
+ //Cq2 = q2ref/5.0;
+
+ q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem
+ q2_dot = v_y/(L3*cos(q2ref)); // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie
+
+ q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken
+ q2_ii = q2ref + q2_dot*T;
+
+ q1ref = q1_ii;
+ q2ref = q2_ii;
+
+ q1_motor = -q1ref/r_trans; //omgezet q1 naar -q1, zit waarschijnlijk een foutje in ergens bovenin.
+ q2_motor = q2ref*5.0;
+
+
+ //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
+
+
+ //start_control = 1;
+ engine_control1();
+ engine_control2();
+}
+
+void v_des_calculate_qref()
+{
+ while(start_v_des_calculate_qref == 1)
+ {
+ while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
+ {
+ if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
+ {
+ v_x = 0.5; //movement in +x direction
+ v_y = 0.0;
+
+ ledr = 0; //red
+ ledb = 1;
+ ledg = 1;
+ }
+ else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
+ {
+ v_y = 0.5; //Movement in +y direction
+ v_x = 0.0;
+
+ ledr = 1; //Green
+ ledb = 1;
+ ledg = 0;
+ }
+
+ else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
+ {
+ v_y = 0.0; //Movement in -x direction
+ v_x = -0.5;
+
+ ledr = 0; //Purple
+ ledb = 0;
+ ledg = 1;
+ }
+
+ else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
+ {
+ v_y = -0.5; //Movement in -y direction
+ v_x = 0.0;
+
+ ledr = 1; //Blue
+ ledb = 0;
+ ledg = 1;
+ }
+ else //If not higher than any threshold, motors will not turn at all
+ {
+ v_x = 0;
+ v_y = 0;
+
+ ledr = 0; //White
+ ledb = 0;
+ ledg = 0;
+ }
+
+ inverse_kinematics(); //Call inverse kinematics function
+
+ break;
+ }
+ break;
+ }
+}
+
+void direct_control()
+{
+ while(start_direct_control == 1)
+ {
+ while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
+ {
+ if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
+ {
+ pwmpin1 = 1;
+ directionpin1.write(0); //Translation up
+ }
+ else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
+ {
+ pwmpin2 = 1;
+ directionpin2.write(0); //Rotation up
+ }
+
+ else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
+ {
+ pwmpin1 = 1;
+ directionpin1.write(1); //Translation down
+ }
+
+ else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
+ {
+ pwmpin2 = 1;
+ directionpin2.write(1); //Rotation up
+ }
+ else //If not higher than any threshold, motors will not turn at all
+ {
+ pwmpin1 = 0;
+ pwmpin2 = 0;
+ }
+
+ break;
+ }
+ break;
+ }
+}
+
+void printFunction()
+{
+ pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
+ pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
+}
+
+//---------------------------State machine-------------------------------------------------------------------
void ProcessStateMachine(void)
{
switch(CurrentState)
{
- case WAITING: //This state is used to only start calibrating upon user input
+ case WAITING: //This state is used to only start calibrating upon user input
//State initialization
if(StateChanged)
{
- pc.printf("Entering WAITING\r\n");
+ //pc.printf("Entering WAITING\r\n");
ledr = 0;
ledg = 1;
ledb = 1;
StateChanged = false;
}
//State actions
- pc.printf("Still WAITING\r\n");
+ //pc.printf("Still WAITING\r\n");
while(button1)
- {
- }
+ {;}
//State transition
CurrentState = EMG_CAL;
StateChanged = true;
break; // End of WAITING
- case EMG_CAL: //State for calibrating EMG channels
+
+ case EMG_CAL: //State for calibrating EMG channels
//State initialization
if(StateChanged)
{
- pc.printf("Entering EMG calibration\r\n");
- x = 0;
+ //pc.printf("Entering EMG calibration\r\n");
+ x = -1;
ledr = 1;
ledg = 0;
ledb = 1;
StateChanged = false;
}
//State actions
- pc.printf("EMG calibration starting\r\n");
- wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
- // calibratiestappen voor de emg hier invoegen
- while(x < 4 && !failure)
+ //pc.printf("EMG calibration starting\r\n");
+ //wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
+ while(x < 4 && !failure) //x<4??
{
- if(x == 1)
- {
- //Functie die het eerste EMG signaal calibreert
- }
- if(x == 2)
- {
- //Functie die het tweede EMG signaal calibreert
- }
- if(x == 3)
- {
- //Functie die het derde EMG signaal calibreert
- }
+ button3.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
+ //wait(0.2f); //Wait to avoid bouncing of button
+ button4.rise(calibrate); //Calibrate threshold for 3 muscles
+ //wait(0.2f); //Wait to avoid bouncing of button
+
if(!button2)
{
failure = true;
}
- if(!button1)
- {
- x++;
- pc.printf("EMG calibration %i is now running\r\n", x);
- wait(0.2f);
- }
+
}
//State transition
if(failure)
@@ -84,24 +585,149 @@
}
else
{
- CurrentState = BCM;
+ CurrentState = CHOOSE;
StateChanged = true;
}
break; //End of EMG_CAL state
- case BCM: //State for moving the robot
+
+ case CHOOSE: //State to choose between RKI and DC
+ if(StateChanged)
+ {
+ //pc.printf("Entering Basic Control Mode\r\n");
+ ledr = 1;
+ ledg = 1;
+ ledb = 0;
+ StateChanged = false;
+ }
+ if(!button3)
+ {
+ CurrentState = RKI;
+ StateChanged = true;
+ }
+ else if(!button4)
+ {
+ CurrentState = DC;
+ StateChanged = true;
+ }
+ break; //End of "choose" state, move on to RKI or DC (straight lines or direct control)
+
+ case RKI: //State for moving the robot in straight lines in cartesian space
//State initialization
if(StateChanged)
{
- pc.printf("Entering Basic Control Mode\r\n");
+ //pc.printf("Entering Basic Control Mode\r\n");
ledr = 1;
ledg = 1;
ledb = 0;
StateChanged = false;
}
//State actions
- pc.printf("Basic Control Mode is active\r\n");
- while(button1 && !failure)
+ //pc.printf("Basic Control Mode is active\r\n");
+ while(button1 && !failure &&!homing)
+ {
+ start_v_des_calculate_qref = 1;
+
+
+ if(!button2)
+ {
+ failure = true;
+ }
+ if(!button3)
+ {
+ homing = true;
+ }
+
+ }
+ //State transition
+ if(failure)
+ {
+ pc.printf("Go to fail mode\r\n");
+ start_v_des_calculate_qref = 0;
+ CurrentState = FAIL;
+ StateChanged = true;
+ }
+ else if(homing)
+ {
+ CurrentState = HOMING;
+ start_v_des_calculate_qref = 0; CurrentState = HOMING;
+ StateChanged = true;
+ }
+ else
+ {
+ CurrentState = WAITING;
+ start_v_des_calculate_qref = 0;
+ StateChanged = true;
+ }
+ break; //End of BCM state
+
+ case DC:
+ if(StateChanged)
+ {
+ ledr = 1;
+ ledg = 1;
+ ledb = 0;
+ StateChanged = false;
+ }
+
+ while(button1 && !failure &&!homing)
{
+ start_direct_control = 1;
+
+ if(!button2)
+ {
+ failure = true;
+ }
+
+ if(!button3)
+ {
+ homing = true;
+ }
+ }
+ //State transition
+ if(failure)
+ {
+ start_direct_control = 0;
+ CurrentState = FAIL;
+ StateChanged = true;
+ }
+ else if(homing)
+ {
+ start_direct_control = 0;
+ CurrentState = HOMING;
+ StateChanged = true;
+ }
+ else
+ {
+ CurrentState = WAITING;
+ start_direct_control = 0;
+ StateChanged = true;
+ }
+
+ case HOMING: //Back to encoder position zero
+ if(StateChanged)
+ {
+ ledr = 1;
+ ledg = 1;
+ ledb = 0;
+ StateChanged = false;
+ }
+
+ while(button1 && !failure &&!backRKI &&!backDC)
+ {
+ //Back to encoder position zero function
+
+
+
+ if(!button3)
+ {
+ backRKI = true;
+ }
+
+ if(!button4)
+ {
+ backDC = true;
+ }
+
if(!button2)
{
failure = true;
@@ -110,26 +736,35 @@
//State transition
if(failure)
{
- pc.printf("Go to fail mode\r\n");
CurrentState = FAIL;
StateChanged = true;
}
+ else if(backRKI)
+ {
+ CurrentState = RKI;
+ StateChanged = true;
+ }
+ else if(backDC)
+ {
+ CurrentState = DC;
+ StateChanged = true;
+ }
else
{
CurrentState = WAITING;
+ start_direct_control = 0;
StateChanged = true;
}
- break; //End of BCM state
+
case FAIL: //Fail state
//State initialization
if(StateChanged)
{
- pc.printf("Fail mode is entered\r\n");
failure = false;
StateChanged = false;
}
//State actions
- pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n");
+ //"Fail mode is active. Press button1 to enter WAITING
while(button1)
{
}
@@ -137,9 +772,9 @@
CurrentState = WAITING;
StateChanged = true;
break; //End of FAIL state
+
default: //Emergency state for when something unexpected happens
//motoren uit enzo
- pc.printf("SOS SOS SOS\r\n");
ledr = 0;
ledg = 0;
ledb = 0;
@@ -153,12 +788,29 @@
}
}
+
+//------------------ Start main function --------------------------//
+
+
int main()
{
- pc.baud(115200);
- ledr = 1;
- ledg = 1;
- ledb = 1;
+ pc.baud(115200);
+ pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
+ pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
+
+ emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain
+ emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 );
+ emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 );
+
+ emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
+ movag_tick.attach(&MovAg,T);
+
+ RKI_tick.attach(&v_des_calculate_qref,T); //Ticker RKI
+ DC_tick.attach(&direct_control,T); //Ticker DC
+
+ print_tick.attach(&printFunction,T2);
+ // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
+
while (true)
{
ProcessStateMachine(); //Activates state machine