Chess robot with NR method
Dependencies: QEI biquadFilter mbed HIDScope
NR_method_1.cpp@2:f68fd7b1c655, 2018-11-01 (annotated)
- Committer:
- Thijsjeee
- Date:
- Thu Nov 01 11:13:32 2018 +0000
- Revision:
- 2:f68fd7b1c655
- Parent:
- 1:fafea1d00d0c
- Child:
- 3:40427c0157a0
nieuwe versie
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Thijsjeee | 0:0af507ea0d83 | 1 | #include "mbed.h" |
Thijsjeee | 0:0af507ea0d83 | 2 | #include "BiQuad.h" |
Thijsjeee | 0:0af507ea0d83 | 3 | #include <math.h> |
Thijsjeee | 0:0af507ea0d83 | 4 | #include <stdio.h> |
Thijsjeee | 0:0af507ea0d83 | 5 | #include <iostream> |
Thijsjeee | 0:0af507ea0d83 | 6 | #include <stdlib.h> |
Thijsjeee | 0:0af507ea0d83 | 7 | #include <ctime> |
Thijsjeee | 0:0af507ea0d83 | 8 | #include <QEI.h> |
Thijsjeee | 1:fafea1d00d0c | 9 | #include "PID_controler.h" |
Thijsjeee | 1:fafea1d00d0c | 10 | //#include <MODSERIAL.h> |
Thijsjeee | 1:fafea1d00d0c | 11 | |
Thijsjeee | 1:fafea1d00d0c | 12 | bool bas; |
Thijsjeee | 2:f68fd7b1c655 | 13 | int waiting; |
Thijsjeee | 2:f68fd7b1c655 | 14 | InterruptIn button(SW3); |
Thijsjeee | 1:fafea1d00d0c | 15 | |
Thijsjeee | 1:fafea1d00d0c | 16 | Serial pc(USBTX, USBRX); |
Thijsjeee | 1:fafea1d00d0c | 17 | // emg signals input |
Thijsjeee | 1:fafea1d00d0c | 18 | AnalogIn emg1(A0); |
Thijsjeee | 1:fafea1d00d0c | 19 | AnalogIn emg2(A1); |
Thijsjeee | 1:fafea1d00d0c | 20 | InterruptIn sw2(SW2); |
Thijsjeee | 1:fafea1d00d0c | 21 | // tickers |
Thijsjeee | 1:fafea1d00d0c | 22 | Ticker sample_timer; |
Thijsjeee | 1:fafea1d00d0c | 23 | |
Thijsjeee | 1:fafea1d00d0c | 24 | volatile int x1; |
Thijsjeee | 1:fafea1d00d0c | 25 | volatile int y1; |
Thijsjeee | 1:fafea1d00d0c | 26 | double emgFiltered3; |
Thijsjeee | 1:fafea1d00d0c | 27 | double emgFiltered23; |
Thijsjeee | 1:fafea1d00d0c | 28 | bool dir = true; |
Thijsjeee | 1:fafea1d00d0c | 29 | // filtering |
Thijsjeee | 1:fafea1d00d0c | 30 | //filter coeffiecents |
Thijsjeee | 1:fafea1d00d0c | 31 | // highpass |
Thijsjeee | 1:fafea1d00d0c | 32 | double b01h = 0.978030479206560; |
Thijsjeee | 1:fafea1d00d0c | 33 | double b02h = -1.95606095841312; |
Thijsjeee | 1:fafea1d00d0c | 34 | double b03h = 0.978030479206560; |
Thijsjeee | 1:fafea1d00d0c | 35 | double a01h = -1.95557824031504; |
Thijsjeee | 1:fafea1d00d0c | 36 | double a02h = 0.956543676511203; |
Thijsjeee | 1:fafea1d00d0c | 37 | // notchfilter |
Thijsjeee | 1:fafea1d00d0c | 38 | double b01n = 0.995532032687234; |
Thijsjeee | 1:fafea1d00d0c | 39 | double b02n = -1.89361445373551; |
Thijsjeee | 1:fafea1d00d0c | 40 | double b03n = 0.995532032687234; |
Thijsjeee | 1:fafea1d00d0c | 41 | double a01n = -1.89361445373551; |
Thijsjeee | 1:fafea1d00d0c | 42 | double a02n = 0.991064065374468 ; |
Thijsjeee | 1:fafea1d00d0c | 43 | //lowpass 1 |
Thijsjeee | 1:fafea1d00d0c | 44 | double b01l = 8.76555487540065e-05; |
Thijsjeee | 1:fafea1d00d0c | 45 | double b02l = 0.000175311097508013; |
Thijsjeee | 1:fafea1d00d0c | 46 | double b03l = 8.76555487540065e-05; |
Thijsjeee | 1:fafea1d00d0c | 47 | double a01l = -1.97334424978130; |
Thijsjeee | 1:fafea1d00d0c | 48 | double a02l = 0.973694871976315; |
Thijsjeee | 1:fafea1d00d0c | 49 | |
Thijsjeee | 1:fafea1d00d0c | 50 | BiQuadChain bqc; |
Thijsjeee | 1:fafea1d00d0c | 51 | BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass |
Thijsjeee | 1:fafea1d00d0c | 52 | BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch |
Thijsjeee | 1:fafea1d00d0c | 53 | // than we need to rectifie |
Thijsjeee | 1:fafea1d00d0c | 54 | // and lowpass afterwards |
Thijsjeee | 1:fafea1d00d0c | 55 | BiQuadChain bqc2; |
Thijsjeee | 1:fafea1d00d0c | 56 | BiQuad bq3( b01l, b02l, b03l, a01l, a02l); //lowpass |
Thijsjeee | 1:fafea1d00d0c | 57 | // optional is doing a movingaverage after |
Thijsjeee | 1:fafea1d00d0c | 58 | |
Thijsjeee | 1:fafea1d00d0c | 59 | BiQuadChain bqc3; |
Thijsjeee | 1:fafea1d00d0c | 60 | BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass |
Thijsjeee | 1:fafea1d00d0c | 61 | BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch |
Thijsjeee | 1:fafea1d00d0c | 62 | // than we need to rectifie |
Thijsjeee | 1:fafea1d00d0c | 63 | // and lowpass afterwards |
Thijsjeee | 1:fafea1d00d0c | 64 | BiQuadChain bqc4; |
Thijsjeee | 1:fafea1d00d0c | 65 | BiQuad bq6( b01l, b02l, b03l, a01l, a02l); //lowpass |
Thijsjeee | 1:fafea1d00d0c | 66 | |
Thijsjeee | 0:0af507ea0d83 | 67 | |
Thijsjeee | 0:0af507ea0d83 | 68 | //Define in/outputs |
Thijsjeee | 0:0af507ea0d83 | 69 | |
Thijsjeee | 1:fafea1d00d0c | 70 | int counts = 8400; |
Thijsjeee | 0:0af507ea0d83 | 71 | DigitalOut Led(LED1); |
Thijsjeee | 0:0af507ea0d83 | 72 | PwmOut PMW1(D5); // Motor 1 |
Thijsjeee | 0:0af507ea0d83 | 73 | DigitalOut M1(D4); // direction of motor 1 |
Thijsjeee | 0:0af507ea0d83 | 74 | PwmOut PMW2(D6); // Motor 2 |
Thijsjeee | 0:0af507ea0d83 | 75 | DigitalOut M2(D7); // direction of motor 2 |
Thijsjeee | 0:0af507ea0d83 | 76 | |
Thijsjeee | 0:0af507ea0d83 | 77 | //initializing Encoders |
Thijsjeee | 0:0af507ea0d83 | 78 | QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder |
Thijsjeee | 0:0af507ea0d83 | 79 | QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated |
Thijsjeee | 0:0af507ea0d83 | 80 | |
Thijsjeee | 0:0af507ea0d83 | 81 | double Kp = 1; |
Thijsjeee | 2:f68fd7b1c655 | 82 | double Ki = 1; |
Thijsjeee | 1:fafea1d00d0c | 83 | double Kd = 0.3; |
Thijsjeee | 0:0af507ea0d83 | 84 | double Ts = 0.001; |
Thijsjeee | 0:0af507ea0d83 | 85 | |
Thijsjeee | 0:0af507ea0d83 | 86 | float counts_a; |
Thijsjeee | 0:0af507ea0d83 | 87 | float counts_b; |
Thijsjeee | 0:0af507ea0d83 | 88 | |
Thijsjeee | 0:0af507ea0d83 | 89 | |
Thijsjeee | 0:0af507ea0d83 | 90 | //Define Variables |
Thijsjeee | 0:0af507ea0d83 | 91 | |
Thijsjeee | 0:0af507ea0d83 | 92 | double pi = 3.14159265359; |
Thijsjeee | 1:fafea1d00d0c | 93 | int bb; |
Thijsjeee | 1:fafea1d00d0c | 94 | int bc; |
Thijsjeee | 0:0af507ea0d83 | 95 | float z; |
Thijsjeee | 0:0af507ea0d83 | 96 | |
Thijsjeee | 1:fafea1d00d0c | 97 | double angle_a = 0; //in rad |
Thijsjeee | 1:fafea1d00d0c | 98 | double angle_b = 0.5 * pi; //in rad |
Thijsjeee | 0:0af507ea0d83 | 99 | |
Thijsjeee | 0:0af507ea0d83 | 100 | double X0[2][1] = {{angle_a},{angle_b}}; |
Thijsjeee | 0:0af507ea0d83 | 101 | double X[2][1]; |
Thijsjeee | 0:0af507ea0d83 | 102 | double Xold[2][1]; |
Thijsjeee | 0:0af507ea0d83 | 103 | double fval[2][1]; |
Thijsjeee | 0:0af507ea0d83 | 104 | double J[2][2]; |
Thijsjeee | 0:0af507ea0d83 | 105 | double err[2][1]; |
Thijsjeee | 0:0af507ea0d83 | 106 | |
Thijsjeee | 0:0af507ea0d83 | 107 | double MaxIter = 20; |
Thijsjeee | 0:0af507ea0d83 | 108 | double tolX = 1e-4; |
Thijsjeee | 1:fafea1d00d0c | 109 | double A = 20; |
Thijsjeee | 1:fafea1d00d0c | 110 | double B = 30; |
Thijsjeee | 1:fafea1d00d0c | 111 | double C = 20; |
Thijsjeee | 1:fafea1d00d0c | 112 | double D = 27; |
Thijsjeee | 1:fafea1d00d0c | 113 | double E = 35; |
Thijsjeee | 1:fafea1d00d0c | 114 | double ex = -35; // current position |
Thijsjeee | 1:fafea1d00d0c | 115 | double ey = 27; // current position |
Thijsjeee | 1:fafea1d00d0c | 116 | double Cxx = -35; // Goal position |
Thijsjeee | 1:fafea1d00d0c | 117 | double Cyy = 27; // Goal position |
Thijsjeee | 0:0af507ea0d83 | 118 | |
Thijsjeee | 0:0af507ea0d83 | 119 | Ticker position_controll; |
Thijsjeee | 0:0af507ea0d83 | 120 | |
Thijsjeee | 1:fafea1d00d0c | 121 | void filteren () |
Thijsjeee | 1:fafea1d00d0c | 122 | { |
Thijsjeee | 1:fafea1d00d0c | 123 | double emgSignal1 = emg1.read(); |
Thijsjeee | 1:fafea1d00d0c | 124 | double emgSignal2 = emg2.read(); |
Thijsjeee | 1:fafea1d00d0c | 125 | |
Thijsjeee | 1:fafea1d00d0c | 126 | double emgFiltered1 = bqc.step(emgSignal1); |
Thijsjeee | 1:fafea1d00d0c | 127 | double emgFiltered2 = fabs(emgFiltered1); |
Thijsjeee | 1:fafea1d00d0c | 128 | emgFiltered3 = bqc2.step(emgFiltered2); |
Thijsjeee | 1:fafea1d00d0c | 129 | |
Thijsjeee | 1:fafea1d00d0c | 130 | double emgFiltered21 = bqc3.step(emgSignal2); |
Thijsjeee | 1:fafea1d00d0c | 131 | double emgFiltered22 = fabs(emgFiltered21); |
Thijsjeee | 1:fafea1d00d0c | 132 | emgFiltered23 = bqc4.step(emgFiltered22); |
Thijsjeee | 1:fafea1d00d0c | 133 | |
Thijsjeee | 1:fafea1d00d0c | 134 | } |
Thijsjeee | 1:fafea1d00d0c | 135 | |
Thijsjeee | 1:fafea1d00d0c | 136 | void Position1x(double b) |
Thijsjeee | 1:fafea1d00d0c | 137 | { |
Thijsjeee | 1:fafea1d00d0c | 138 | if (b > 0.20) { |
Thijsjeee | 1:fafea1d00d0c | 139 | Cxx =x1; |
Thijsjeee | 1:fafea1d00d0c | 140 | if (dir == true) { |
Thijsjeee | 1:fafea1d00d0c | 141 | if(x1 > -46) { |
Thijsjeee | 1:fafea1d00d0c | 142 | x1 = x1-4; |
Thijsjeee | 1:fafea1d00d0c | 143 | pc.printf(" posx is %f\n\r",Cxx); |
Thijsjeee | 1:fafea1d00d0c | 144 | //return x1; |
Thijsjeee | 1:fafea1d00d0c | 145 | |
Thijsjeee | 1:fafea1d00d0c | 146 | } else if ( x1 <= -46) { |
Thijsjeee | 1:fafea1d00d0c | 147 | x1 =-14; |
Thijsjeee | 1:fafea1d00d0c | 148 | //return x1; |
Thijsjeee | 1:fafea1d00d0c | 149 | } else { |
Thijsjeee | 1:fafea1d00d0c | 150 | } |
Thijsjeee | 1:fafea1d00d0c | 151 | } else { |
Thijsjeee | 1:fafea1d00d0c | 152 | if(x1 < -14) { |
Thijsjeee | 1:fafea1d00d0c | 153 | x1 = x1+4; |
Thijsjeee | 1:fafea1d00d0c | 154 | //return x1; |
Thijsjeee | 1:fafea1d00d0c | 155 | |
Thijsjeee | 1:fafea1d00d0c | 156 | } else if ( x1 >= -14) { |
Thijsjeee | 1:fafea1d00d0c | 157 | x1 = -46; |
Thijsjeee | 1:fafea1d00d0c | 158 | //return x1; |
Thijsjeee | 1:fafea1d00d0c | 159 | } else { |
Thijsjeee | 1:fafea1d00d0c | 160 | } |
Thijsjeee | 1:fafea1d00d0c | 161 | } |
Thijsjeee | 1:fafea1d00d0c | 162 | |
Thijsjeee | 1:fafea1d00d0c | 163 | wait(0.5); |
Thijsjeee | 1:fafea1d00d0c | 164 | } |
Thijsjeee | 1:fafea1d00d0c | 165 | } |
Thijsjeee | 1:fafea1d00d0c | 166 | |
Thijsjeee | 1:fafea1d00d0c | 167 | void Position1y(double b) |
Thijsjeee | 1:fafea1d00d0c | 168 | { |
Thijsjeee | 1:fafea1d00d0c | 169 | if (b > 0.20) { |
Thijsjeee | 1:fafea1d00d0c | 170 | Cyy=y1; |
Thijsjeee | 1:fafea1d00d0c | 171 | if(dir == true) { |
Thijsjeee | 1:fafea1d00d0c | 172 | if(y1 < 43) { |
Thijsjeee | 1:fafea1d00d0c | 173 | y1 = y1+4; |
Thijsjeee | 1:fafea1d00d0c | 174 | //return y1; |
Thijsjeee | 1:fafea1d00d0c | 175 | } else if ( y1 >= 43) { |
Thijsjeee | 1:fafea1d00d0c | 176 | y1 = 11; |
Thijsjeee | 1:fafea1d00d0c | 177 | //return y1; |
Thijsjeee | 1:fafea1d00d0c | 178 | } else { |
Thijsjeee | 1:fafea1d00d0c | 179 | } |
Thijsjeee | 1:fafea1d00d0c | 180 | } else { |
Thijsjeee | 1:fafea1d00d0c | 181 | if(y1 > 11) { |
Thijsjeee | 1:fafea1d00d0c | 182 | y1 = y1-4; |
Thijsjeee | 1:fafea1d00d0c | 183 | //return y1; |
Thijsjeee | 1:fafea1d00d0c | 184 | } else if ( y1 <= 11) { |
Thijsjeee | 1:fafea1d00d0c | 185 | y1 = 43; |
Thijsjeee | 1:fafea1d00d0c | 186 | //return y1; |
Thijsjeee | 1:fafea1d00d0c | 187 | } else { |
Thijsjeee | 1:fafea1d00d0c | 188 | } |
Thijsjeee | 1:fafea1d00d0c | 189 | } |
Thijsjeee | 1:fafea1d00d0c | 190 | |
Thijsjeee | 1:fafea1d00d0c | 191 | wait(0.5); |
Thijsjeee | 1:fafea1d00d0c | 192 | } |
Thijsjeee | 1:fafea1d00d0c | 193 | } |
Thijsjeee | 1:fafea1d00d0c | 194 | void change() |
Thijsjeee | 1:fafea1d00d0c | 195 | { |
Thijsjeee | 1:fafea1d00d0c | 196 | dir = !dir; |
Thijsjeee | 1:fafea1d00d0c | 197 | } |
Thijsjeee | 0:0af507ea0d83 | 198 | void NR() //Newton Rapshon Calculation |
Thijsjeee | 0:0af507ea0d83 | 199 | { |
Thijsjeee | 0:0af507ea0d83 | 200 | //Variables |
Thijsjeee | 1:fafea1d00d0c | 201 | double Hoa = X[0][0]; |
Thijsjeee | 1:fafea1d00d0c | 202 | double Hob = X[1][0]; |
Thijsjeee | 1:fafea1d00d0c | 203 | |
Thijsjeee | 1:fafea1d00d0c | 204 | double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex; |
Thijsjeee | 1:fafea1d00d0c | 205 | double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey; |
Thijsjeee | 1:fafea1d00d0c | 206 | |
Thijsjeee | 0:0af507ea0d83 | 207 | //Define f(x) |
Thijsjeee | 1:fafea1d00d0c | 208 | fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2); |
Thijsjeee | 1:fafea1d00d0c | 209 | fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2); |
Thijsjeee | 0:0af507ea0d83 | 210 | //Jacobian |
Thijsjeee | 1:fafea1d00d0c | 211 | |
Thijsjeee | 1:fafea1d00d0c | 212 | |
Thijsjeee | 1:fafea1d00d0c | 213 | |
Thijsjeee | 1:fafea1d00d0c | 214 | J[0][0]= 0; |
Thijsjeee | 1:fafea1d00d0c | 215 | J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob)); |
Thijsjeee | 1:fafea1d00d0c | 216 | J[1][0]= - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2; |
Thijsjeee | 1:fafea1d00d0c | 217 | J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2; |
Thijsjeee | 0:0af507ea0d83 | 218 | } |
Thijsjeee | 0:0af507ea0d83 | 219 | |
Thijsjeee | 1:fafea1d00d0c | 220 | void angle_define() //define the angle needed. |
Thijsjeee | 1:fafea1d00d0c | 221 | { |
Thijsjeee | 1:fafea1d00d0c | 222 | for(int i=1 ; i <= MaxIter; i++) { |
Thijsjeee | 1:fafea1d00d0c | 223 | NR(); |
Thijsjeee | 0:0af507ea0d83 | 224 | |
Thijsjeee | 1:fafea1d00d0c | 225 | X[0][0] = X[0][0] - ((-J[1][1]/(J[0][1] *J[1][0]))*fval[0][0] + (1/J[1][0])* fval[0][1]); |
Thijsjeee | 1:fafea1d00d0c | 226 | X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]); |
Thijsjeee | 0:0af507ea0d83 | 227 | |
Thijsjeee | 0:0af507ea0d83 | 228 | err[0][0] = abs(X[0][0] - Xold[0][0]); |
Thijsjeee | 0:0af507ea0d83 | 229 | err[1][0] = abs(X[1][0] - Xold[1][0]); |
Thijsjeee | 0:0af507ea0d83 | 230 | |
Thijsjeee | 0:0af507ea0d83 | 231 | Xold[0][0] = X[0][0]; |
Thijsjeee | 0:0af507ea0d83 | 232 | Xold[1][0] = X[1][0]; |
Thijsjeee | 1:fafea1d00d0c | 233 | |
Thijsjeee | 0:0af507ea0d83 | 234 | counts_a = ((X[0][0]) / (2* pi)) * 8400; |
Thijsjeee | 1:fafea1d00d0c | 235 | counts_b = ((X[1][0]) / (2* pi)) * 8400; |
Thijsjeee | 1:fafea1d00d0c | 236 | |
Thijsjeee | 1:fafea1d00d0c | 237 | if(err[0][0] <= tolX) { |
Thijsjeee | 1:fafea1d00d0c | 238 | if(err[1][0] <= tolX) { |
Thijsjeee | 0:0af507ea0d83 | 239 | break; |
Thijsjeee | 0:0af507ea0d83 | 240 | } |
Thijsjeee | 1:fafea1d00d0c | 241 | } |
Thijsjeee | 1:fafea1d00d0c | 242 | } |
Thijsjeee | 0:0af507ea0d83 | 243 | } |
Thijsjeee | 0:0af507ea0d83 | 244 | |
Thijsjeee | 0:0af507ea0d83 | 245 | void position_define() |
Thijsjeee | 0:0af507ea0d83 | 246 | { |
Thijsjeee | 1:fafea1d00d0c | 247 | if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) { |
Thijsjeee | 1:fafea1d00d0c | 248 | if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) { |
Thijsjeee | 1:fafea1d00d0c | 249 | } else { |
Thijsjeee | 1:fafea1d00d0c | 250 | if (ey > Cyy) { |
Thijsjeee | 1:fafea1d00d0c | 251 | ey = ey - 0.004; |
Thijsjeee | 1:fafea1d00d0c | 252 | } |
Thijsjeee | 1:fafea1d00d0c | 253 | if (ey < Cyy) { |
Thijsjeee | 1:fafea1d00d0c | 254 | ey = ey + 0.004; |
Thijsjeee | 1:fafea1d00d0c | 255 | } |
Thijsjeee | 0:0af507ea0d83 | 256 | } |
Thijsjeee | 1:fafea1d00d0c | 257 | } else { |
Thijsjeee | 1:fafea1d00d0c | 258 | if (ex > Cxx) { |
Thijsjeee | 1:fafea1d00d0c | 259 | ex = ex - 0.004; |
Thijsjeee | 1:fafea1d00d0c | 260 | } |
Thijsjeee | 1:fafea1d00d0c | 261 | if (ex < Cxx) { |
Thijsjeee | 1:fafea1d00d0c | 262 | ex = ex + 0.004; |
Thijsjeee | 0:0af507ea0d83 | 263 | } |
Thijsjeee | 0:0af507ea0d83 | 264 | } |
Thijsjeee | 0:0af507ea0d83 | 265 | } |
Thijsjeee | 0:0af507ea0d83 | 266 | |
Thijsjeee | 1:fafea1d00d0c | 267 | |
Thijsjeee | 1:fafea1d00d0c | 268 | |
Thijsjeee | 1:fafea1d00d0c | 269 | |
Thijsjeee | 1:fafea1d00d0c | 270 | |
Thijsjeee | 1:fafea1d00d0c | 271 | |
Thijsjeee | 1:fafea1d00d0c | 272 | |
Thijsjeee | 1:fafea1d00d0c | 273 | void position_controll_void() |
Thijsjeee | 0:0af507ea0d83 | 274 | { |
Thijsjeee | 1:fafea1d00d0c | 275 | bas = true; |
Thijsjeee | 0:0af507ea0d83 | 276 | } |
Thijsjeee | 0:0af507ea0d83 | 277 | |
Thijsjeee | 1:fafea1d00d0c | 278 | |
Thijsjeee | 0:0af507ea0d83 | 279 | void motor_controler() |
Thijsjeee | 0:0af507ea0d83 | 280 | { |
Thijsjeee | 1:fafea1d00d0c | 281 | bb = -(Enc1.getPulses()) - 201; |
Thijsjeee | 1:fafea1d00d0c | 282 | bc = Enc2.getPulses() + 2100; |
Thijsjeee | 0:0af507ea0d83 | 283 | |
Thijsjeee | 1:fafea1d00d0c | 284 | if (bb >= counts_a) { |
Thijsjeee | 1:fafea1d00d0c | 285 | z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts); |
Thijsjeee | 0:0af507ea0d83 | 286 | PMW1.write(abs(z)); |
Thijsjeee | 0:0af507ea0d83 | 287 | M1 = 1; |
Thijsjeee | 0:0af507ea0d83 | 288 | } |
Thijsjeee | 1:fafea1d00d0c | 289 | if (bb <= counts_a) { |
Thijsjeee | 1:fafea1d00d0c | 290 | z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts); |
Thijsjeee | 1:fafea1d00d0c | 291 | PMW1.write(abs(z)); |
Thijsjeee | 1:fafea1d00d0c | 292 | M1 = 0; |
Thijsjeee | 1:fafea1d00d0c | 293 | } |
Thijsjeee | 1:fafea1d00d0c | 294 | if (bc >= counts_b) { |
Thijsjeee | 0:0af507ea0d83 | 295 | M2 = 0; |
Thijsjeee | 1:fafea1d00d0c | 296 | z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts); |
Thijsjeee | 0:0af507ea0d83 | 297 | PMW2.write(abs(z)); |
Thijsjeee | 1:fafea1d00d0c | 298 | } |
Thijsjeee | 1:fafea1d00d0c | 299 | if (bc <= counts_b) { |
Thijsjeee | 0:0af507ea0d83 | 300 | M2 = 1; |
Thijsjeee | 1:fafea1d00d0c | 301 | z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts); |
Thijsjeee | 0:0af507ea0d83 | 302 | PMW2.write(abs(z)); |
Thijsjeee | 1:fafea1d00d0c | 303 | } |
Thijsjeee | 0:0af507ea0d83 | 304 | } |
Thijsjeee | 0:0af507ea0d83 | 305 | |
Thijsjeee | 2:f68fd7b1c655 | 306 | |
Thijsjeee | 2:f68fd7b1c655 | 307 | void change_wait() |
Thijsjeee | 2:f68fd7b1c655 | 308 | { |
Thijsjeee | 2:f68fd7b1c655 | 309 | waiting = waiting++; |
Thijsjeee | 2:f68fd7b1c655 | 310 | } |
Thijsjeee | 2:f68fd7b1c655 | 311 | |
Thijsjeee | 2:f68fd7b1c655 | 312 | |
Thijsjeee | 2:f68fd7b1c655 | 313 | void initializeren() |
Thijsjeee | 2:f68fd7b1c655 | 314 | { |
Thijsjeee | 2:f68fd7b1c655 | 315 | waiting = 1; |
Thijsjeee | 2:f68fd7b1c655 | 316 | while(waiting <=3) |
Thijsjeee | 2:f68fd7b1c655 | 317 | if (bas == true) |
Thijsjeee | 2:f68fd7b1c655 | 318 | { |
Thijsjeee | 2:f68fd7b1c655 | 319 | if (waiting == 1) { |
Thijsjeee | 2:f68fd7b1c655 | 320 | Cxx = -20; |
Thijsjeee | 2:f68fd7b1c655 | 321 | Cyy = 10; |
Thijsjeee | 2:f68fd7b1c655 | 322 | |
Thijsjeee | 2:f68fd7b1c655 | 323 | position_define(); |
Thijsjeee | 2:f68fd7b1c655 | 324 | angle_define(); |
Thijsjeee | 2:f68fd7b1c655 | 325 | motor_controler(); |
Thijsjeee | 2:f68fd7b1c655 | 326 | |
Thijsjeee | 2:f68fd7b1c655 | 327 | } |
Thijsjeee | 2:f68fd7b1c655 | 328 | |
Thijsjeee | 2:f68fd7b1c655 | 329 | if(waiting == 2) { |
Thijsjeee | 2:f68fd7b1c655 | 330 | Cxx = -40; |
Thijsjeee | 2:f68fd7b1c655 | 331 | Cyy = 10; |
Thijsjeee | 2:f68fd7b1c655 | 332 | position_define(); |
Thijsjeee | 2:f68fd7b1c655 | 333 | angle_define(); |
Thijsjeee | 2:f68fd7b1c655 | 334 | motor_controler(); |
Thijsjeee | 2:f68fd7b1c655 | 335 | button.fall(&change_wait); |
Thijsjeee | 2:f68fd7b1c655 | 336 | } |
Thijsjeee | 2:f68fd7b1c655 | 337 | } |
Thijsjeee | 2:f68fd7b1c655 | 338 | } |
Thijsjeee | 2:f68fd7b1c655 | 339 | |
Thijsjeee | 2:f68fd7b1c655 | 340 | |
Thijsjeee | 0:0af507ea0d83 | 341 | int main() |
Thijsjeee | 0:0af507ea0d83 | 342 | { |
Thijsjeee | 0:0af507ea0d83 | 343 | PMW1.period_us(60); |
Thijsjeee | 2:f68fd7b1c655 | 344 | button.fall(&change_wait); |
Thijsjeee | 2:f68fd7b1c655 | 345 | position_controll.attach(position_controll_void,0.002); |
Thijsjeee | 0:0af507ea0d83 | 346 | X[0][0] = X0[0][0]; |
Thijsjeee | 0:0af507ea0d83 | 347 | X[1][0] = X0[1][0]; |
Thijsjeee | 0:0af507ea0d83 | 348 | Xold[0][0] = X0[0][0]; |
Thijsjeee | 0:0af507ea0d83 | 349 | Xold[1][0] = X0[1][0]; |
Thijsjeee | 2:f68fd7b1c655 | 350 | //pc.baud(115200); |
Thijsjeee | 2:f68fd7b1c655 | 351 | initializeren(); |
Thijsjeee | 1:fafea1d00d0c | 352 | x1 = Cxx; |
Thijsjeee | 1:fafea1d00d0c | 353 | y1= Cyy; |
Thijsjeee | 1:fafea1d00d0c | 354 | bqc.add( &bq1 ).add( &bq2 ); |
Thijsjeee | 1:fafea1d00d0c | 355 | bqc2.add( &bq3 ); |
Thijsjeee | 1:fafea1d00d0c | 356 | bqc3.add( &bq4 ).add( &bq5 ); |
Thijsjeee | 1:fafea1d00d0c | 357 | bqc4.add( &bq6 ); |
Thijsjeee | 2:f68fd7b1c655 | 358 | |
Thijsjeee | 1:fafea1d00d0c | 359 | while(true) { |
Thijsjeee | 0:0af507ea0d83 | 360 | |
Thijsjeee | 1:fafea1d00d0c | 361 | if(bas == true) { |
Thijsjeee | 1:fafea1d00d0c | 362 | Led = 1; |
Thijsjeee | 1:fafea1d00d0c | 363 | filteren(); |
Thijsjeee | 1:fafea1d00d0c | 364 | position_define(); |
Thijsjeee | 1:fafea1d00d0c | 365 | angle_define(); |
Thijsjeee | 1:fafea1d00d0c | 366 | motor_controler(); |
Thijsjeee | 2:f68fd7b1c655 | 367 | |
Thijsjeee | 1:fafea1d00d0c | 368 | sw2.fall(change); |
Thijsjeee | 1:fafea1d00d0c | 369 | Position1x(emgFiltered3); |
Thijsjeee | 1:fafea1d00d0c | 370 | Position1y(emgFiltered23); |
Thijsjeee | 1:fafea1d00d0c | 371 | Led = 0; |
Thijsjeee | 1:fafea1d00d0c | 372 | bas= false; |
Thijsjeee | 1:fafea1d00d0c | 373 | } |
Thijsjeee | 0:0af507ea0d83 | 374 | } |
Thijsjeee | 1:fafea1d00d0c | 375 | |
Thijsjeee | 0:0af507ea0d83 | 376 | } |