![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mbed bordje 1 -af
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by
main.cpp@5:3562c205d001, 2017-11-03 (annotated)
- Committer:
- RoyvZ
- Date:
- Fri Nov 03 06:40:08 2017 +0000
- Revision:
- 5:3562c205d001
- Parent:
- 4:afa85283eb18
- Child:
- 6:643c44fb044a
MBed bordje 1 -> af;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 5:3562c205d001 | 1 | //initialisation |
RoyvZ | 0:b6c8d56842ce | 2 | #include "mbed.h" |
RoyvZ | 5:3562c205d001 | 3 | #include "encoder.h" |
RoyvZ | 0:b6c8d56842ce | 4 | #include "HIDScope.h" |
RoyvZ | 5:3562c205d001 | 5 | #include "MODSERIAL.h" |
RoyvZ | 5:3562c205d001 | 6 | #include "math.h" |
RoyvZ | 5:3562c205d001 | 7 | #include "BiQuad.h" |
RoyvZ | 5:3562c205d001 | 8 | #include "Matrix.h" |
RoyvZ | 5:3562c205d001 | 9 | #include "MatrixMath.h" |
RoyvZ | 0:b6c8d56842ce | 10 | #include <stdlib.h> |
RoyvZ | 0:b6c8d56842ce | 11 | #include <iostream> |
RoyvZ | 0:b6c8d56842ce | 12 | #include <iomanip> |
RoyvZ | 0:b6c8d56842ce | 13 | #include <complex> |
RoyvZ | 3:2ffbee47fb38 | 14 | #include <vector> |
RoyvZ | 5:3562c205d001 | 15 | |
RoyvZ | 5:3562c205d001 | 16 | Ticker MainTicker; |
RoyvZ | 2:293665548183 | 17 | |
RoyvZ | 5:3562c205d001 | 18 | //Define objects EMG |
RoyvZ | 5:3562c205d001 | 19 | AnalogIn emg0( A0 ); |
RoyvZ | 5:3562c205d001 | 20 | AnalogIn emg1( A1 ); |
RoyvZ | 5:3562c205d001 | 21 | DigitalIn buttonCal(D11); |
RoyvZ | 5:3562c205d001 | 22 | DigitalIn changeX(D2); |
RoyvZ | 5:3562c205d001 | 23 | DigitalIn changeY(D3); |
RoyvZ | 5:3562c205d001 | 24 | InterruptIn turning(D10); |
RoyvZ | 0:b6c8d56842ce | 25 | |
RoyvZ | 5:3562c205d001 | 26 | Ticker sample_timer; |
RoyvZ | 5:3562c205d001 | 27 | Ticker calX_timer; |
RoyvZ | 5:3562c205d001 | 28 | Ticker calY_timer; |
RoyvZ | 5:3562c205d001 | 29 | //HIDScope scope( 4 ); |
RoyvZ | 5:3562c205d001 | 30 | DigitalOut led(LED1); |
RoyvZ | 5:3562c205d001 | 31 | DigitalOut led2(LED_GREEN); |
RoyvZ | 5:3562c205d001 | 32 | |
RoyvZ | 5:3562c205d001 | 33 | //Define variables EMG |
RoyvZ | 3:2ffbee47fb38 | 34 | const int size = 100; |
RoyvZ | 5:3562c205d001 | 35 | vector<double> SX(size,0); |
RoyvZ | 5:3562c205d001 | 36 | vector<double> SY(size,0); |
RoyvZ | 3:2ffbee47fb38 | 37 | double meanSum = 0; |
RoyvZ | 3:2ffbee47fb38 | 38 | double maxsignal = 0; |
RoyvZ | 2:293665548183 | 39 | |
RoyvZ | 5:3562c205d001 | 40 | //Filters |
RoyvZ | 2:293665548183 | 41 | BiQuadChain Notch; |
RoyvZ | 3:2ffbee47fb38 | 42 | BiQuadChain Notch50; |
RoyvZ | 3:2ffbee47fb38 | 43 | BiQuadChain bqcLP; |
RoyvZ | 3:2ffbee47fb38 | 44 | BiQuadChain bqc; |
RoyvZ | 0:b6c8d56842ce | 45 | |
RoyvZ | 2:293665548183 | 46 | //Notch filter chain for 100 Hz |
RoyvZ | 2:293665548183 | 47 | BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); |
RoyvZ | 2:293665548183 | 48 | BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); |
RoyvZ | 2:293665548183 | 49 | BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); |
RoyvZ | 2:293665548183 | 50 | |
RoyvZ | 2:293665548183 | 51 | //Notch filter chain for 50 Hz |
RoyvZ | 3:2ffbee47fb38 | 52 | BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 53 | BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 54 | BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 ); |
RoyvZ | 2:293665548183 | 55 | |
RoyvZ | 5:3562c205d001 | 56 | //2 Hz High Pass filter |
RoyvZ | 3:2ffbee47fb38 | 57 | BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01); |
RoyvZ | 0:b6c8d56842ce | 58 | |
RoyvZ | 5:3562c205d001 | 59 | //80 Hz Low Pass filter |
RoyvZ | 3:2ffbee47fb38 | 60 | BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 ); |
RoyvZ | 3:2ffbee47fb38 | 61 | BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 ); |
RoyvZ | 3:2ffbee47fb38 | 62 | |
RoyvZ | 3:2ffbee47fb38 | 63 | |
RoyvZ | 5:3562c205d001 | 64 | //Define objects MotorControl |
RoyvZ | 5:3562c205d001 | 65 | DigitalOut gpo(D0); |
RoyvZ | 5:3562c205d001 | 66 | DigitalOut UpperMotorDir(D4); |
RoyvZ | 5:3562c205d001 | 67 | PwmOut UpperMotorVel(D5); |
RoyvZ | 5:3562c205d001 | 68 | DigitalOut LowerMotorDir(D6); |
RoyvZ | 5:3562c205d001 | 69 | PwmOut LowerMotorVel(D7); |
RoyvZ | 5:3562c205d001 | 70 | AnalogIn potMeterIn(A1); |
RoyvZ | 5:3562c205d001 | 71 | InterruptIn ButMotorDir(D3); |
RoyvZ | 5:3562c205d001 | 72 | Encoder encoder1(D13,D12); |
RoyvZ | 5:3562c205d001 | 73 | Encoder encoder2(D9,D8); |
RoyvZ | 5:3562c205d001 | 74 | Ticker m1_Ticker; |
RoyvZ | 5:3562c205d001 | 75 | Ticker setpoint_Ticker; |
RoyvZ | 0:b6c8d56842ce | 76 | |
RoyvZ | 5:3562c205d001 | 77 | //Define variables MotorControl |
RoyvZ | 5:3562c205d001 | 78 | float M_PI = 3.14159265359; |
RoyvZ | 5:3562c205d001 | 79 | |
RoyvZ | 5:3562c205d001 | 80 | float M1_KP = 1.0f; |
RoyvZ | 5:3562c205d001 | 81 | float M1_KI = 0.1f; |
RoyvZ | 5:3562c205d001 | 82 | const float M1_TS = 0.002; |
RoyvZ | 5:3562c205d001 | 83 | const float q_RAD_PER_PULSE_m2 = 0.36f*M_PI/1300.0f; //2*M_PI/4320 voor als het nu geen rechte lijn meer is8 |
RoyvZ | 5:3562c205d001 | 84 | const float q_RAD_PER_PULSE_m1 = 0.5f*M_PI/3300.0f; |
RoyvZ | 5:3562c205d001 | 85 | |
RoyvZ | 5:3562c205d001 | 86 | float m1_err_int = 0; |
RoyvZ | 5:3562c205d001 | 87 | float m2_err_int = 0; |
RoyvZ | 5:3562c205d001 | 88 | int motorD1 = 0; |
RoyvZ | 5:3562c205d001 | 89 | float motor1 = 0; |
RoyvZ | 5:3562c205d001 | 90 | int motorD2 = 0; |
RoyvZ | 5:3562c205d001 | 91 | float motor2 = 0; |
RoyvZ | 5:3562c205d001 | 92 | float position1 = 0; |
RoyvZ | 5:3562c205d001 | 93 | float position2 = 0; |
RoyvZ | 5:3562c205d001 | 94 | |
RoyvZ | 5:3562c205d001 | 95 | float emgX = 0; |
RoyvZ | 5:3562c205d001 | 96 | float emgY = 0; |
DiondeGreef | 4:afa85283eb18 | 97 | |
RoyvZ | 5:3562c205d001 | 98 | float L0 = 0.123; |
RoyvZ | 5:3562c205d001 | 99 | float L1 = 0.37; |
RoyvZ | 5:3562c205d001 | 100 | float L2 = 0.41; |
RoyvZ | 5:3562c205d001 | 101 | float q1 = 0.0f*M_PI; |
RoyvZ | 5:3562c205d001 | 102 | float q2 = 0.0f*M_PI; |
RoyvZ | 5:3562c205d001 | 103 | float Periode = 0.001; //1000 is het aantal Hz |
RoyvZ | 5:3562c205d001 | 104 | |
RoyvZ | 5:3562c205d001 | 105 | float ref_pos1; |
RoyvZ | 5:3562c205d001 | 106 | float ref_pos2; |
RoyvZ | 5:3562c205d001 | 107 | |
RoyvZ | 5:3562c205d001 | 108 | float Qset1 = 0; |
RoyvZ | 5:3562c205d001 | 109 | float Qset2 = 0; |
RoyvZ | 5:3562c205d001 | 110 | |
RoyvZ | 5:3562c205d001 | 111 | int UpperMotorPos; |
RoyvZ | 5:3562c205d001 | 112 | int UpperMotorPos_prev; |
RoyvZ | 5:3562c205d001 | 113 | int LowerMotorPos; |
RoyvZ | 5:3562c205d001 | 114 | int LowerMotorPos_prev; |
RoyvZ | 5:3562c205d001 | 115 | |
RoyvZ | 5:3562c205d001 | 116 | int NPC = 0; |
RoyvZ | 5:3562c205d001 | 117 | float NoTurnies = 1; |
RoyvZ | 5:3562c205d001 | 118 | |
RoyvZ | 5:3562c205d001 | 119 | //Define matrices MotorControl |
RoyvZ | 5:3562c205d001 | 120 | Matrix JAPPAPP(2,2); |
RoyvZ | 5:3562c205d001 | 121 | Matrix qdot(2,1); |
RoyvZ | 5:3562c205d001 | 122 | Matrix Vdes(2,1); |
RoyvZ | 5:3562c205d001 | 123 | |
RoyvZ | 5:3562c205d001 | 124 | //Define object Servo Motors |
RoyvZ | 5:3562c205d001 | 125 | DigitalOut Ledr(LED_RED); |
RoyvZ | 5:3562c205d001 | 126 | DigitalOut Ledg(LED_GREEN); |
RoyvZ | 5:3562c205d001 | 127 | DigitalOut Ledb(LED_BLUE); |
RoyvZ | 5:3562c205d001 | 128 | PwmOut LeftServoVel(D11); |
RoyvZ | 5:3562c205d001 | 129 | PwmOut RightServoVel(D10); |
RoyvZ | 5:3562c205d001 | 130 | DigitalIn ServoButton(D2); |
RoyvZ | 5:3562c205d001 | 131 | Ticker ServoTick; |
DiondeGreef | 4:afa85283eb18 | 132 | |
DiondeGreef | 4:afa85283eb18 | 133 | |
RoyvZ | 5:3562c205d001 | 134 | //Other variables |
RoyvZ | 5:3562c205d001 | 135 | float Cal_Strength = 0.6f; |
RoyvZ | 5:3562c205d001 | 136 | |
RoyvZ | 5:3562c205d001 | 137 | |
RoyvZ | 5:3562c205d001 | 138 | //Connection with TeraTerm |
RoyvZ | 5:3562c205d001 | 139 | MODSERIAL pc(USBTX, USBRX); |
RoyvZ | 5:3562c205d001 | 140 | |
RoyvZ | 5:3562c205d001 | 141 | //Creating of the states |
RoyvZ | 5:3562c205d001 | 142 | enum states{calib1, calib2, calibEMG, UpDown, LeftRight, Vertical, MotionEndEffector, GrabbingPlacing, Off, Idle, No_State}; |
RoyvZ | 5:3562c205d001 | 143 | |
RoyvZ | 5:3562c205d001 | 144 | //Root Mean Square value calculator for useful envelope, using vectors |
RoyvZ | 5:3562c205d001 | 145 | double findRMS(vector<double> array) { |
RoyvZ | 3:2ffbee47fb38 | 146 | int i; |
RoyvZ | 5:3562c205d001 | 147 | double sumsquared = 0.000; //define variable that resets to 0 for every call of the function |
RoyvZ | 3:2ffbee47fb38 | 148 | double RMS; |
RoyvZ | 5:3562c205d001 | 149 | |
RoyvZ | 5:3562c205d001 | 150 | for (i = 0; i < size; i++) //for loop to calculate the total sum of the squared values |
RoyvZ | 3:2ffbee47fb38 | 151 | { |
RoyvZ | 3:2ffbee47fb38 | 152 | sumsquared = sumsquared + array.at(i)*array.at(i); |
RoyvZ | 3:2ffbee47fb38 | 153 | } |
RoyvZ | 5:3562c205d001 | 154 | RMS = sqrt((double(1)/size)*(sumsquared)); //root mean square calculation |
RoyvZ | 3:2ffbee47fb38 | 155 | return RMS; |
RoyvZ | 3:2ffbee47fb38 | 156 | } |
RoyvZ | 5:3562c205d001 | 157 | |
RoyvZ | 5:3562c205d001 | 158 | |
RoyvZ | 5:3562c205d001 | 159 | //Function to show the EMG signals in HIDScope |
RoyvZ | 5:3562c205d001 | 160 | void sample() { |
RoyvZ | 0:b6c8d56842ce | 161 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
RoyvZ | 5:3562c205d001 | 162 | //Standard emg value without filters, offset removed with the - value |
RoyvZ | 5:3562c205d001 | 163 | //scope.set(0, emg0.read()-0.4542f); |
RoyvZ | 5:3562c205d001 | 164 | |
RoyvZ | 5:3562c205d001 | 165 | //Use S vector to save values coming from EMG signal |
RoyvZ | 5:3562c205d001 | 166 | SX.erase(SX.begin()); //remove first value |
RoyvZ | 5:3562c205d001 | 167 | double emgXvalue = bqc.step(emg0.read()-0.4542f); |
RoyvZ | 5:3562c205d001 | 168 | SX.push_back(emgXvalue); //add extra value to begin of vector |
RoyvZ | 5:3562c205d001 | 169 | |
RoyvZ | 5:3562c205d001 | 170 | SY.erase(SY.begin()); //remove first value |
RoyvZ | 5:3562c205d001 | 171 | double emgYvalue = bqc.step(emg0.read()-0.4542f); |
RoyvZ | 5:3562c205d001 | 172 | SY.push_back(emgYvalue); //add extra value to begin of vector |
RoyvZ | 3:2ffbee47fb38 | 173 | |
RoyvZ | 3:2ffbee47fb38 | 174 | |
RoyvZ | 5:3562c205d001 | 175 | //Calculate Root Mean Square value using findRMS function as defined above |
RoyvZ | 5:3562c205d001 | 176 | double emgX = findRMS(SX); |
RoyvZ | 5:3562c205d001 | 177 | double emgY = findRMS(SY); |
RoyvZ | 5:3562c205d001 | 178 | |
RoyvZ | 5:3562c205d001 | 179 | //Display results |
RoyvZ | 5:3562c205d001 | 180 | //scope.set(1, emgX); |
RoyvZ | 5:3562c205d001 | 181 | //scope.set(2, emgY); |
RoyvZ | 5:3562c205d001 | 182 | |
RoyvZ | 5:3562c205d001 | 183 | //Send all scopes to HIDScope |
RoyvZ | 5:3562c205d001 | 184 | //scope.send(); |
RoyvZ | 5:3562c205d001 | 185 | |
RoyvZ | 0:b6c8d56842ce | 186 | /* To indicate that the function is working, the LED is toggled */ |
RoyvZ | 0:b6c8d56842ce | 187 | led = !led; |
RoyvZ | 0:b6c8d56842ce | 188 | } |
RoyvZ | 0:b6c8d56842ce | 189 | |
RoyvZ | 5:3562c205d001 | 190 | //Calibration function for the EMG signals |
RoyvZ | 5:3562c205d001 | 191 | void calibrationX() { |
RoyvZ | 2:293665548183 | 192 | //function to determine maximal EMG input in order to allow motorcontrol to be activated |
RoyvZ | 3:2ffbee47fb38 | 193 | |
RoyvZ | 5:3562c205d001 | 194 | if (buttonCal.read() == false){ //activated when button is pressed |
RoyvZ | 3:2ffbee47fb38 | 195 | led2 = !led2; |
RoyvZ | 2:293665548183 | 196 | for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds |
RoyvZ | 5:3562c205d001 | 197 | SX.erase(SX.begin()); |
RoyvZ | 5:3562c205d001 | 198 | double emgXCalvalue = bqc.step(emg0.read()-0.4542f); |
RoyvZ | 5:3562c205d001 | 199 | SX.push_back(emgXCalvalue); |
RoyvZ | 5:3562c205d001 | 200 | double signalfinal = findRMS(SX); //calculate RMS values from EMG signals |
RoyvZ | 5:3562c205d001 | 201 | if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal |
RoyvZ | 5:3562c205d001 | 202 | maxsignal = signalfinal; //keep changing the maximal signal |
RoyvZ | 2:293665548183 | 203 | } |
RoyvZ | 2:293665548183 | 204 | } |
RoyvZ | 5:3562c205d001 | 205 | led2 = 1; //Turn LED off when calibration is finished |
RoyvZ | 5:3562c205d001 | 206 | } |
RoyvZ | 5:3562c205d001 | 207 | } |
RoyvZ | 5:3562c205d001 | 208 | |
RoyvZ | 5:3562c205d001 | 209 | void calibrationY() { |
RoyvZ | 5:3562c205d001 | 210 | //function to determine maximal EMG input in order to allow motorcontrol to be activated |
RoyvZ | 5:3562c205d001 | 211 | |
RoyvZ | 5:3562c205d001 | 212 | if (buttonCal.read() == false){ //activated when button is pressed |
RoyvZ | 5:3562c205d001 | 213 | led2 = !led2; |
RoyvZ | 5:3562c205d001 | 214 | for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds |
RoyvZ | 5:3562c205d001 | 215 | SY.erase(SY.begin()); |
RoyvZ | 5:3562c205d001 | 216 | double emgYCalvalue = bqc.step(emg1.read()-0.4542f); |
RoyvZ | 5:3562c205d001 | 217 | SY.push_back(emgYCalvalue); |
RoyvZ | 5:3562c205d001 | 218 | double signalfinal = findRMS(SY); //calculate RMS values from EMG signals |
RoyvZ | 5:3562c205d001 | 219 | if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal |
RoyvZ | 5:3562c205d001 | 220 | maxsignal = signalfinal; //keep changing the maximal signal |
RoyvZ | 5:3562c205d001 | 221 | } |
RoyvZ | 5:3562c205d001 | 222 | } |
RoyvZ | 5:3562c205d001 | 223 | led2 = 1; //Turn LED off when calibration is finished |
RoyvZ | 5:3562c205d001 | 224 | } |
RoyvZ | 5:3562c205d001 | 225 | } |
RoyvZ | 5:3562c205d001 | 226 | |
RoyvZ | 5:3562c205d001 | 227 | |
RoyvZ | 5:3562c205d001 | 228 | void NegPosChanger(){ |
RoyvZ | 5:3562c205d001 | 229 | if(NPC == 0){ |
RoyvZ | 5:3562c205d001 | 230 | NoTurnies = 0; |
RoyvZ | 5:3562c205d001 | 231 | NPC++; |
RoyvZ | 5:3562c205d001 | 232 | } else { |
RoyvZ | 5:3562c205d001 | 233 | NoTurnies = 1; |
RoyvZ | 5:3562c205d001 | 234 | NPC= 0; |
RoyvZ | 2:293665548183 | 235 | } |
RoyvZ | 2:293665548183 | 236 | } |
RoyvZ | 2:293665548183 | 237 | |
DiondeGreef | 4:afa85283eb18 | 238 | |
RoyvZ | 5:3562c205d001 | 239 | void qSetpointSet() { |
DiondeGreef | 4:afa85283eb18 | 240 | // Fill Matrix with data. |
RoyvZ | 5:3562c205d001 | 241 | q1 = encoder1.getPosition()*q_RAD_PER_PULSE_m1; //Calibreren nog toevoegen |
RoyvZ | 5:3562c205d001 | 242 | q2 = 0.5f*M_PI - q1 + encoder2.getPosition()*q_RAD_PER_PULSE_m2; //Calibreren mist nog |
RoyvZ | 5:3562c205d001 | 243 | |
RoyvZ | 5:3562c205d001 | 244 | JAPPAPP(1,1) = (cos(q1)*cos(q2) - sin(q1)*sin(q2))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1)); |
RoyvZ | 5:3562c205d001 | 245 | JAPPAPP(1,2) = (cos(q1)*sin(q2) + cos(q2)*sin(q1))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1)); |
RoyvZ | 5:3562c205d001 | 246 | JAPPAPP(2,1) = -(L1*cos(q1) + L2*cos(q1)*cos(q2) - L2*sin(q1)*sin(q2))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1)); |
RoyvZ | 5:3562c205d001 | 247 | JAPPAPP(2,2) = -(L1*sin(q1) + L2*cos(q1)*sin(q2) + L2*cos(q2)*sin(q1))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1)); |
RoyvZ | 5:3562c205d001 | 248 | |
RoyvZ | 5:3562c205d001 | 249 | // Fill Matrix with data. |
RoyvZ | 5:3562c205d001 | 250 | int Xchanger = (changeX == 1) ? -1 : 1; |
RoyvZ | 5:3562c205d001 | 251 | int Ychanger = (changeY == 1) ? -1 : 1; |
RoyvZ | 5:3562c205d001 | 252 | Vdes(1,1) = emgX*Xchanger*NoTurnies; |
RoyvZ | 5:3562c205d001 | 253 | Vdes(2,1) = emgY*Ychanger*NoTurnies; |
RoyvZ | 5:3562c205d001 | 254 | int turnChanger = (turning == 1) ? -1 : 1; |
DiondeGreef | 4:afa85283eb18 | 255 | |
DiondeGreef | 4:afa85283eb18 | 256 | qdot = JAPPAPP*Vdes; |
RoyvZ | 5:3562c205d001 | 257 | |
RoyvZ | 5:3562c205d001 | 258 | Qset1 = Qset1 + qdot(1,1)*Periode; //Vdes(1,1) |
RoyvZ | 5:3562c205d001 | 259 | Qset2 = Qset2 + qdot(2,1)*Periode; //Vdes(2,1) |
RoyvZ | 5:3562c205d001 | 260 | |
RoyvZ | 5:3562c205d001 | 261 | if (Qset1 <= 0.43f*M_PI && Qset1 >= 0.01f){ |
RoyvZ | 5:3562c205d001 | 262 | Qset1 = Qset1; |
RoyvZ | 5:3562c205d001 | 263 | } |
RoyvZ | 5:3562c205d001 | 264 | else if (Qset1 < 0.01f){ |
RoyvZ | 5:3562c205d001 | 265 | Qset1 = 0.01f; |
RoyvZ | 5:3562c205d001 | 266 | } |
RoyvZ | 5:3562c205d001 | 267 | else if (Qset1 > 0.43f*M_PI){ |
RoyvZ | 5:3562c205d001 | 268 | Qset1 = 0.43f*M_PI; |
RoyvZ | 5:3562c205d001 | 269 | } |
RoyvZ | 5:3562c205d001 | 270 | if (Qset1 + Qset2 < 0.5f*M_PI){ |
RoyvZ | 5:3562c205d001 | 271 | Qset2 = 0.5f*M_PI - Qset1; |
RoyvZ | 5:3562c205d001 | 272 | } |
RoyvZ | 5:3562c205d001 | 273 | if (Qset2 <= 0.5f*M_PI && Qset2 >= 0.1f){ |
RoyvZ | 5:3562c205d001 | 274 | Qset2 = Qset2; |
RoyvZ | 5:3562c205d001 | 275 | } |
RoyvZ | 5:3562c205d001 | 276 | else if (Qset2 < .1f){ |
RoyvZ | 5:3562c205d001 | 277 | Qset2 = 0.1f; |
RoyvZ | 5:3562c205d001 | 278 | } |
RoyvZ | 5:3562c205d001 | 279 | else if (Qset1+Qset2 > M_PI){ |
RoyvZ | 5:3562c205d001 | 280 | Qset2 = M_PI - Qset1; |
RoyvZ | 5:3562c205d001 | 281 | } |
RoyvZ | 5:3562c205d001 | 282 | |
RoyvZ | 5:3562c205d001 | 283 | } |
RoyvZ | 5:3562c205d001 | 284 | |
RoyvZ | 5:3562c205d001 | 285 | float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){ // Reusable PI controller |
RoyvZ | 5:3562c205d001 | 286 | return fabs(Kp * e); |
RoyvZ | 5:3562c205d001 | 287 | } |
RoyvZ | 5:3562c205d001 | 288 | |
RoyvZ | 5:3562c205d001 | 289 | void PositionControl() { |
RoyvZ | 5:3562c205d001 | 290 | //position1 = RAD_PER_PULSE * encoder1.getPosition(); |
RoyvZ | 5:3562c205d001 | 291 | //position2 = RAD_PER_PULSE * encoder2.getPosition(); |
RoyvZ | 5:3562c205d001 | 292 | ref_pos1 = Qset1 - q1; |
RoyvZ | 5:3562c205d001 | 293 | //if (ref_pos1 < 0.1) { ref_pos1 = 0; } |
RoyvZ | 5:3562c205d001 | 294 | ref_pos2 = Qset2 - q2; // Don’t use magic numbers! |
RoyvZ | 5:3562c205d001 | 295 | //if (ref_pos2 < 0.1) { ref_pos2 = 0; } |
RoyvZ | 5:3562c205d001 | 296 | motor1 = PI(ref_pos1, M1_KP, M1_KI, M1_TS, m1_err_int ); |
RoyvZ | 5:3562c205d001 | 297 | motor2 = PI(ref_pos2, M1_KP, M1_KI, M1_TS, m2_err_int ); |
RoyvZ | 5:3562c205d001 | 298 | if (ref_pos1 <= 0) { |
RoyvZ | 5:3562c205d001 | 299 | motorD1=1; |
RoyvZ | 5:3562c205d001 | 300 | } |
RoyvZ | 5:3562c205d001 | 301 | else{ |
RoyvZ | 5:3562c205d001 | 302 | motorD1 = 0; |
RoyvZ | 5:3562c205d001 | 303 | } |
RoyvZ | 5:3562c205d001 | 304 | if (ref_pos2 <= 0) { |
RoyvZ | 5:3562c205d001 | 305 | motorD2=0; |
RoyvZ | 5:3562c205d001 | 306 | } |
RoyvZ | 5:3562c205d001 | 307 | else{ |
RoyvZ | 5:3562c205d001 | 308 | motorD2=1; |
RoyvZ | 5:3562c205d001 | 309 | } |
RoyvZ | 5:3562c205d001 | 310 | UpperMotorDir.write(motorD1); |
RoyvZ | 5:3562c205d001 | 311 | UpperMotorVel.write(motor1); |
RoyvZ | 5:3562c205d001 | 312 | LowerMotorDir.write(motorD2); |
RoyvZ | 5:3562c205d001 | 313 | LowerMotorVel.write(motor2); |
RoyvZ | 5:3562c205d001 | 314 | } |
RoyvZ | 5:3562c205d001 | 315 | |
RoyvZ | 5:3562c205d001 | 316 | /** Servo motor control function **/ |
RoyvZ | 5:3562c205d001 | 317 | |
RoyvZ | 5:3562c205d001 | 318 | //Servo initialisation |
RoyvZ | 5:3562c205d001 | 319 | enum servostates{Close, Open}; |
RoyvZ | 5:3562c205d001 | 320 | |
RoyvZ | 5:3562c205d001 | 321 | //Set default state to open, as mentioned in paragraph ... of the report |
RoyvZ | 5:3562c205d001 | 322 | servostates CurrentState = Open; |
RoyvZ | 5:3562c205d001 | 323 | |
RoyvZ | 5:3562c205d001 | 324 | int i = 0; |
RoyvZ | 5:3562c205d001 | 325 | int j = 0; |
RoyvZ | 5:3562c205d001 | 326 | |
RoyvZ | 5:3562c205d001 | 327 | //Simply switch the current state when button is pressed |
RoyvZ | 5:3562c205d001 | 328 | void Change() { |
RoyvZ | 5:3562c205d001 | 329 | if(CurrentState == Close){ |
RoyvZ | 5:3562c205d001 | 330 | CurrentState = Open; |
RoyvZ | 5:3562c205d001 | 331 | } |
RoyvZ | 5:3562c205d001 | 332 | else{ |
RoyvZ | 5:3562c205d001 | 333 | CurrentState = Close; |
RoyvZ | 5:3562c205d001 | 334 | } |
DiondeGreef | 4:afa85283eb18 | 335 | } |
DiondeGreef | 4:afa85283eb18 | 336 | |
RoyvZ | 5:3562c205d001 | 337 | //State machine for the servo motors, to determine the state |
RoyvZ | 5:3562c205d001 | 338 | void ProcessStateMachine(void) { |
RoyvZ | 5:3562c205d001 | 339 | /** |
RoyvZ | 5:3562c205d001 | 340 | State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation. |
RoyvZ | 5:3562c205d001 | 341 | The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation. |
RoyvZ | 5:3562c205d001 | 342 | For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2. |
RoyvZ | 5:3562c205d001 | 343 | **/ |
RoyvZ | 5:3562c205d001 | 344 | switch (CurrentState) |
RoyvZ | 5:3562c205d001 | 345 | { |
RoyvZ | 5:3562c205d001 | 346 | case Close: |
RoyvZ | 5:3562c205d001 | 347 | if(j <= 35){ |
RoyvZ | 5:3562c205d001 | 348 | RightServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 349 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 350 | j++; |
RoyvZ | 5:3562c205d001 | 351 | } |
RoyvZ | 5:3562c205d001 | 352 | else if (j == 36){ |
RoyvZ | 5:3562c205d001 | 353 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 354 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 355 | j++; |
RoyvZ | 5:3562c205d001 | 356 | } |
RoyvZ | 5:3562c205d001 | 357 | else if (j == 37){ |
RoyvZ | 5:3562c205d001 | 358 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 359 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 360 | j++; |
RoyvZ | 5:3562c205d001 | 361 | } |
RoyvZ | 5:3562c205d001 | 362 | else if (j == 38){ |
RoyvZ | 5:3562c205d001 | 363 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 364 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 365 | j++; |
RoyvZ | 5:3562c205d001 | 366 | } |
RoyvZ | 5:3562c205d001 | 367 | else if (j == 39){ |
RoyvZ | 5:3562c205d001 | 368 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 369 | LeftServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 370 | j = 0; |
RoyvZ | 5:3562c205d001 | 371 | } |
RoyvZ | 5:3562c205d001 | 372 | break; |
RoyvZ | 5:3562c205d001 | 373 | |
RoyvZ | 5:3562c205d001 | 374 | case Open: |
RoyvZ | 5:3562c205d001 | 375 | if(j <= 35){ |
RoyvZ | 5:3562c205d001 | 376 | RightServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 377 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 378 | j++; |
RoyvZ | 5:3562c205d001 | 379 | } |
RoyvZ | 5:3562c205d001 | 380 | else if (j == 36){ |
RoyvZ | 5:3562c205d001 | 381 | RightServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 382 | LeftServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 383 | j++; |
RoyvZ | 5:3562c205d001 | 384 | } |
RoyvZ | 5:3562c205d001 | 385 | else if (j == 37){ |
RoyvZ | 5:3562c205d001 | 386 | RightServoVel.write(0); |
RoyvZ | 5:3562c205d001 | 387 | LeftServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 388 | j++; |
RoyvZ | 5:3562c205d001 | 389 | } |
RoyvZ | 5:3562c205d001 | 390 | else if (j == 38){ |
RoyvZ | 5:3562c205d001 | 391 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 392 | LeftServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 393 | j++; |
RoyvZ | 5:3562c205d001 | 394 | } |
RoyvZ | 5:3562c205d001 | 395 | else if (j == 39){ |
RoyvZ | 5:3562c205d001 | 396 | RightServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 397 | LeftServoVel.write(1); |
RoyvZ | 5:3562c205d001 | 398 | j = 0; |
RoyvZ | 5:3562c205d001 | 399 | } |
RoyvZ | 5:3562c205d001 | 400 | break; |
RoyvZ | 5:3562c205d001 | 401 | } |
RoyvZ | 5:3562c205d001 | 402 | } |
RoyvZ | 5:3562c205d001 | 403 | |
RoyvZ | 5:3562c205d001 | 404 | //StateMachine for the moving of the end effector |
RoyvZ | 5:3562c205d001 | 405 | |
RoyvZ | 5:3562c205d001 | 406 | void ProcessStateMachineMain(void) { |
RoyvZ | 5:3562c205d001 | 407 | states CurrentState1 = calibEMG; |
RoyvZ | 5:3562c205d001 | 408 | switch (CurrentState1) |
RoyvZ | 5:3562c205d001 | 409 | { |
RoyvZ | 5:3562c205d001 | 410 | case MotionEndEffector: |
RoyvZ | 5:3562c205d001 | 411 | m1_Ticker.attach( &PositionControl, M1_TS ); |
RoyvZ | 5:3562c205d001 | 412 | setpoint_Ticker.attach(&qSetpointSet, Periode); |
RoyvZ | 5:3562c205d001 | 413 | break; |
RoyvZ | 5:3562c205d001 | 414 | |
RoyvZ | 5:3562c205d001 | 415 | case calibEMG: |
RoyvZ | 5:3562c205d001 | 416 | //Function to calibrate the filtered EMG signals to the user, in order to control the motors |
RoyvZ | 5:3562c205d001 | 417 | double maxsignal = 0; //empty variable to check the maximal EMG output |
RoyvZ | 5:3562c205d001 | 418 | for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 5 seconds |
RoyvZ | 5:3562c205d001 | 419 | double signalfinal = fabs(bqc.step(emg0.read())); //get values from the filtered EMG signal |
RoyvZ | 5:3562c205d001 | 420 | if (signalfinal >= maxsignal){ //check whether the received signal is higher or lower than the current highest signal value |
RoyvZ | 5:3562c205d001 | 421 | maxsignal = signalfinal; //keep changing the maximal signal |
RoyvZ | 5:3562c205d001 | 422 | } |
RoyvZ | 5:3562c205d001 | 423 | } |
RoyvZ | 5:3562c205d001 | 424 | break; |
RoyvZ | 5:3562c205d001 | 425 | |
RoyvZ | 5:3562c205d001 | 426 | case calib1: |
RoyvZ | 5:3562c205d001 | 427 | UpperMotorVel = Cal_Strength; UpperMotorDir = true; //depends on definitions |
RoyvZ | 5:3562c205d001 | 428 | LowerMotorVel = 0.0f; |
RoyvZ | 5:3562c205d001 | 429 | UpperMotorPos = encoder1.getPosition(); |
RoyvZ | 5:3562c205d001 | 430 | if (abs(UpperMotorPos - UpperMotorPos_prev) < 0.01f){ |
RoyvZ | 5:3562c205d001 | 431 | if (Cal_Strength < 1.0f) { |
RoyvZ | 5:3562c205d001 | 432 | Cal_Strength += 0.05f; |
RoyvZ | 5:3562c205d001 | 433 | } |
RoyvZ | 5:3562c205d001 | 434 | else{ |
RoyvZ | 5:3562c205d001 | 435 | encoder1.setPosition(0); //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi |
RoyvZ | 5:3562c205d001 | 436 | CurrentState1 = calib2; |
RoyvZ | 5:3562c205d001 | 437 | Cal_Strength = 0.6f; |
RoyvZ | 5:3562c205d001 | 438 | } |
RoyvZ | 5:3562c205d001 | 439 | int UpperMotorPos_prev = UpperMotorPos; |
RoyvZ | 5:3562c205d001 | 440 | } |
RoyvZ | 5:3562c205d001 | 441 | break; |
RoyvZ | 5:3562c205d001 | 442 | |
RoyvZ | 5:3562c205d001 | 443 | case calib2: |
RoyvZ | 5:3562c205d001 | 444 | LowerMotorVel = Cal_Strength; LowerMotorDir = true; // Check definitions |
RoyvZ | 5:3562c205d001 | 445 | UpperMotorVel = 1.0f; |
RoyvZ | 5:3562c205d001 | 446 | LowerMotorPos = encoder2.getPosition(); |
RoyvZ | 5:3562c205d001 | 447 | if (abs(LowerMotorPos - UpperMotorPos_prev) < 0.01f){ |
RoyvZ | 5:3562c205d001 | 448 | if (Cal_Strength < 1.0f) { |
RoyvZ | 5:3562c205d001 | 449 | Cal_Strength += 0.05f; |
RoyvZ | 5:3562c205d001 | 450 | } |
RoyvZ | 5:3562c205d001 | 451 | else{ |
RoyvZ | 5:3562c205d001 | 452 | encoder2.setPosition(0); |
RoyvZ | 5:3562c205d001 | 453 | CurrentState1 = Idle; |
RoyvZ | 5:3562c205d001 | 454 | //M1_controller = true; //turn on the position controller here |
RoyvZ | 5:3562c205d001 | 455 | UpperMotorVel = 0.0f; LowerMotorVel = 0.0f; |
RoyvZ | 5:3562c205d001 | 456 | } |
RoyvZ | 5:3562c205d001 | 457 | int UpperMotorPos_prev = UpperMotorPos; |
RoyvZ | 5:3562c205d001 | 458 | } |
RoyvZ | 5:3562c205d001 | 459 | break; |
RoyvZ | 5:3562c205d001 | 460 | |
RoyvZ | 5:3562c205d001 | 461 | case Idle: |
RoyvZ | 5:3562c205d001 | 462 | emgX = 0; |
RoyvZ | 5:3562c205d001 | 463 | emgY = 0; |
RoyvZ | 5:3562c205d001 | 464 | m1_Ticker.attach( &PositionControl, M1_TS ); |
RoyvZ | 5:3562c205d001 | 465 | setpoint_Ticker.attach(&qSetpointSet, Periode); |
RoyvZ | 5:3562c205d001 | 466 | break; |
RoyvZ | 5:3562c205d001 | 467 | |
RoyvZ | 5:3562c205d001 | 468 | case Off: |
RoyvZ | 5:3562c205d001 | 469 | //Robot is off, also return to default state so you have a reference position when it starts up again |
RoyvZ | 5:3562c205d001 | 470 | break; |
RoyvZ | 5:3562c205d001 | 471 | |
RoyvZ | 5:3562c205d001 | 472 | default: |
RoyvZ | 5:3562c205d001 | 473 | //Robot is in idle mode, not doing anything (motors still powered to hold current position) (Posible when waiting for your turn) |
RoyvZ | 5:3562c205d001 | 474 | |
RoyvZ | 5:3562c205d001 | 475 | } |
RoyvZ | 5:3562c205d001 | 476 | } |
RoyvZ | 5:3562c205d001 | 477 | |
RoyvZ | 5:3562c205d001 | 478 | //Main function for the servo motors, to change open or closed state |
RoyvZ | 5:3562c205d001 | 479 | int main() { |
RoyvZ | 5:3562c205d001 | 480 | //allow for the 2000 Hz sampling of the Servo state machine function |
RoyvZ | 5:3562c205d001 | 481 | MainTicker.attach(ProcessStateMachineMain, 0.0005); |
RoyvZ | 5:3562c205d001 | 482 | |
RoyvZ | 5:3562c205d001 | 483 | ServoTick.attach(ProcessStateMachine, 0.0005); |
RoyvZ | 5:3562c205d001 | 484 | |
RoyvZ | 5:3562c205d001 | 485 | |
RoyvZ | 5:3562c205d001 | 486 | |
RoyvZ | 5:3562c205d001 | 487 | |
RoyvZ | 5:3562c205d001 | 488 | |
RoyvZ | 5:3562c205d001 | 489 | /** EMG signals **/ |
RoyvZ | 2:293665548183 | 490 | //Constructing the notch filter chain |
RoyvZ | 3:2ffbee47fb38 | 491 | bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 ); |
RoyvZ | 5:3562c205d001 | 492 | //Sample every 0.002 seconds, or 500 Hz |
RoyvZ | 5:3562c205d001 | 493 | sample_timer.attach(&sample, 0.002); |
RoyvZ | 5:3562c205d001 | 494 | //Ticker to check if motor is being calibrated, also at 500 Hz |
RoyvZ | 5:3562c205d001 | 495 | calX_timer.attach(&calibrationX, 0.002); |
RoyvZ | 5:3562c205d001 | 496 | calY_timer.attach(&calibrationY, 0.002); |
RoyvZ | 5:3562c205d001 | 497 | //Turn LED off, turned on when calibrating |
RoyvZ | 5:3562c205d001 | 498 | led2 = 1; |
RoyvZ | 3:2ffbee47fb38 | 499 | |
RoyvZ | 5:3562c205d001 | 500 | turning.rise(&NegPosChanger); |
RoyvZ | 5:3562c205d001 | 501 | |
RoyvZ | 5:3562c205d001 | 502 | //MODSERIAL connection |
RoyvZ | 5:3562c205d001 | 503 | pc.baud(115200); |
RoyvZ | 5:3562c205d001 | 504 | |
RoyvZ | 5:3562c205d001 | 505 | |
RoyvZ | 5:3562c205d001 | 506 | while (true) |
RoyvZ | 5:3562c205d001 | 507 | { |
DiondeGreef | 4:afa85283eb18 | 508 | |
RoyvZ | 5:3562c205d001 | 509 | if (buttonCal.read() == false){ //when calibrate button is pressed |
RoyvZ | 5:3562c205d001 | 510 | states CurrentState1 = calibEMG; |
RoyvZ | 5:3562c205d001 | 511 | } |
RoyvZ | 5:3562c205d001 | 512 | |
RoyvZ | 5:3562c205d001 | 513 | if (!ServoButton) { |
RoyvZ | 5:3562c205d001 | 514 | Change(); |
RoyvZ | 5:3562c205d001 | 515 | wait(0.3); |
RoyvZ | 5:3562c205d001 | 516 | } |
RoyvZ | 5:3562c205d001 | 517 | |
RoyvZ | 5:3562c205d001 | 518 | |
RoyvZ | 5:3562c205d001 | 519 | /*Control end-effector |
RoyvZ | 5:3562c205d001 | 520 | wait(0.1); |
RoyvZ | 5:3562c205d001 | 521 | float v_ref = GetReferenceVelocity(); |
RoyvZ | 5:3562c205d001 | 522 | setMotor(v_ref); |
RoyvZ | 5:3562c205d001 | 523 | pc.printf("%f \r\n", FeedForwardControl(v_ref)); |
RoyvZ | 5:3562c205d001 | 524 | UpperMotorDir.write(motorDirection); |
RoyvZ | 5:3562c205d001 | 525 | UpperMotorVel.write(RightServoVel); //PWM Speed Control |
RoyvZ | 5:3562c205d001 | 526 | */ |
RoyvZ | 0:b6c8d56842ce | 527 | } |
RoyvZ | 5:3562c205d001 | 528 | } |