Thijs Rakels / Mbed 2 deprecated NR_method

Chess robot with NR method

Dependencies:   QEI biquadFilter mbed HIDScope

NR_method_1.cpp@1:fafea1d00d0c, 2018-11-01 (annotated)

Committer:
Thijsjeee
Date:
Thu Nov 01 09:42:53 2018 +0000
Revision:
1:fafea1d00d0c
Parent:
0:0af507ea0d83
Child:
2:f68fd7b1c655
laatste versie;

Who changed what in which revision?

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