![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Fork of statemachineopzet by
main.cpp@2:afa78538ef33, 2018-11-02 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Fri Nov 02 14:01:29 2018 +0000
- Revision:
- 2:afa78538ef33
- Parent:
- 1:1541f49d8680
State machine af, nu testen, met homing erin
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
onnoderkman | 0:64c100cd152a | 1 | #include "mbed.h" |
onnoderkman | 0:64c100cd152a | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 1:1541f49d8680 | 3 | #include "BiQuad.h" |
MarijkeZondag | 1:1541f49d8680 | 4 | //#include "HIDScope.h" |
MarijkeZondag | 1:1541f49d8680 | 5 | #include <math.h> |
MarijkeZondag | 1:1541f49d8680 | 6 | #include "QEI.h" |
onnoderkman | 0:64c100cd152a | 7 | |
MarijkeZondag | 1:1541f49d8680 | 8 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 1:1541f49d8680 | 9 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 1:1541f49d8680 | 10 | // set MODSERIAL to version 44 |
MarijkeZondag | 1:1541f49d8680 | 11 | // set HIDScope to version 7 |
MarijkeZondag | 1:1541f49d8680 | 12 | // set biquadFilter to version 7 |
MarijkeZondag | 1:1541f49d8680 | 13 | |
MarijkeZondag | 1:1541f49d8680 | 14 | AnalogIn emg0_in (A0); //First raw EMG signal input |
MarijkeZondag | 1:1541f49d8680 | 15 | AnalogIn emg1_in (A1); //Second raw EMG signal input |
MarijkeZondag | 1:1541f49d8680 | 16 | AnalogIn emg2_in (A2); //Third raw EMG signal input |
MarijkeZondag | 1:1541f49d8680 | 17 | |
MarijkeZondag | 1:1541f49d8680 | 18 | DigitalIn button1 (SW2); |
MarijkeZondag | 1:1541f49d8680 | 19 | DigitalIn button2 (SW3); |
MarijkeZondag | 1:1541f49d8680 | 20 | InterruptIn button3 (D10); |
MarijkeZondag | 1:1541f49d8680 | 21 | InterruptIn button4 (D11); |
MarijkeZondag | 1:1541f49d8680 | 22 | |
MarijkeZondag | 1:1541f49d8680 | 23 | DigitalOut directionpin1 (D4); |
MarijkeZondag | 1:1541f49d8680 | 24 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 1:1541f49d8680 | 25 | |
MarijkeZondag | 1:1541f49d8680 | 26 | PwmOut pwmpin1 (D5); |
MarijkeZondag | 1:1541f49d8680 | 27 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 1:1541f49d8680 | 28 | |
MarijkeZondag | 1:1541f49d8680 | 29 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 1:1541f49d8680 | 30 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 1:1541f49d8680 | 31 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 1:1541f49d8680 | 32 | |
MarijkeZondag | 1:1541f49d8680 | 33 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off |
MarijkeZondag | 1:1541f49d8680 | 34 | |
MarijkeZondag | 1:1541f49d8680 | 35 | QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 1:1541f49d8680 | 36 | QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 1:1541f49d8680 | 37 | |
MarijkeZondag | 1:1541f49d8680 | 38 | //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
MarijkeZondag | 1:1541f49d8680 | 39 | |
MarijkeZondag | 1:1541f49d8680 | 40 | //Tickers |
MarijkeZondag | 1:1541f49d8680 | 41 | Ticker RKI_tick; |
MarijkeZondag | 1:1541f49d8680 | 42 | Ticker DC_tick; |
MarijkeZondag | 1:1541f49d8680 | 43 | Ticker movag_tick; |
MarijkeZondag | 1:1541f49d8680 | 44 | Ticker emg_tick; |
MarijkeZondag | 1:1541f49d8680 | 45 | Ticker print_tick; |
MarijkeZondag | 1:1541f49d8680 | 46 | |
MarijkeZondag | 1:1541f49d8680 | 47 | //Global variables |
MarijkeZondag | 1:1541f49d8680 | 48 | const float T = 0.002f; //Ticker period EMG, engine control |
MarijkeZondag | 1:1541f49d8680 | 49 | const float T2 = 0.2f; //Ticker print function |
MarijkeZondag | 1:1541f49d8680 | 50 | |
MarijkeZondag | 1:1541f49d8680 | 51 | //State machine |
MarijkeZondag | 1:1541f49d8680 | 52 | enum states{WAITING, EMG_CAL, CHOOSE, RKI, DC, HOMING, FAIL}; |
onnoderkman | 0:64c100cd152a | 53 | states CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 54 | bool StateChanged = true; |
onnoderkman | 0:64c100cd152a | 55 | bool failure = false; |
MarijkeZondag | 1:1541f49d8680 | 56 | bool homing = false; |
MarijkeZondag | 1:1541f49d8680 | 57 | bool backRKI = false; |
MarijkeZondag | 1:1541f49d8680 | 58 | bool backDC = false; |
MarijkeZondag | 1:1541f49d8680 | 59 | int start_v_des_calculate_qref = 0; |
MarijkeZondag | 1:1541f49d8680 | 60 | int start_direct_control = 0; |
onnoderkman | 0:64c100cd152a | 61 | |
MarijkeZondag | 1:1541f49d8680 | 62 | |
MarijkeZondag | 1:1541f49d8680 | 63 | |
MarijkeZondag | 1:1541f49d8680 | 64 | //EMG filter |
MarijkeZondag | 1:1541f49d8680 | 65 | double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 1:1541f49d8680 | 66 | double emg0_raw, emg1_raw, emg2_raw; |
MarijkeZondag | 1:1541f49d8680 | 67 | double emg0_filt_x, emg1_filt_x, emg2_filt_x; |
MarijkeZondag | 1:1541f49d8680 | 68 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) |
MarijkeZondag | 1:1541f49d8680 | 69 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 1:1541f49d8680 | 70 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 1:1541f49d8680 | 71 | double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??) |
MarijkeZondag | 1:1541f49d8680 | 72 | |
MarijkeZondag | 1:1541f49d8680 | 73 | //Calibration variables |
MarijkeZondag | 1:1541f49d8680 | 74 | int x = -1; //Start switch, colour LED is blue. |
MarijkeZondag | 1:1541f49d8680 | 75 | int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) |
MarijkeZondag | 1:1541f49d8680 | 76 | const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000 |
MarijkeZondag | 1:1541f49d8680 | 77 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in |
MarijkeZondag | 1:1541f49d8680 | 78 | double Mean0,Mean1,Mean2; //average of maximum tightening |
MarijkeZondag | 1:1541f49d8680 | 79 | double Threshold0, Threshold1, Threshold2; |
MarijkeZondag | 1:1541f49d8680 | 80 | |
MarijkeZondag | 1:1541f49d8680 | 81 | //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) |
MarijkeZondag | 1:1541f49d8680 | 82 | BiQuadChain emg0filter; |
MarijkeZondag | 1:1541f49d8680 | 83 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 84 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 85 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 86 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients |
MarijkeZondag | 1:1541f49d8680 | 87 | |
MarijkeZondag | 1:1541f49d8680 | 88 | BiQuadChain emg1filter; |
MarijkeZondag | 1:1541f49d8680 | 89 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 90 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 91 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 92 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 1:1541f49d8680 | 93 | |
MarijkeZondag | 1:1541f49d8680 | 94 | BiQuadChain emg2filter; |
MarijkeZondag | 1:1541f49d8680 | 95 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 96 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 97 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 1:1541f49d8680 | 98 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 1:1541f49d8680 | 99 | |
MarijkeZondag | 1:1541f49d8680 | 100 | //Variables PID controller |
MarijkeZondag | 1:1541f49d8680 | 101 | double PI = 3.14159; |
MarijkeZondag | 1:1541f49d8680 | 102 | double Kp1 = 20.0; //Motor 1 |
MarijkeZondag | 1:1541f49d8680 | 103 | double Ki1 = 1.02; |
MarijkeZondag | 1:1541f49d8680 | 104 | double Kd1 = 1.0; |
MarijkeZondag | 1:1541f49d8680 | 105 | double encoder_radians1=0; |
MarijkeZondag | 1:1541f49d8680 | 106 | double err_integral1 = 0; |
MarijkeZondag | 1:1541f49d8680 | 107 | double err_prev1, err_prev2; |
MarijkeZondag | 1:1541f49d8680 | 108 | double err1, err2; |
MarijkeZondag | 1:1541f49d8680 | 109 | 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 |
MarijkeZondag | 1:1541f49d8680 | 110 | |
MarijkeZondag | 1:1541f49d8680 | 111 | double Kp2 = 20.0; //Motor 2 |
MarijkeZondag | 1:1541f49d8680 | 112 | double Ki2 = 1.02; |
MarijkeZondag | 1:1541f49d8680 | 113 | double Kd2 = 1.0; |
MarijkeZondag | 1:1541f49d8680 | 114 | double encoder_radians2=0; |
MarijkeZondag | 1:1541f49d8680 | 115 | double err_integral2 = 0; |
MarijkeZondag | 1:1541f49d8680 | 116 | double u1, u2; |
MarijkeZondag | 1:1541f49d8680 | 117 | BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); |
MarijkeZondag | 1:1541f49d8680 | 118 | |
MarijkeZondag | 1:1541f49d8680 | 119 | // Inverse Kinematica variables |
MarijkeZondag | 1:1541f49d8680 | 120 | //const double L1 = 0.208; // Hoogte van tafel tot joint 1 |
MarijkeZondag | 1:1541f49d8680 | 121 | //const double L2 = 0.288; // Hoogte van tafel tot joint 2 |
MarijkeZondag | 1:1541f49d8680 | 122 | const double L3 = 0.212; // Lengte van de arm |
MarijkeZondag | 1:1541f49d8680 | 123 | //const double L4 = 0.089; // Afstand van achterkant base tot joint 1 |
MarijkeZondag | 1:1541f49d8680 | 124 | //const double L5 = 0.030; // Afstand van joint 1 naar joint 2 |
MarijkeZondag | 1:1541f49d8680 | 125 | const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
MarijkeZondag | 1:1541f49d8680 | 126 | |
MarijkeZondag | 1:1541f49d8680 | 127 | // Variërende variabelen inverse kinematics: |
MarijkeZondag | 1:1541f49d8680 | 128 | double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is |
MarijkeZondag | 1:1541f49d8680 | 129 | double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is |
MarijkeZondag | 1:1541f49d8680 | 130 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
MarijkeZondag | 1:1541f49d8680 | 131 | double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals |
MarijkeZondag | 1:1541f49d8680 | 132 | |
MarijkeZondag | 1:1541f49d8680 | 133 | //double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 |
MarijkeZondag | 1:1541f49d8680 | 134 | //double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
MarijkeZondag | 1:1541f49d8680 | 135 | |
MarijkeZondag | 1:1541f49d8680 | 136 | double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken |
MarijkeZondag | 1:1541f49d8680 | 137 | double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken |
MarijkeZondag | 1:1541f49d8680 | 138 | |
MarijkeZondag | 1:1541f49d8680 | 139 | double q1_ii=0.0; // Reference signal for motorangle q1ref |
MarijkeZondag | 1:1541f49d8680 | 140 | double q2_ii=0.0; // Reference signal for motorangle q2ref |
MarijkeZondag | 1:1541f49d8680 | 141 | |
MarijkeZondag | 1:1541f49d8680 | 142 | double q1_motor; |
MarijkeZondag | 1:1541f49d8680 | 143 | double q2_motor; |
MarijkeZondag | 1:1541f49d8680 | 144 | |
MarijkeZondag | 1:1541f49d8680 | 145 | //---------------------------------Functions-----------------------------------------------// |
MarijkeZondag | 1:1541f49d8680 | 146 | |
MarijkeZondag | 1:1541f49d8680 | 147 | //---------------------------------EMG calibration + filter--------------------------------// |
MarijkeZondag | 1:1541f49d8680 | 148 | void switch_to_calibrate() |
MarijkeZondag | 1:1541f49d8680 | 149 | { |
MarijkeZondag | 1:1541f49d8680 | 150 | x++; //Every time function gets called, x increases. Every button press --> new calibration state. |
MarijkeZondag | 1:1541f49d8680 | 151 | //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. |
MarijkeZondag | 1:1541f49d8680 | 152 | |
MarijkeZondag | 1:1541f49d8680 | 153 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 1:1541f49d8680 | 154 | { |
MarijkeZondag | 1:1541f49d8680 | 155 | ledr = 0; |
MarijkeZondag | 1:1541f49d8680 | 156 | ledb = 1; |
MarijkeZondag | 1:1541f49d8680 | 157 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 158 | } |
MarijkeZondag | 1:1541f49d8680 | 159 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 1:1541f49d8680 | 160 | { |
MarijkeZondag | 1:1541f49d8680 | 161 | ledr = 1; |
MarijkeZondag | 1:1541f49d8680 | 162 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 163 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 164 | } |
MarijkeZondag | 1:1541f49d8680 | 165 | else if (x==2) //If x = 2, led is green |
MarijkeZondag | 1:1541f49d8680 | 166 | { |
MarijkeZondag | 1:1541f49d8680 | 167 | ledr = 1; |
MarijkeZondag | 1:1541f49d8680 | 168 | ledb = 1; |
MarijkeZondag | 1:1541f49d8680 | 169 | ledg = 0; |
MarijkeZondag | 1:1541f49d8680 | 170 | } |
MarijkeZondag | 1:1541f49d8680 | 171 | else //If x = 3 or 4, led is white |
MarijkeZondag | 1:1541f49d8680 | 172 | { |
MarijkeZondag | 1:1541f49d8680 | 173 | ledr = 0; |
MarijkeZondag | 1:1541f49d8680 | 174 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 175 | ledg = 0; |
MarijkeZondag | 1:1541f49d8680 | 176 | } |
MarijkeZondag | 1:1541f49d8680 | 177 | |
MarijkeZondag | 1:1541f49d8680 | 178 | if(x==4) //Reset back to x = -1 |
MarijkeZondag | 1:1541f49d8680 | 179 | { |
MarijkeZondag | 1:1541f49d8680 | 180 | x = -1; |
MarijkeZondag | 1:1541f49d8680 | 181 | emg_cal=0; //reset, motors off |
MarijkeZondag | 1:1541f49d8680 | 182 | } |
MarijkeZondag | 1:1541f49d8680 | 183 | } |
MarijkeZondag | 1:1541f49d8680 | 184 | |
MarijkeZondag | 1:1541f49d8680 | 185 | |
MarijkeZondag | 1:1541f49d8680 | 186 | void calibrate(void) |
MarijkeZondag | 1:1541f49d8680 | 187 | { |
MarijkeZondag | 1:1541f49d8680 | 188 | switch(x) |
MarijkeZondag | 1:1541f49d8680 | 189 | { |
MarijkeZondag | 1:1541f49d8680 | 190 | case 0: //If calibration state 0: |
MarijkeZondag | 1:1541f49d8680 | 191 | { |
MarijkeZondag | 1:1541f49d8680 | 192 | sum = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 193 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 |
MarijkeZondag | 1:1541f49d8680 | 194 | { |
MarijkeZondag | 1:1541f49d8680 | 195 | StoreCal0[j] = emg0_filt; |
MarijkeZondag | 1:1541f49d8680 | 196 | sum+=StoreCal0[j]; |
MarijkeZondag | 1:1541f49d8680 | 197 | wait(0.001f); //Does there need to be a wait? |
MarijkeZondag | 1:1541f49d8680 | 198 | } |
MarijkeZondag | 1:1541f49d8680 | 199 | Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) |
MarijkeZondag | 1:1541f49d8680 | 200 | Threshold0 = Mean0*0.8; //Threshold calculation calve = 0.8*mean |
MarijkeZondag | 1:1541f49d8680 | 201 | break; //Stop. Threshold is calculated, we will use this further in the code |
MarijkeZondag | 1:1541f49d8680 | 202 | } |
MarijkeZondag | 1:1541f49d8680 | 203 | case 1: //If calibration state 1: |
MarijkeZondag | 1:1541f49d8680 | 204 | { |
MarijkeZondag | 1:1541f49d8680 | 205 | sum = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 206 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 |
MarijkeZondag | 1:1541f49d8680 | 207 | { |
MarijkeZondag | 1:1541f49d8680 | 208 | StoreCal1[j] = emg1_filt; |
MarijkeZondag | 1:1541f49d8680 | 209 | sum+=StoreCal1[j]; |
MarijkeZondag | 1:1541f49d8680 | 210 | wait(0.001f); |
MarijkeZondag | 1:1541f49d8680 | 211 | } |
MarijkeZondag | 1:1541f49d8680 | 212 | Mean1 = sum/sizeCal; |
MarijkeZondag | 1:1541f49d8680 | 213 | Threshold1 = Mean1/2; |
MarijkeZondag | 1:1541f49d8680 | 214 | break; |
MarijkeZondag | 1:1541f49d8680 | 215 | } |
MarijkeZondag | 1:1541f49d8680 | 216 | case 2: //If calibration state 2: |
MarijkeZondag | 1:1541f49d8680 | 217 | { |
MarijkeZondag | 1:1541f49d8680 | 218 | sum = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 219 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 |
MarijkeZondag | 1:1541f49d8680 | 220 | { |
MarijkeZondag | 1:1541f49d8680 | 221 | StoreCal2[j] = emg2_filt; |
MarijkeZondag | 1:1541f49d8680 | 222 | sum+=StoreCal2[j]; |
MarijkeZondag | 1:1541f49d8680 | 223 | wait(0.001f); |
MarijkeZondag | 1:1541f49d8680 | 224 | } |
MarijkeZondag | 1:1541f49d8680 | 225 | Mean2 = sum/sizeCal; |
MarijkeZondag | 1:1541f49d8680 | 226 | Threshold2 = Mean2/2; |
MarijkeZondag | 1:1541f49d8680 | 227 | break; |
MarijkeZondag | 1:1541f49d8680 | 228 | } |
MarijkeZondag | 1:1541f49d8680 | 229 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 1:1541f49d8680 | 230 | { |
MarijkeZondag | 1:1541f49d8680 | 231 | emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) |
MarijkeZondag | 1:1541f49d8680 | 232 | |
MarijkeZondag | 1:1541f49d8680 | 233 | wait(0.001f); |
MarijkeZondag | 1:1541f49d8680 | 234 | break; |
MarijkeZondag | 1:1541f49d8680 | 235 | } |
MarijkeZondag | 1:1541f49d8680 | 236 | default: //Ensures nothing happens if x is not 0,1 or 2. |
MarijkeZondag | 1:1541f49d8680 | 237 | { |
MarijkeZondag | 1:1541f49d8680 | 238 | break; |
MarijkeZondag | 1:1541f49d8680 | 239 | } |
MarijkeZondag | 1:1541f49d8680 | 240 | } |
MarijkeZondag | 1:1541f49d8680 | 241 | } |
MarijkeZondag | 1:1541f49d8680 | 242 | |
MarijkeZondag | 1:1541f49d8680 | 243 | void EMGFilter0() |
MarijkeZondag | 1:1541f49d8680 | 244 | { |
MarijkeZondag | 1:1541f49d8680 | 245 | emg0_raw = emg0_in.read(); //give name to raw EMG0 data |
MarijkeZondag | 1:1541f49d8680 | 246 | emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 1:1541f49d8680 | 247 | 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. |
MarijkeZondag | 1:1541f49d8680 | 248 | } |
MarijkeZondag | 1:1541f49d8680 | 249 | |
MarijkeZondag | 1:1541f49d8680 | 250 | void EMGFilter1() |
MarijkeZondag | 1:1541f49d8680 | 251 | { |
MarijkeZondag | 1:1541f49d8680 | 252 | emg1_raw = emg1_in.read(); //give name to raw EMG1 data |
MarijkeZondag | 1:1541f49d8680 | 253 | emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 1:1541f49d8680 | 254 | emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. |
MarijkeZondag | 1:1541f49d8680 | 255 | } |
MarijkeZondag | 1:1541f49d8680 | 256 | |
MarijkeZondag | 1:1541f49d8680 | 257 | void EMGFilter2() |
MarijkeZondag | 1:1541f49d8680 | 258 | { |
MarijkeZondag | 1:1541f49d8680 | 259 | emg2_raw = emg2_in.read(); //Give name to raw EMG1 data |
MarijkeZondag | 1:1541f49d8680 | 260 | emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 1:1541f49d8680 | 261 | emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. |
MarijkeZondag | 1:1541f49d8680 | 262 | } |
MarijkeZondag | 1:1541f49d8680 | 263 | |
MarijkeZondag | 1:1541f49d8680 | 264 | void MovAg() //Calculate moving average (MovAg), klopt nog niet!! |
MarijkeZondag | 1:1541f49d8680 | 265 | { |
MarijkeZondag | 1:1541f49d8680 | 266 | for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals |
MarijkeZondag | 1:1541f49d8680 | 267 | { |
MarijkeZondag | 1:1541f49d8680 | 268 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. |
MarijkeZondag | 1:1541f49d8680 | 269 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 1:1541f49d8680 | 270 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 1:1541f49d8680 | 271 | } |
MarijkeZondag | 1:1541f49d8680 | 272 | |
MarijkeZondag | 1:1541f49d8680 | 273 | StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array |
MarijkeZondag | 1:1541f49d8680 | 274 | StoreArray1[0] = emg1_filt; |
MarijkeZondag | 1:1541f49d8680 | 275 | StoreArray2[0] = emg2_filt; |
MarijkeZondag | 1:1541f49d8680 | 276 | |
MarijkeZondag | 1:1541f49d8680 | 277 | sum1 = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 278 | sum2 = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 279 | sum3 = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 280 | |
MarijkeZondag | 1:1541f49d8680 | 281 | for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays |
MarijkeZondag | 1:1541f49d8680 | 282 | { |
MarijkeZondag | 1:1541f49d8680 | 283 | sum1 += StoreArray0[a]; |
MarijkeZondag | 1:1541f49d8680 | 284 | sum2 += StoreArray1[a]; |
MarijkeZondag | 1:1541f49d8680 | 285 | sum3 += StoreArray2[a]; |
MarijkeZondag | 1:1541f49d8680 | 286 | } |
MarijkeZondag | 1:1541f49d8680 | 287 | |
MarijkeZondag | 1:1541f49d8680 | 288 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 1:1541f49d8680 | 289 | movAg1 = sum2/windowsize; |
MarijkeZondag | 1:1541f49d8680 | 290 | movAg2 = sum3/windowsize; |
MarijkeZondag | 1:1541f49d8680 | 291 | //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. |
MarijkeZondag | 1:1541f49d8680 | 292 | } |
MarijkeZondag | 1:1541f49d8680 | 293 | |
MarijkeZondag | 1:1541f49d8680 | 294 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 1:1541f49d8680 | 295 | { |
MarijkeZondag | 1:1541f49d8680 | 296 | EMGFilter0(); |
MarijkeZondag | 1:1541f49d8680 | 297 | EMGFilter1(); |
MarijkeZondag | 1:1541f49d8680 | 298 | EMGFilter2(); |
MarijkeZondag | 1:1541f49d8680 | 299 | } |
MarijkeZondag | 1:1541f49d8680 | 300 | |
MarijkeZondag | 1:1541f49d8680 | 301 | |
MarijkeZondag | 1:1541f49d8680 | 302 | //---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------// |
MarijkeZondag | 1:1541f49d8680 | 303 | void PID_control1() |
MarijkeZondag | 1:1541f49d8680 | 304 | { |
MarijkeZondag | 1:1541f49d8680 | 305 | //pc.printf("ik doe het, PDI \n\r"); |
MarijkeZondag | 1:1541f49d8680 | 306 | |
MarijkeZondag | 1:1541f49d8680 | 307 | // Proportional part: |
MarijkeZondag | 1:1541f49d8680 | 308 | double u_k1 = Kp1 * err1; |
MarijkeZondag | 1:1541f49d8680 | 309 | |
MarijkeZondag | 1:1541f49d8680 | 310 | //Integral part |
MarijkeZondag | 1:1541f49d8680 | 311 | err_integral1 = err_integral1 + err1 * T; |
MarijkeZondag | 1:1541f49d8680 | 312 | double u_i1 = Ki1 * err_integral1; |
MarijkeZondag | 1:1541f49d8680 | 313 | |
MarijkeZondag | 1:1541f49d8680 | 314 | // Derivative part |
MarijkeZondag | 1:1541f49d8680 | 315 | double err_derivative1 = (err1 - err_prev1)/T; |
MarijkeZondag | 1:1541f49d8680 | 316 | double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); |
MarijkeZondag | 1:1541f49d8680 | 317 | double u_d1 = Kd1 * filtered_err_derivative1; |
MarijkeZondag | 1:1541f49d8680 | 318 | err_prev1 = err1; |
MarijkeZondag | 1:1541f49d8680 | 319 | |
MarijkeZondag | 1:1541f49d8680 | 320 | |
MarijkeZondag | 1:1541f49d8680 | 321 | // Sum all parts and return it |
MarijkeZondag | 1:1541f49d8680 | 322 | u1 = u_k1 + u_i1 + u_d1; |
MarijkeZondag | 1:1541f49d8680 | 323 | } |
MarijkeZondag | 1:1541f49d8680 | 324 | |
MarijkeZondag | 1:1541f49d8680 | 325 | void PID_control2() |
MarijkeZondag | 1:1541f49d8680 | 326 | { |
MarijkeZondag | 1:1541f49d8680 | 327 | //pc.printf("ik doe het, PDI \n\r"); |
MarijkeZondag | 1:1541f49d8680 | 328 | |
MarijkeZondag | 1:1541f49d8680 | 329 | // Proportional part: |
MarijkeZondag | 1:1541f49d8680 | 330 | double u_k2 = Kp2 * err2; |
MarijkeZondag | 1:1541f49d8680 | 331 | |
MarijkeZondag | 1:1541f49d8680 | 332 | //Integral part |
MarijkeZondag | 1:1541f49d8680 | 333 | err_integral2 = err_integral2 + err2 * T; |
MarijkeZondag | 1:1541f49d8680 | 334 | double u_i2 = Ki2 * err_integral2; |
MarijkeZondag | 1:1541f49d8680 | 335 | |
MarijkeZondag | 1:1541f49d8680 | 336 | // Derivative part |
MarijkeZondag | 1:1541f49d8680 | 337 | double err_derivative2 = (err2 - err_prev2)/T; |
MarijkeZondag | 1:1541f49d8680 | 338 | double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2); |
MarijkeZondag | 1:1541f49d8680 | 339 | double u_d2 = Kd2 * filtered_err_derivative2; |
MarijkeZondag | 1:1541f49d8680 | 340 | err_prev2 = err2; |
MarijkeZondag | 1:1541f49d8680 | 341 | |
MarijkeZondag | 1:1541f49d8680 | 342 | |
MarijkeZondag | 1:1541f49d8680 | 343 | // Sum all parts and return it |
MarijkeZondag | 1:1541f49d8680 | 344 | u2 = u_k2 + u_i2 + u_d2; |
MarijkeZondag | 1:1541f49d8680 | 345 | } |
MarijkeZondag | 1:1541f49d8680 | 346 | |
MarijkeZondag | 1:1541f49d8680 | 347 | void engine_control1() //Engine 1 is translational engine, connected with left side pins |
MarijkeZondag | 1:1541f49d8680 | 348 | { |
MarijkeZondag | 1:1541f49d8680 | 349 | //pc.printf("ik doe het, engine control 1\n\r"); |
MarijkeZondag | 1:1541f49d8680 | 350 | encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 1:1541f49d8680 | 351 | //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses()); |
MarijkeZondag | 1:1541f49d8680 | 352 | //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1); |
MarijkeZondag | 1:1541f49d8680 | 353 | err1 = q1_motor - encoder_radians1; |
MarijkeZondag | 1:1541f49d8680 | 354 | //pc.printf("err1 = %f\n\r",err1); |
MarijkeZondag | 1:1541f49d8680 | 355 | PID_control1(); //PID controller function call |
MarijkeZondag | 1:1541f49d8680 | 356 | |
MarijkeZondag | 1:1541f49d8680 | 357 | //pc.printf("u1 = %f\n\r",u1); |
MarijkeZondag | 1:1541f49d8680 | 358 | |
MarijkeZondag | 1:1541f49d8680 | 359 | //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 |
MarijkeZondag | 1:1541f49d8680 | 360 | //{ |
MarijkeZondag | 1:1541f49d8680 | 361 | pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 1:1541f49d8680 | 362 | directionpin1.write(u1<0); |
MarijkeZondag | 1:1541f49d8680 | 363 | //} |
MarijkeZondag | 1:1541f49d8680 | 364 | //else |
MarijkeZondag | 1:1541f49d8680 | 365 | // { |
MarijkeZondag | 1:1541f49d8680 | 366 | // pwmpin1 = 0; |
MarijkeZondag | 1:1541f49d8680 | 367 | // } |
MarijkeZondag | 1:1541f49d8680 | 368 | } |
MarijkeZondag | 1:1541f49d8680 | 369 | |
MarijkeZondag | 1:1541f49d8680 | 370 | void engine_control2() //Engine 2 is rotational engine, connected with right side wires |
MarijkeZondag | 1:1541f49d8680 | 371 | { |
MarijkeZondag | 1:1541f49d8680 | 372 | encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 1:1541f49d8680 | 373 | //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses()); |
MarijkeZondag | 1:1541f49d8680 | 374 | //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2); |
MarijkeZondag | 1:1541f49d8680 | 375 | err2 = q2_motor - encoder_radians2; |
MarijkeZondag | 1:1541f49d8680 | 376 | //pc.printf("err2 = %f\n\r",err2); |
MarijkeZondag | 1:1541f49d8680 | 377 | PID_control2(); //PID controller function call |
MarijkeZondag | 1:1541f49d8680 | 378 | //pc.printf("u2 = %f\n\r",u2); |
MarijkeZondag | 1:1541f49d8680 | 379 | |
MarijkeZondag | 1:1541f49d8680 | 380 | //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts |
MarijkeZondag | 1:1541f49d8680 | 381 | // { |
MarijkeZondag | 1:1541f49d8680 | 382 | pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 1:1541f49d8680 | 383 | directionpin2.write(u2>0); |
MarijkeZondag | 1:1541f49d8680 | 384 | // } |
MarijkeZondag | 1:1541f49d8680 | 385 | //else |
MarijkeZondag | 1:1541f49d8680 | 386 | // { |
MarijkeZondag | 1:1541f49d8680 | 387 | // pwmpin2 = 0; |
MarijkeZondag | 1:1541f49d8680 | 388 | // } |
MarijkeZondag | 1:1541f49d8680 | 389 | } |
MarijkeZondag | 1:1541f49d8680 | 390 | |
MarijkeZondag | 1:1541f49d8680 | 391 | //------------------ Inversed Kinematics --------------------------------// |
MarijkeZondag | 1:1541f49d8680 | 392 | |
MarijkeZondag | 1:1541f49d8680 | 393 | void inverse_kinematics() |
MarijkeZondag | 1:1541f49d8680 | 394 | { |
MarijkeZondag | 1:1541f49d8680 | 395 | //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y); |
MarijkeZondag | 1:1541f49d8680 | 396 | |
MarijkeZondag | 1:1541f49d8680 | 397 | //Lq1 = q1ref*r_trans; |
MarijkeZondag | 1:1541f49d8680 | 398 | //Cq2 = q2ref/5.0; |
MarijkeZondag | 1:1541f49d8680 | 399 | |
MarijkeZondag | 1:1541f49d8680 | 400 | q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //RKI systeem |
MarijkeZondag | 1:1541f49d8680 | 401 | 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 |
MarijkeZondag | 1:1541f49d8680 | 402 | |
MarijkeZondag | 1:1541f49d8680 | 403 | q1_ii = q1ref + q1_dot*T; //Omgezet naar motorhoeken |
MarijkeZondag | 1:1541f49d8680 | 404 | q2_ii = q2ref + q2_dot*T; |
MarijkeZondag | 1:1541f49d8680 | 405 | |
MarijkeZondag | 1:1541f49d8680 | 406 | q1ref = q1_ii; |
MarijkeZondag | 1:1541f49d8680 | 407 | q2ref = q2_ii; |
MarijkeZondag | 1:1541f49d8680 | 408 | |
MarijkeZondag | 1:1541f49d8680 | 409 | q1_motor = -q1ref/r_trans; //omgezet q1 naar -q1, zit waarschijnlijk een foutje in ergens bovenin. |
MarijkeZondag | 1:1541f49d8680 | 410 | q2_motor = q2ref*5.0; |
MarijkeZondag | 1:1541f49d8680 | 411 | |
MarijkeZondag | 1:1541f49d8680 | 412 | |
MarijkeZondag | 1:1541f49d8680 | 413 | //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref); |
MarijkeZondag | 1:1541f49d8680 | 414 | |
MarijkeZondag | 1:1541f49d8680 | 415 | |
MarijkeZondag | 1:1541f49d8680 | 416 | //start_control = 1; |
MarijkeZondag | 1:1541f49d8680 | 417 | engine_control1(); |
MarijkeZondag | 1:1541f49d8680 | 418 | engine_control2(); |
MarijkeZondag | 1:1541f49d8680 | 419 | } |
MarijkeZondag | 1:1541f49d8680 | 420 | |
MarijkeZondag | 1:1541f49d8680 | 421 | void v_des_calculate_qref() |
MarijkeZondag | 1:1541f49d8680 | 422 | { |
MarijkeZondag | 1:1541f49d8680 | 423 | while(start_v_des_calculate_qref == 1) |
MarijkeZondag | 1:1541f49d8680 | 424 | { |
MarijkeZondag | 1:1541f49d8680 | 425 | while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 1:1541f49d8680 | 426 | { |
MarijkeZondag | 1:1541f49d8680 | 427 | if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) |
MarijkeZondag | 1:1541f49d8680 | 428 | { |
MarijkeZondag | 1:1541f49d8680 | 429 | v_x = 0.5; //movement in +x direction |
MarijkeZondag | 1:1541f49d8680 | 430 | v_y = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 431 | |
MarijkeZondag | 1:1541f49d8680 | 432 | ledr = 0; //red |
MarijkeZondag | 1:1541f49d8680 | 433 | ledb = 1; |
MarijkeZondag | 1:1541f49d8680 | 434 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 435 | } |
MarijkeZondag | 1:1541f49d8680 | 436 | 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) |
MarijkeZondag | 1:1541f49d8680 | 437 | { |
MarijkeZondag | 1:1541f49d8680 | 438 | v_y = 0.5; //Movement in +y direction |
MarijkeZondag | 1:1541f49d8680 | 439 | v_x = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 440 | |
MarijkeZondag | 1:1541f49d8680 | 441 | ledr = 1; //Green |
MarijkeZondag | 1:1541f49d8680 | 442 | ledb = 1; |
MarijkeZondag | 1:1541f49d8680 | 443 | ledg = 0; |
MarijkeZondag | 1:1541f49d8680 | 444 | } |
MarijkeZondag | 1:1541f49d8680 | 445 | |
MarijkeZondag | 1:1541f49d8680 | 446 | 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) |
MarijkeZondag | 1:1541f49d8680 | 447 | { |
MarijkeZondag | 1:1541f49d8680 | 448 | v_y = 0.0; //Movement in -x direction |
MarijkeZondag | 1:1541f49d8680 | 449 | v_x = -0.5; |
MarijkeZondag | 1:1541f49d8680 | 450 | |
MarijkeZondag | 1:1541f49d8680 | 451 | ledr = 0; //Purple |
MarijkeZondag | 1:1541f49d8680 | 452 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 453 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 454 | } |
MarijkeZondag | 1:1541f49d8680 | 455 | |
MarijkeZondag | 1:1541f49d8680 | 456 | 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) |
MarijkeZondag | 1:1541f49d8680 | 457 | { |
MarijkeZondag | 1:1541f49d8680 | 458 | v_y = -0.5; //Movement in -y direction |
MarijkeZondag | 1:1541f49d8680 | 459 | v_x = 0.0; |
MarijkeZondag | 1:1541f49d8680 | 460 | |
MarijkeZondag | 1:1541f49d8680 | 461 | ledr = 1; //Blue |
MarijkeZondag | 1:1541f49d8680 | 462 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 463 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 464 | } |
MarijkeZondag | 1:1541f49d8680 | 465 | else //If not higher than any threshold, motors will not turn at all |
MarijkeZondag | 1:1541f49d8680 | 466 | { |
MarijkeZondag | 1:1541f49d8680 | 467 | v_x = 0; |
MarijkeZondag | 1:1541f49d8680 | 468 | v_y = 0; |
MarijkeZondag | 1:1541f49d8680 | 469 | |
MarijkeZondag | 1:1541f49d8680 | 470 | ledr = 0; //White |
MarijkeZondag | 1:1541f49d8680 | 471 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 472 | ledg = 0; |
MarijkeZondag | 1:1541f49d8680 | 473 | } |
MarijkeZondag | 1:1541f49d8680 | 474 | |
MarijkeZondag | 1:1541f49d8680 | 475 | inverse_kinematics(); //Call inverse kinematics function |
MarijkeZondag | 1:1541f49d8680 | 476 | |
MarijkeZondag | 1:1541f49d8680 | 477 | break; |
MarijkeZondag | 1:1541f49d8680 | 478 | } |
MarijkeZondag | 1:1541f49d8680 | 479 | break; |
MarijkeZondag | 1:1541f49d8680 | 480 | } |
MarijkeZondag | 1:1541f49d8680 | 481 | } |
MarijkeZondag | 1:1541f49d8680 | 482 | |
MarijkeZondag | 1:1541f49d8680 | 483 | void direct_control() |
MarijkeZondag | 1:1541f49d8680 | 484 | { |
MarijkeZondag | 1:1541f49d8680 | 485 | while(start_direct_control == 1) |
MarijkeZondag | 1:1541f49d8680 | 486 | { |
MarijkeZondag | 1:1541f49d8680 | 487 | while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 1:1541f49d8680 | 488 | { |
MarijkeZondag | 1:1541f49d8680 | 489 | if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) |
MarijkeZondag | 1:1541f49d8680 | 490 | { |
MarijkeZondag | 1:1541f49d8680 | 491 | pwmpin1 = 1; |
MarijkeZondag | 1:1541f49d8680 | 492 | directionpin1.write(0); //Translation up |
MarijkeZondag | 1:1541f49d8680 | 493 | } |
MarijkeZondag | 1:1541f49d8680 | 494 | 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) |
MarijkeZondag | 1:1541f49d8680 | 495 | { |
MarijkeZondag | 1:1541f49d8680 | 496 | pwmpin2 = 1; |
MarijkeZondag | 1:1541f49d8680 | 497 | directionpin2.write(0); //Rotation up |
MarijkeZondag | 1:1541f49d8680 | 498 | } |
MarijkeZondag | 1:1541f49d8680 | 499 | |
MarijkeZondag | 1:1541f49d8680 | 500 | 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) |
MarijkeZondag | 1:1541f49d8680 | 501 | { |
MarijkeZondag | 1:1541f49d8680 | 502 | pwmpin1 = 1; |
MarijkeZondag | 1:1541f49d8680 | 503 | directionpin1.write(1); //Translation down |
MarijkeZondag | 1:1541f49d8680 | 504 | } |
MarijkeZondag | 1:1541f49d8680 | 505 | |
MarijkeZondag | 1:1541f49d8680 | 506 | 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) |
MarijkeZondag | 1:1541f49d8680 | 507 | { |
MarijkeZondag | 1:1541f49d8680 | 508 | pwmpin2 = 1; |
MarijkeZondag | 1:1541f49d8680 | 509 | directionpin2.write(1); //Rotation up |
MarijkeZondag | 1:1541f49d8680 | 510 | } |
MarijkeZondag | 1:1541f49d8680 | 511 | else //If not higher than any threshold, motors will not turn at all |
MarijkeZondag | 1:1541f49d8680 | 512 | { |
MarijkeZondag | 1:1541f49d8680 | 513 | pwmpin1 = 0; |
MarijkeZondag | 1:1541f49d8680 | 514 | pwmpin2 = 0; |
MarijkeZondag | 1:1541f49d8680 | 515 | } |
MarijkeZondag | 1:1541f49d8680 | 516 | |
MarijkeZondag | 1:1541f49d8680 | 517 | break; |
MarijkeZondag | 1:1541f49d8680 | 518 | } |
MarijkeZondag | 1:1541f49d8680 | 519 | break; |
MarijkeZondag | 1:1541f49d8680 | 520 | } |
MarijkeZondag | 1:1541f49d8680 | 521 | } |
MarijkeZondag | 1:1541f49d8680 | 522 | |
MarijkeZondag | 1:1541f49d8680 | 523 | void printFunction() |
MarijkeZondag | 1:1541f49d8680 | 524 | { |
MarijkeZondag | 1:1541f49d8680 | 525 | pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); |
MarijkeZondag | 1:1541f49d8680 | 526 | pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); |
MarijkeZondag | 1:1541f49d8680 | 527 | } |
MarijkeZondag | 1:1541f49d8680 | 528 | |
MarijkeZondag | 1:1541f49d8680 | 529 | //---------------------------State machine------------------------------------------------------------------- |
onnoderkman | 0:64c100cd152a | 530 | void ProcessStateMachine(void) |
onnoderkman | 0:64c100cd152a | 531 | { |
onnoderkman | 0:64c100cd152a | 532 | switch(CurrentState) |
onnoderkman | 0:64c100cd152a | 533 | { |
MarijkeZondag | 1:1541f49d8680 | 534 | case WAITING: //This state is used to only start calibrating upon user input |
onnoderkman | 0:64c100cd152a | 535 | //State initialization |
onnoderkman | 0:64c100cd152a | 536 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 537 | { |
MarijkeZondag | 1:1541f49d8680 | 538 | //pc.printf("Entering WAITING\r\n"); |
onnoderkman | 0:64c100cd152a | 539 | ledr = 0; |
onnoderkman | 0:64c100cd152a | 540 | ledg = 1; |
onnoderkman | 0:64c100cd152a | 541 | ledb = 1; |
onnoderkman | 0:64c100cd152a | 542 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 543 | } |
onnoderkman | 0:64c100cd152a | 544 | //State actions |
MarijkeZondag | 1:1541f49d8680 | 545 | //pc.printf("Still WAITING\r\n"); |
onnoderkman | 0:64c100cd152a | 546 | while(button1) |
MarijkeZondag | 1:1541f49d8680 | 547 | {;} |
onnoderkman | 0:64c100cd152a | 548 | //State transition |
onnoderkman | 0:64c100cd152a | 549 | CurrentState = EMG_CAL; |
onnoderkman | 0:64c100cd152a | 550 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 551 | break; // End of WAITING |
MarijkeZondag | 1:1541f49d8680 | 552 | |
MarijkeZondag | 1:1541f49d8680 | 553 | case EMG_CAL: //State for calibrating EMG channels |
onnoderkman | 0:64c100cd152a | 554 | //State initialization |
onnoderkman | 0:64c100cd152a | 555 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 556 | { |
MarijkeZondag | 1:1541f49d8680 | 557 | //pc.printf("Entering EMG calibration\r\n"); |
MarijkeZondag | 1:1541f49d8680 | 558 | x = -1; |
onnoderkman | 0:64c100cd152a | 559 | ledr = 1; |
onnoderkman | 0:64c100cd152a | 560 | ledg = 0; |
onnoderkman | 0:64c100cd152a | 561 | ledb = 1; |
onnoderkman | 0:64c100cd152a | 562 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 563 | } |
onnoderkman | 0:64c100cd152a | 564 | //State actions |
MarijkeZondag | 1:1541f49d8680 | 565 | //pc.printf("EMG calibration starting\r\n"); |
MarijkeZondag | 1:1541f49d8680 | 566 | //wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit. |
MarijkeZondag | 1:1541f49d8680 | 567 | while(x < 4 && !failure) //x<4?? |
onnoderkman | 0:64c100cd152a | 568 | { |
MarijkeZondag | 1:1541f49d8680 | 569 | button3.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 1:1541f49d8680 | 570 | //wait(0.2f); //Wait to avoid bouncing of button |
MarijkeZondag | 1:1541f49d8680 | 571 | button4.rise(calibrate); //Calibrate threshold for 3 muscles |
MarijkeZondag | 1:1541f49d8680 | 572 | //wait(0.2f); //Wait to avoid bouncing of button |
MarijkeZondag | 1:1541f49d8680 | 573 | |
onnoderkman | 0:64c100cd152a | 574 | if(!button2) |
onnoderkman | 0:64c100cd152a | 575 | { |
onnoderkman | 0:64c100cd152a | 576 | failure = true; |
onnoderkman | 0:64c100cd152a | 577 | } |
MarijkeZondag | 1:1541f49d8680 | 578 | |
onnoderkman | 0:64c100cd152a | 579 | } |
onnoderkman | 0:64c100cd152a | 580 | //State transition |
onnoderkman | 0:64c100cd152a | 581 | if(failure) |
onnoderkman | 0:64c100cd152a | 582 | { |
onnoderkman | 0:64c100cd152a | 583 | CurrentState = FAIL; |
onnoderkman | 0:64c100cd152a | 584 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 585 | } |
onnoderkman | 0:64c100cd152a | 586 | else |
onnoderkman | 0:64c100cd152a | 587 | { |
MarijkeZondag | 1:1541f49d8680 | 588 | CurrentState = CHOOSE; |
onnoderkman | 0:64c100cd152a | 589 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 590 | } |
onnoderkman | 0:64c100cd152a | 591 | break; //End of EMG_CAL state |
MarijkeZondag | 1:1541f49d8680 | 592 | |
MarijkeZondag | 1:1541f49d8680 | 593 | case CHOOSE: //State to choose between RKI and DC |
MarijkeZondag | 1:1541f49d8680 | 594 | if(StateChanged) |
MarijkeZondag | 1:1541f49d8680 | 595 | { |
MarijkeZondag | 1:1541f49d8680 | 596 | ledr = 1; |
MarijkeZondag | 1:1541f49d8680 | 597 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 598 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 599 | StateChanged = false; |
MarijkeZondag | 1:1541f49d8680 | 600 | } |
MarijkeZondag | 2:afa78538ef33 | 601 | |
MarijkeZondag | 2:afa78538ef33 | 602 | while(button1 && button3 && button4) |
MarijkeZondag | 2:afa78538ef33 | 603 | {;} |
MarijkeZondag | 2:afa78538ef33 | 604 | |
MarijkeZondag | 1:1541f49d8680 | 605 | if(!button3) |
MarijkeZondag | 2:afa78538ef33 | 606 | { |
MarijkeZondag | 2:afa78538ef33 | 607 | CurrentState = RKI; |
MarijkeZondag | 2:afa78538ef33 | 608 | StateChanged = true; |
MarijkeZondag | 2:afa78538ef33 | 609 | } |
MarijkeZondag | 1:1541f49d8680 | 610 | else if(!button4) |
MarijkeZondag | 2:afa78538ef33 | 611 | { |
MarijkeZondag | 2:afa78538ef33 | 612 | CurrentState = DC; |
MarijkeZondag | 2:afa78538ef33 | 613 | StateChanged = true; |
MarijkeZondag | 2:afa78538ef33 | 614 | } |
MarijkeZondag | 2:afa78538ef33 | 615 | else |
MarijkeZondag | 2:afa78538ef33 | 616 | { |
MarijkeZondag | 2:afa78538ef33 | 617 | CurrentState = WAITING; |
MarijkeZondag | 2:afa78538ef33 | 618 | StateChanged = true; |
MarijkeZondag | 2:afa78538ef33 | 619 | } |
MarijkeZondag | 2:afa78538ef33 | 620 | |
MarijkeZondag | 1:1541f49d8680 | 621 | break; //End of "choose" state, move on to RKI or DC (straight lines or direct control) |
MarijkeZondag | 1:1541f49d8680 | 622 | |
MarijkeZondag | 1:1541f49d8680 | 623 | case RKI: //State for moving the robot in straight lines in cartesian space |
onnoderkman | 0:64c100cd152a | 624 | //State initialization |
onnoderkman | 0:64c100cd152a | 625 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 626 | { |
MarijkeZondag | 1:1541f49d8680 | 627 | //pc.printf("Entering Basic Control Mode\r\n"); |
onnoderkman | 0:64c100cd152a | 628 | ledr = 1; |
MarijkeZondag | 2:afa78538ef33 | 629 | ledg = 0; |
onnoderkman | 0:64c100cd152a | 630 | ledb = 0; |
onnoderkman | 0:64c100cd152a | 631 | StateChanged = false; |
onnoderkman | 0:64c100cd152a | 632 | } |
onnoderkman | 0:64c100cd152a | 633 | //State actions |
MarijkeZondag | 1:1541f49d8680 | 634 | //pc.printf("Basic Control Mode is active\r\n"); |
MarijkeZondag | 1:1541f49d8680 | 635 | while(button1 && !failure &&!homing) |
MarijkeZondag | 1:1541f49d8680 | 636 | { |
MarijkeZondag | 1:1541f49d8680 | 637 | start_v_des_calculate_qref = 1; |
MarijkeZondag | 1:1541f49d8680 | 638 | |
MarijkeZondag | 1:1541f49d8680 | 639 | |
MarijkeZondag | 1:1541f49d8680 | 640 | if(!button2) |
MarijkeZondag | 1:1541f49d8680 | 641 | { |
MarijkeZondag | 1:1541f49d8680 | 642 | failure = true; |
MarijkeZondag | 1:1541f49d8680 | 643 | } |
MarijkeZondag | 1:1541f49d8680 | 644 | if(!button3) |
MarijkeZondag | 1:1541f49d8680 | 645 | { |
MarijkeZondag | 1:1541f49d8680 | 646 | homing = true; |
MarijkeZondag | 1:1541f49d8680 | 647 | } |
MarijkeZondag | 1:1541f49d8680 | 648 | |
MarijkeZondag | 1:1541f49d8680 | 649 | } |
MarijkeZondag | 1:1541f49d8680 | 650 | //State transition |
MarijkeZondag | 1:1541f49d8680 | 651 | if(failure) |
MarijkeZondag | 1:1541f49d8680 | 652 | { |
MarijkeZondag | 1:1541f49d8680 | 653 | pc.printf("Go to fail mode\r\n"); |
MarijkeZondag | 1:1541f49d8680 | 654 | start_v_des_calculate_qref = 0; |
MarijkeZondag | 1:1541f49d8680 | 655 | CurrentState = FAIL; |
MarijkeZondag | 1:1541f49d8680 | 656 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 657 | } |
MarijkeZondag | 1:1541f49d8680 | 658 | else if(homing) |
MarijkeZondag | 1:1541f49d8680 | 659 | { |
MarijkeZondag | 1:1541f49d8680 | 660 | CurrentState = HOMING; |
MarijkeZondag | 1:1541f49d8680 | 661 | start_v_des_calculate_qref = 0; CurrentState = HOMING; |
MarijkeZondag | 1:1541f49d8680 | 662 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 663 | } |
MarijkeZondag | 1:1541f49d8680 | 664 | else |
MarijkeZondag | 1:1541f49d8680 | 665 | { |
MarijkeZondag | 1:1541f49d8680 | 666 | CurrentState = WAITING; |
MarijkeZondag | 1:1541f49d8680 | 667 | start_v_des_calculate_qref = 0; |
MarijkeZondag | 1:1541f49d8680 | 668 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 669 | } |
MarijkeZondag | 2:afa78538ef33 | 670 | break; //End of RKI state |
MarijkeZondag | 1:1541f49d8680 | 671 | |
MarijkeZondag | 1:1541f49d8680 | 672 | case DC: |
MarijkeZondag | 1:1541f49d8680 | 673 | if(StateChanged) |
MarijkeZondag | 1:1541f49d8680 | 674 | { |
MarijkeZondag | 2:afa78538ef33 | 675 | ledr = 0; |
MarijkeZondag | 1:1541f49d8680 | 676 | ledg = 1; |
MarijkeZondag | 1:1541f49d8680 | 677 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 678 | StateChanged = false; |
MarijkeZondag | 1:1541f49d8680 | 679 | } |
MarijkeZondag | 1:1541f49d8680 | 680 | |
MarijkeZondag | 1:1541f49d8680 | 681 | while(button1 && !failure &&!homing) |
onnoderkman | 0:64c100cd152a | 682 | { |
MarijkeZondag | 1:1541f49d8680 | 683 | start_direct_control = 1; |
MarijkeZondag | 1:1541f49d8680 | 684 | |
MarijkeZondag | 1:1541f49d8680 | 685 | if(!button2) |
MarijkeZondag | 1:1541f49d8680 | 686 | { |
MarijkeZondag | 1:1541f49d8680 | 687 | failure = true; |
MarijkeZondag | 1:1541f49d8680 | 688 | } |
MarijkeZondag | 1:1541f49d8680 | 689 | |
MarijkeZondag | 1:1541f49d8680 | 690 | if(!button3) |
MarijkeZondag | 1:1541f49d8680 | 691 | { |
MarijkeZondag | 1:1541f49d8680 | 692 | homing = true; |
MarijkeZondag | 1:1541f49d8680 | 693 | } |
MarijkeZondag | 1:1541f49d8680 | 694 | } |
MarijkeZondag | 1:1541f49d8680 | 695 | //State transition |
MarijkeZondag | 1:1541f49d8680 | 696 | if(failure) |
MarijkeZondag | 1:1541f49d8680 | 697 | { |
MarijkeZondag | 1:1541f49d8680 | 698 | start_direct_control = 0; |
MarijkeZondag | 1:1541f49d8680 | 699 | CurrentState = FAIL; |
MarijkeZondag | 1:1541f49d8680 | 700 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 701 | } |
MarijkeZondag | 1:1541f49d8680 | 702 | else if(homing) |
MarijkeZondag | 1:1541f49d8680 | 703 | { |
MarijkeZondag | 1:1541f49d8680 | 704 | start_direct_control = 0; |
MarijkeZondag | 1:1541f49d8680 | 705 | CurrentState = HOMING; |
MarijkeZondag | 1:1541f49d8680 | 706 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 707 | } |
MarijkeZondag | 1:1541f49d8680 | 708 | else |
MarijkeZondag | 1:1541f49d8680 | 709 | { |
MarijkeZondag | 1:1541f49d8680 | 710 | CurrentState = WAITING; |
MarijkeZondag | 1:1541f49d8680 | 711 | start_direct_control = 0; |
MarijkeZondag | 1:1541f49d8680 | 712 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 713 | } |
MarijkeZondag | 1:1541f49d8680 | 714 | |
MarijkeZondag | 1:1541f49d8680 | 715 | case HOMING: //Back to encoder position zero |
MarijkeZondag | 1:1541f49d8680 | 716 | if(StateChanged) |
MarijkeZondag | 1:1541f49d8680 | 717 | { |
MarijkeZondag | 2:afa78538ef33 | 718 | ledr = 0; |
MarijkeZondag | 2:afa78538ef33 | 719 | ledg = 0; |
MarijkeZondag | 1:1541f49d8680 | 720 | ledb = 0; |
MarijkeZondag | 1:1541f49d8680 | 721 | StateChanged = false; |
MarijkeZondag | 1:1541f49d8680 | 722 | } |
MarijkeZondag | 1:1541f49d8680 | 723 | |
MarijkeZondag | 1:1541f49d8680 | 724 | while(button1 && !failure &&!backRKI &&!backDC) |
MarijkeZondag | 1:1541f49d8680 | 725 | { |
MarijkeZondag | 1:1541f49d8680 | 726 | //Back to encoder position zero function |
MarijkeZondag | 2:afa78538ef33 | 727 | |
MarijkeZondag | 2:afa78538ef33 | 728 | //translation back |
MarijkeZondag | 2:afa78538ef33 | 729 | while(encoder1.getPulses()>10.0 && encoder1.getPulses()<-10.0) |
MarijkeZondag | 2:afa78538ef33 | 730 | { |
MarijkeZondag | 2:afa78538ef33 | 731 | |
MarijkeZondag | 2:afa78538ef33 | 732 | if(encoder1.getPulses()>10.0) |
MarijkeZondag | 2:afa78538ef33 | 733 | { |
MarijkeZondag | 2:afa78538ef33 | 734 | pwmpin1 = 0.3f; |
MarijkeZondag | 2:afa78538ef33 | 735 | directionpin1.write(0); |
MarijkeZondag | 2:afa78538ef33 | 736 | } |
MarijkeZondag | 2:afa78538ef33 | 737 | else if (encoder1.getPulses()<-10.0) |
MarijkeZondag | 2:afa78538ef33 | 738 | { |
MarijkeZondag | 2:afa78538ef33 | 739 | pwmpin1 = 0.3f; |
MarijkeZondag | 2:afa78538ef33 | 740 | directionpin1.write(1); |
MarijkeZondag | 2:afa78538ef33 | 741 | } |
MarijkeZondag | 2:afa78538ef33 | 742 | } |
MarijkeZondag | 1:1541f49d8680 | 743 | |
MarijkeZondag | 2:afa78538ef33 | 744 | //Rotation back |
MarijkeZondag | 2:afa78538ef33 | 745 | while(encoder2.getPulses()>10.0 && encoder2.getPulses()<-10.0) |
MarijkeZondag | 2:afa78538ef33 | 746 | { |
MarijkeZondag | 2:afa78538ef33 | 747 | |
MarijkeZondag | 2:afa78538ef33 | 748 | if(encoder2.getPulses()>10.0) |
MarijkeZondag | 2:afa78538ef33 | 749 | { |
MarijkeZondag | 2:afa78538ef33 | 750 | pmwpin2 = 0.3f; |
MarijkeZondag | 2:afa78538ef33 | 751 | directionpin2.write(1); |
MarijkeZondag | 2:afa78538ef33 | 752 | } |
MarijkeZondag | 2:afa78538ef33 | 753 | else if (encoder2.getPulses()<-10.0) |
MarijkeZondag | 2:afa78538ef33 | 754 | { |
MarijkeZondag | 2:afa78538ef33 | 755 | pwmpin2 = 0.3f; |
MarijkeZondag | 2:afa78538ef33 | 756 | directionpin2.write(0); |
MarijkeZondag | 2:afa78538ef33 | 757 | } |
MarijkeZondag | 2:afa78538ef33 | 758 | } |
MarijkeZondag | 1:1541f49d8680 | 759 | |
MarijkeZondag | 1:1541f49d8680 | 760 | |
MarijkeZondag | 1:1541f49d8680 | 761 | if(!button3) |
MarijkeZondag | 1:1541f49d8680 | 762 | { |
MarijkeZondag | 1:1541f49d8680 | 763 | backRKI = true; |
MarijkeZondag | 1:1541f49d8680 | 764 | } |
MarijkeZondag | 1:1541f49d8680 | 765 | |
MarijkeZondag | 1:1541f49d8680 | 766 | if(!button4) |
MarijkeZondag | 1:1541f49d8680 | 767 | { |
MarijkeZondag | 1:1541f49d8680 | 768 | backDC = true; |
MarijkeZondag | 1:1541f49d8680 | 769 | } |
MarijkeZondag | 1:1541f49d8680 | 770 | |
onnoderkman | 0:64c100cd152a | 771 | if(!button2) |
onnoderkman | 0:64c100cd152a | 772 | { |
onnoderkman | 0:64c100cd152a | 773 | failure = true; |
onnoderkman | 0:64c100cd152a | 774 | } |
onnoderkman | 0:64c100cd152a | 775 | } |
onnoderkman | 0:64c100cd152a | 776 | //State transition |
onnoderkman | 0:64c100cd152a | 777 | if(failure) |
onnoderkman | 0:64c100cd152a | 778 | { |
onnoderkman | 0:64c100cd152a | 779 | CurrentState = FAIL; |
onnoderkman | 0:64c100cd152a | 780 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 781 | } |
MarijkeZondag | 1:1541f49d8680 | 782 | else if(backRKI) |
MarijkeZondag | 1:1541f49d8680 | 783 | { |
MarijkeZondag | 1:1541f49d8680 | 784 | CurrentState = RKI; |
MarijkeZondag | 1:1541f49d8680 | 785 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 786 | } |
MarijkeZondag | 1:1541f49d8680 | 787 | else if(backDC) |
MarijkeZondag | 1:1541f49d8680 | 788 | { |
MarijkeZondag | 1:1541f49d8680 | 789 | CurrentState = DC; |
MarijkeZondag | 1:1541f49d8680 | 790 | StateChanged = true; |
MarijkeZondag | 1:1541f49d8680 | 791 | } |
onnoderkman | 0:64c100cd152a | 792 | else |
onnoderkman | 0:64c100cd152a | 793 | { |
onnoderkman | 0:64c100cd152a | 794 | CurrentState = WAITING; |
MarijkeZondag | 1:1541f49d8680 | 795 | start_direct_control = 0; |
onnoderkman | 0:64c100cd152a | 796 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 797 | } |
MarijkeZondag | 1:1541f49d8680 | 798 | |
onnoderkman | 0:64c100cd152a | 799 | case FAIL: //Fail state |
onnoderkman | 0:64c100cd152a | 800 | //State initialization |
onnoderkman | 0:64c100cd152a | 801 | if(StateChanged) |
onnoderkman | 0:64c100cd152a | 802 | { |
onnoderkman | 0:64c100cd152a | 803 | failure = false; |
onnoderkman | 0:64c100cd152a | 804 | StateChanged = false; |
MarijkeZondag | 2:afa78538ef33 | 805 | |
MarijkeZondag | 2:afa78538ef33 | 806 | ledr=0; |
MarijkeZondag | 2:afa78538ef33 | 807 | ledg=0; |
MarijkeZondag | 2:afa78538ef33 | 808 | ledb=0; |
onnoderkman | 0:64c100cd152a | 809 | } |
onnoderkman | 0:64c100cd152a | 810 | //State actions |
MarijkeZondag | 1:1541f49d8680 | 811 | //"Fail mode is active. Press button1 to enter WAITING |
MarijkeZondag | 2:afa78538ef33 | 812 | while(button1) |
MarijkeZondag | 2:afa78538ef33 | 813 | { |
MarijkeZondag | 2:afa78538ef33 | 814 | ledr = !ledr; |
MarijkeZondag | 2:afa78538ef33 | 815 | wait(0.3f); |
MarijkeZondag | 2:afa78538ef33 | 816 | } |
onnoderkman | 0:64c100cd152a | 817 | //State transition |
onnoderkman | 0:64c100cd152a | 818 | CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 819 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 820 | break; //End of FAIL state |
MarijkeZondag | 1:1541f49d8680 | 821 | |
onnoderkman | 0:64c100cd152a | 822 | default: //Emergency state for when something unexpected happens |
onnoderkman | 0:64c100cd152a | 823 | //motoren uit enzo |
onnoderkman | 0:64c100cd152a | 824 | ledr = 0; |
onnoderkman | 0:64c100cd152a | 825 | ledg = 0; |
onnoderkman | 0:64c100cd152a | 826 | ledb = 0; |
onnoderkman | 0:64c100cd152a | 827 | while(button1) |
onnoderkman | 0:64c100cd152a | 828 | { |
MarijkeZondag | 2:afa78538ef33 | 829 | ledb = !ledb; |
onnoderkman | 0:64c100cd152a | 830 | wait(0.3f); |
onnoderkman | 0:64c100cd152a | 831 | } |
onnoderkman | 0:64c100cd152a | 832 | CurrentState = WAITING; |
onnoderkman | 0:64c100cd152a | 833 | StateChanged = true; |
onnoderkman | 0:64c100cd152a | 834 | } |
onnoderkman | 0:64c100cd152a | 835 | } |
onnoderkman | 0:64c100cd152a | 836 | |
MarijkeZondag | 1:1541f49d8680 | 837 | |
MarijkeZondag | 1:1541f49d8680 | 838 | //------------------ Start main function --------------------------// |
MarijkeZondag | 1:1541f49d8680 | 839 | |
MarijkeZondag | 1:1541f49d8680 | 840 | |
onnoderkman | 0:64c100cd152a | 841 | int main() |
onnoderkman | 0:64c100cd152a | 842 | { |
MarijkeZondag | 1:1541f49d8680 | 843 | pc.baud(115200); |
MarijkeZondag | 1:1541f49d8680 | 844 | pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. |
MarijkeZondag | 1:1541f49d8680 | 845 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 1:1541f49d8680 | 846 | |
MarijkeZondag | 1:1541f49d8680 | 847 | emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain |
MarijkeZondag | 1:1541f49d8680 | 848 | emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); |
MarijkeZondag | 1:1541f49d8680 | 849 | emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); |
MarijkeZondag | 1:1541f49d8680 | 850 | |
MarijkeZondag | 1:1541f49d8680 | 851 | emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. |
MarijkeZondag | 1:1541f49d8680 | 852 | movag_tick.attach(&MovAg,T); |
MarijkeZondag | 1:1541f49d8680 | 853 | |
MarijkeZondag | 1:1541f49d8680 | 854 | RKI_tick.attach(&v_des_calculate_qref,T); //Ticker RKI |
MarijkeZondag | 1:1541f49d8680 | 855 | DC_tick.attach(&direct_control,T); //Ticker DC |
MarijkeZondag | 1:1541f49d8680 | 856 | |
MarijkeZondag | 1:1541f49d8680 | 857 | print_tick.attach(&printFunction,T2); |
MarijkeZondag | 1:1541f49d8680 | 858 | // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. |
MarijkeZondag | 1:1541f49d8680 | 859 | |
onnoderkman | 0:64c100cd152a | 860 | while (true) |
onnoderkman | 0:64c100cd152a | 861 | { |
onnoderkman | 0:64c100cd152a | 862 | ProcessStateMachine(); //Activates state machine |
onnoderkman | 0:64c100cd152a | 863 | } |
onnoderkman | 0:64c100cd152a | 864 | } |