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