![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Fork of statemachineopzet by
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