Marijke Zondag / Mbed OS statemachineopzet

..

Dependencies:   MODSERIAL

Fork of statemachineopzet by Onno Derkman

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?

UserRevisionLine numberNew 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( &notch1 ); //attach biquad elements to chain
MarijkeZondag 1:1541f49d8680 848 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 1:1541f49d8680 849 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
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 }