Thijs Rakels / Mbed 2 deprecated NR_method

Chess robot with NR method

Dependencies:   QEI biquadFilter mbed HIDScope

NR_method_1.cpp@8:364ea64ae86b, 2018-11-01 (annotated)

Committer:
Thijsjeee
Date:
Thu Nov 01 18:10:54 2018 +0000
Revision:
8:364ea64ae86b
Parent:
7:fcb20c3ccee9 
nieuwste 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 6:e492bc8fc3fb 10 #include <HIDScope.h>
Thijsjeee 6:e492bc8fc3fb 11
Thijsjeee 6:e492bc8fc3fb 12
Thijsjeee 1:fafea1d00d0c 13 //#include <MODSERIAL.h>
Thijsjeee 1:fafea1d00d0c 14
Thijsjeee 5:d3031d082c22 15 // hidscope
Thijsjeee 8:364ea64ae86b 16 HIDScope scope( 4 );
Thijsjeee 7:fcb20c3ccee9 17 bool startCalc;
Thijsjeee 7:fcb20c3ccee9 18 bool calpos1;
Thijsjeee 7:fcb20c3ccee9 19 bool calpos2;
Thijsjeee 1:fafea1d00d0c 20 bool bas;
Thijsjeee 2:f68fd7b1c655 21 int waiting;
Thijsjeee 7:fcb20c3ccee9 22 int count1;
Thijsjeee 7:fcb20c3ccee9 23 int count2;
Thijsjeee 7:fcb20c3ccee9 24 int count3;
Thijsjeee 7:fcb20c3ccee9 25 int count4;
Thijsjeee 2:f68fd7b1c655 26 InterruptIn button(SW3);
Thijsjeee 1:fafea1d00d0c 27
Thijsjeee 1:fafea1d00d0c 28 Serial pc(USBTX, USBRX);
Thijsjeee 1:fafea1d00d0c 29 // emg signals input
Thijsjeee 1:fafea1d00d0c 30 AnalogIn emg1(A0);
Thijsjeee 1:fafea1d00d0c 31 AnalogIn emg2(A1);
Thijsjeee 1:fafea1d00d0c 32 InterruptIn sw2(SW2);
Thijsjeee 1:fafea1d00d0c 33 // tickers
Thijsjeee 1:fafea1d00d0c 34 Ticker sample_timer;
Thijsjeee 1:fafea1d00d0c 35
Thijsjeee 8:364ea64ae86b 36 volatile float x1;
Thijsjeee 8:364ea64ae86b 37 volatile float y1;
Thijsjeee 1:fafea1d00d0c 38 double emgFiltered3;
Thijsjeee 1:fafea1d00d0c 39 double emgFiltered23;
Thijsjeee 1:fafea1d00d0c 40 bool dir = true;
Thijsjeee 1:fafea1d00d0c 41 // filtering
Thijsjeee 1:fafea1d00d0c 42 //filter coeffiecents
Thijsjeee 1:fafea1d00d0c 43 // highpass
Thijsjeee 6:e492bc8fc3fb 44 double b01h = 0.956543225556877;
Thijsjeee 6:e492bc8fc3fb 45 double b02h = -1.91308645111375;
Thijsjeee 6:e492bc8fc3fb 46 double b03h = 0.956543225556877;
Thijsjeee 6:e492bc8fc3fb 47 double a01h = -1.91119706742607;
Thijsjeee 6:e492bc8fc3fb 48 double a02h = 0.914975834801434;
Thijsjeee 1:fafea1d00d0c 49 // notchfilter
Thijsjeee 6:e492bc8fc3fb 50 double b01n = 0.991103635646810;
Thijsjeee 6:e492bc8fc3fb 51 double b02n = -1.60363936885013;
Thijsjeee 6:e492bc8fc3fb 52 double b03n = 0.991103635646810;
Thijsjeee 6:e492bc8fc3fb 53 double a01n = -1.60363936885013;
Thijsjeee 6:e492bc8fc3fb 54 double a02n = 0.982207271293620;
Thijsjeee 1:fafea1d00d0c 55 //lowpass 1
Thijsjeee 6:e492bc8fc3fb 56 double b01l = 0.000346041337639103;
Thijsjeee 6:e492bc8fc3fb 57 double b02l = 0.000692082675278205;
Thijsjeee 6:e492bc8fc3fb 58 double b03l = 0.000346041337639103;
Thijsjeee 6:e492bc8fc3fb 59 double a01l = -1.94669754075618;
Thijsjeee 6:e492bc8fc3fb 60 double a02l = 0.948081706106740;
Thijsjeee 1:fafea1d00d0c 61 BiQuadChain bqc;
Thijsjeee 1:fafea1d00d0c 62 BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
Thijsjeee 1:fafea1d00d0c 63 BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
Thijsjeee 1:fafea1d00d0c 64 // than we need to rectifie
Thijsjeee 1:fafea1d00d0c 65 // and lowpass afterwards
Thijsjeee 1:fafea1d00d0c 66 BiQuadChain bqc2;
Thijsjeee 1:fafea1d00d0c 67 BiQuad bq3( b01l, b02l, b03l, a01l, a02l); //lowpass
Thijsjeee 1:fafea1d00d0c 68 // optional is doing a movingaverage after
Thijsjeee 1:fafea1d00d0c 69
Thijsjeee 1:fafea1d00d0c 70 BiQuadChain bqc3;
Thijsjeee 1:fafea1d00d0c 71 BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass
Thijsjeee 1:fafea1d00d0c 72 BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch
Thijsjeee 1:fafea1d00d0c 73 // than we need to rectifie
Thijsjeee 1:fafea1d00d0c 74 // and lowpass afterwards
Thijsjeee 1:fafea1d00d0c 75 BiQuadChain bqc4;
Thijsjeee 1:fafea1d00d0c 76 BiQuad bq6( b01l, b02l, b03l, a01l, a02l); //lowpass
Thijsjeee 1:fafea1d00d0c 77
Thijsjeee 0:0af507ea0d83 78
Thijsjeee 0:0af507ea0d83 79 //Define in/outputs
Thijsjeee 0:0af507ea0d83 80
Thijsjeee 1:fafea1d00d0c 81 int counts = 8400;
Thijsjeee 0:0af507ea0d83 82 DigitalOut Led(LED1);
Thijsjeee 8:364ea64ae86b 83 DigitalOut Led2(LED2);
Thijsjeee 0:0af507ea0d83 84 PwmOut PMW1(D5); // Motor 1
Thijsjeee 0:0af507ea0d83 85 DigitalOut M1(D4); // direction of motor 1
Thijsjeee 0:0af507ea0d83 86 PwmOut PMW2(D6); // Motor 2
Thijsjeee 0:0af507ea0d83 87 DigitalOut M2(D7); // direction of motor 2
Thijsjeee 0:0af507ea0d83 88
Thijsjeee 0:0af507ea0d83 89 //initializing Encoders
Thijsjeee 0:0af507ea0d83 90 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
Thijsjeee 0:0af507ea0d83 91 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
Thijsjeee 0:0af507ea0d83 92
Thijsjeee 0:0af507ea0d83 93 double Kp = 1;
Thijsjeee 2:f68fd7b1c655 94 double Ki = 1;
Thijsjeee 1:fafea1d00d0c 95 double Kd = 0.3;
Thijsjeee 0:0af507ea0d83 96 double Ts = 0.001;
Thijsjeee 0:0af507ea0d83 97
Thijsjeee 0:0af507ea0d83 98 float counts_a;
Thijsjeee 0:0af507ea0d83 99 float counts_b;
Thijsjeee 0:0af507ea0d83 100
Thijsjeee 0:0af507ea0d83 101
Thijsjeee 0:0af507ea0d83 102 //Define Variables
Thijsjeee 0:0af507ea0d83 103
Thijsjeee 0:0af507ea0d83 104 double pi = 3.14159265359;
Thijsjeee 1:fafea1d00d0c 105 int bb;
Thijsjeee 1:fafea1d00d0c 106 int bc;
Thijsjeee 0:0af507ea0d83 107 float z;
Thijsjeee 6:e492bc8fc3fb 108 int i;
Thijsjeee 0:0af507ea0d83 109
Thijsjeee 1:fafea1d00d0c 110 double angle_a = 0; //in rad
Thijsjeee 1:fafea1d00d0c 111 double angle_b = 0.5 * pi; //in rad
Thijsjeee 0:0af507ea0d83 112
Thijsjeee 0:0af507ea0d83 113 double X0[2][1] = {{angle_a},{angle_b}};
Thijsjeee 0:0af507ea0d83 114 double X[2][1];
Thijsjeee 0:0af507ea0d83 115 double Xold[2][1];
Thijsjeee 0:0af507ea0d83 116 double fval[2][1];
Thijsjeee 0:0af507ea0d83 117 double J[2][2];
Thijsjeee 0:0af507ea0d83 118 double err[2][1];
Thijsjeee 0:0af507ea0d83 119
Thijsjeee 0:0af507ea0d83 120 double MaxIter = 20;
Thijsjeee 0:0af507ea0d83 121 double tolX = 1e-4;
Thijsjeee 1:fafea1d00d0c 122 double A = 20;
Thijsjeee 1:fafea1d00d0c 123 double B = 30;
Thijsjeee 1:fafea1d00d0c 124 double C = 20;
Thijsjeee 1:fafea1d00d0c 125 double D = 27;
Thijsjeee 1:fafea1d00d0c 126 double E = 35;
Thijsjeee 7:fcb20c3ccee9 127 double ex = -40; // current position
Thijsjeee 7:fcb20c3ccee9 128 double ey = -30; // current position
Thijsjeee 1:fafea1d00d0c 129 double Cxx = -35; // Goal position
Thijsjeee 1:fafea1d00d0c 130 double Cyy = 27; // Goal position
Thijsjeee 0:0af507ea0d83 131
Thijsjeee 0:0af507ea0d83 132 Ticker position_controll;
Thijsjeee 0:0af507ea0d83 133
Thijsjeee 1:fafea1d00d0c 134 void filteren ()
Thijsjeee 1:fafea1d00d0c 135 {
Thijsjeee 1:fafea1d00d0c 136 double emgSignal1 = emg1.read();
Thijsjeee 1:fafea1d00d0c 137 double emgSignal2 = emg2.read();
Thijsjeee 1:fafea1d00d0c 138
Thijsjeee 1:fafea1d00d0c 139 double emgFiltered1 = bqc.step(emgSignal1);
Thijsjeee 1:fafea1d00d0c 140 double emgFiltered2 = fabs(emgFiltered1);
Thijsjeee 1:fafea1d00d0c 141 emgFiltered3 = bqc2.step(emgFiltered2);
Thijsjeee 1:fafea1d00d0c 142
Thijsjeee 1:fafea1d00d0c 143 double emgFiltered21 = bqc3.step(emgSignal2);
Thijsjeee 1:fafea1d00d0c 144 double emgFiltered22 = fabs(emgFiltered21);
Thijsjeee 1:fafea1d00d0c 145 emgFiltered23 = bqc4.step(emgFiltered22);
Thijsjeee 1:fafea1d00d0c 146
Thijsjeee 1:fafea1d00d0c 147 }
Thijsjeee 1:fafea1d00d0c 148
Thijsjeee 1:fafea1d00d0c 149 void Position1x(double b)
Thijsjeee 1:fafea1d00d0c 150 {
Thijsjeee 1:fafea1d00d0c 151 if (b > 0.20) {
Thijsjeee 8:364ea64ae86b 152 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 153 i = 0;
Thijsjeee 1:fafea1d00d0c 154 Cxx =x1;
Thijsjeee 1:fafea1d00d0c 155 if (dir == true) {
Thijsjeee 8:364ea64ae86b 156 if(x1 > -46.3) {
Thijsjeee 8:364ea64ae86b 157 x1 = x1-4.2;
Thijsjeee 1:fafea1d00d0c 158 //return x1;
Thijsjeee 1:fafea1d00d0c 159
Thijsjeee 8:364ea64ae86b 160 } else if ( x1 <= -46.3) {
Thijsjeee 8:364ea64ae86b 161 x1 =-17;
Thijsjeee 1:fafea1d00d0c 162 //return x1;
Thijsjeee 1:fafea1d00d0c 163 } else {
Thijsjeee 1:fafea1d00d0c 164 }
Thijsjeee 1:fafea1d00d0c 165 } else {
Thijsjeee 8:364ea64ae86b 166 if(x1 < -17) {
Thijsjeee 8:364ea64ae86b 167 x1 = x1+4.2;
Thijsjeee 1:fafea1d00d0c 168 //return x1;
Thijsjeee 1:fafea1d00d0c 169
Thijsjeee 8:364ea64ae86b 170 } else if ( x1 >= -17) {
Thijsjeee 8:364ea64ae86b 171 x1 = -46.3;
Thijsjeee 1:fafea1d00d0c 172 //return x1;
Thijsjeee 1:fafea1d00d0c 173 } else {
Thijsjeee 1:fafea1d00d0c 174 }
Thijsjeee 1:fafea1d00d0c 175 }
Thijsjeee 1:fafea1d00d0c 176 }
Thijsjeee 1:fafea1d00d0c 177 }
Thijsjeee 1:fafea1d00d0c 178
Thijsjeee 1:fafea1d00d0c 179 void Position1y(double b)
Thijsjeee 1:fafea1d00d0c 180 {
Thijsjeee 8:364ea64ae86b 181 if (b > 0.18) {
Thijsjeee 8:364ea64ae86b 182 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 183 i = 0;
Thijsjeee 1:fafea1d00d0c 184 Cyy=y1;
Thijsjeee 1:fafea1d00d0c 185 if(dir == true) {
Thijsjeee 8:364ea64ae86b 186 if(y1 < 32.4) {
Thijsjeee 8:364ea64ae86b 187 y1 = y1+4.2;
Thijsjeee 1:fafea1d00d0c 188 //return y1;
Thijsjeee 8:364ea64ae86b 189 } else if ( y1 >= 32.4) {
Thijsjeee 8:364ea64ae86b 190 y1 = 3;
Thijsjeee 1:fafea1d00d0c 191 //return y1;
Thijsjeee 1:fafea1d00d0c 192 } else {
Thijsjeee 1:fafea1d00d0c 193 }
Thijsjeee 1:fafea1d00d0c 194 } else {
Thijsjeee 8:364ea64ae86b 195 if(y1 > 3) {
Thijsjeee 8:364ea64ae86b 196 y1 = y1-4.2;
Thijsjeee 1:fafea1d00d0c 197 //return y1;
Thijsjeee 8:364ea64ae86b 198 } else if ( y1 <= 3) {
Thijsjeee 8:364ea64ae86b 199 y1 = 32.4;
Thijsjeee 1:fafea1d00d0c 200 //return y1;
Thijsjeee 1:fafea1d00d0c 201 } else {
Thijsjeee 1:fafea1d00d0c 202 }
Thijsjeee 1:fafea1d00d0c 203 }
Thijsjeee 1:fafea1d00d0c 204 }
Thijsjeee 1:fafea1d00d0c 205 }
Thijsjeee 1:fafea1d00d0c 206 void change()
Thijsjeee 1:fafea1d00d0c 207 {
Thijsjeee 1:fafea1d00d0c 208 dir = !dir;
Thijsjeee 1:fafea1d00d0c 209 }
Thijsjeee 0:0af507ea0d83 210 void NR() //Newton Rapshon Calculation
Thijsjeee 0:0af507ea0d83 211 {
Thijsjeee 0:0af507ea0d83 212 //Variables
Thijsjeee 1:fafea1d00d0c 213 double Hoa = X[0][0];
Thijsjeee 1:fafea1d00d0c 214 double Hob = X[1][0];
Thijsjeee 1:fafea1d00d0c 215
Thijsjeee 1:fafea1d00d0c 216 double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex;
Thijsjeee 1:fafea1d00d0c 217 double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey;
Thijsjeee 1:fafea1d00d0c 218
Thijsjeee 0:0af507ea0d83 219 //Define f(x)
Thijsjeee 1:fafea1d00d0c 220 fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2);
Thijsjeee 1:fafea1d00d0c 221 fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2);
Thijsjeee 0:0af507ea0d83 222 //Jacobian
Thijsjeee 1:fafea1d00d0c 223
Thijsjeee 1:fafea1d00d0c 224
Thijsjeee 1:fafea1d00d0c 225
Thijsjeee 1:fafea1d00d0c 226 J[0][0]= 0;
Thijsjeee 1:fafea1d00d0c 227 J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob));
Thijsjeee 1:fafea1d00d0c 228 J[1][0]= - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2;
Thijsjeee 1:fafea1d00d0c 229 J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2;
Thijsjeee 0:0af507ea0d83 230 }
Thijsjeee 0:0af507ea0d83 231
Thijsjeee 1:fafea1d00d0c 232 void angle_define() //define the angle needed.
Thijsjeee 1:fafea1d00d0c 233 {
Thijsjeee 1:fafea1d00d0c 234 for(int i=1 ; i <= MaxIter; i++) {
Thijsjeee 1:fafea1d00d0c 235 NR();
Thijsjeee 0:0af507ea0d83 236
Thijsjeee 1:fafea1d00d0c 237 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 238 X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]);
Thijsjeee 0:0af507ea0d83 239
Thijsjeee 0:0af507ea0d83 240 err[0][0] = abs(X[0][0] - Xold[0][0]);
Thijsjeee 0:0af507ea0d83 241 err[1][0] = abs(X[1][0] - Xold[1][0]);
Thijsjeee 0:0af507ea0d83 242
Thijsjeee 0:0af507ea0d83 243 Xold[0][0] = X[0][0];
Thijsjeee 0:0af507ea0d83 244 Xold[1][0] = X[1][0];
Thijsjeee 1:fafea1d00d0c 245
Thijsjeee 0:0af507ea0d83 246 counts_a = ((X[0][0]) / (2* pi)) * 8400;
Thijsjeee 1:fafea1d00d0c 247 counts_b = ((X[1][0]) / (2* pi)) * 8400;
Thijsjeee 1:fafea1d00d0c 248
Thijsjeee 1:fafea1d00d0c 249 if(err[0][0] <= tolX) {
Thijsjeee 1:fafea1d00d0c 250 if(err[1][0] <= tolX) {
Thijsjeee 0:0af507ea0d83 251 break;
Thijsjeee 0:0af507ea0d83 252 }
Thijsjeee 1:fafea1d00d0c 253 }
Thijsjeee 1:fafea1d00d0c 254 }
Thijsjeee 0:0af507ea0d83 255 }
Thijsjeee 0:0af507ea0d83 256
Thijsjeee 0:0af507ea0d83 257 void position_define()
Thijsjeee 0:0af507ea0d83 258 {
Thijsjeee 1:fafea1d00d0c 259 if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) {
Thijsjeee 1:fafea1d00d0c 260 if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
Thijsjeee 1:fafea1d00d0c 261 } else {
Thijsjeee 1:fafea1d00d0c 262 if (ey > Cyy) {
Thijsjeee 8:364ea64ae86b 263 ey = ey - 0.008;
Thijsjeee 1:fafea1d00d0c 264 }
Thijsjeee 1:fafea1d00d0c 265 if (ey < Cyy) {
Thijsjeee 8:364ea64ae86b 266 ey = ey + 0.008;
Thijsjeee 1:fafea1d00d0c 267 }
Thijsjeee 0:0af507ea0d83 268 }
Thijsjeee 1:fafea1d00d0c 269 } else {
Thijsjeee 1:fafea1d00d0c 270 if (ex > Cxx) {
Thijsjeee 8:364ea64ae86b 271 ex = ex - 0.008;
Thijsjeee 1:fafea1d00d0c 272 }
Thijsjeee 1:fafea1d00d0c 273 if (ex < Cxx) {
Thijsjeee 8:364ea64ae86b 274 ex = ex + 0.008;
Thijsjeee 0:0af507ea0d83 275 }
Thijsjeee 0:0af507ea0d83 276 }
Thijsjeee 0:0af507ea0d83 277 }
Thijsjeee 0:0af507ea0d83 278
Thijsjeee 1:fafea1d00d0c 279
Thijsjeee 1:fafea1d00d0c 280
Thijsjeee 1:fafea1d00d0c 281
Thijsjeee 1:fafea1d00d0c 282
Thijsjeee 1:fafea1d00d0c 283
Thijsjeee 1:fafea1d00d0c 284
Thijsjeee 1:fafea1d00d0c 285 void position_controll_void()
Thijsjeee 0:0af507ea0d83 286 {
Thijsjeee 1:fafea1d00d0c 287 bas = true;
Thijsjeee 0:0af507ea0d83 288 }
Thijsjeee 0:0af507ea0d83 289
Thijsjeee 1:fafea1d00d0c 290
Thijsjeee 0:0af507ea0d83 291 void motor_controler()
Thijsjeee 0:0af507ea0d83 292 {
Thijsjeee 7:fcb20c3ccee9 293 bb = -(Enc1.getPulses()) - 816;
Thijsjeee 7:fcb20c3ccee9 294 bc = Enc2.getPulses() + 4316;
Thijsjeee 0:0af507ea0d83 295
Thijsjeee 1:fafea1d00d0c 296 if (bb >= counts_a) {
Thijsjeee 1:fafea1d00d0c 297 z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 298 PMW1.write(abs(z));
Thijsjeee 0:0af507ea0d83 299 M1 = 1;
Thijsjeee 0:0af507ea0d83 300 }
Thijsjeee 1:fafea1d00d0c 301 if (bb <= counts_a) {
Thijsjeee 1:fafea1d00d0c 302 z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
Thijsjeee 1:fafea1d00d0c 303 PMW1.write(abs(z));
Thijsjeee 1:fafea1d00d0c 304 M1 = 0;
Thijsjeee 1:fafea1d00d0c 305 }
Thijsjeee 1:fafea1d00d0c 306 if (bc >= counts_b) {
Thijsjeee 0:0af507ea0d83 307 M2 = 0;
Thijsjeee 1:fafea1d00d0c 308 z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 309 PMW2.write(abs(z));
Thijsjeee 1:fafea1d00d0c 310 }
Thijsjeee 1:fafea1d00d0c 311 if (bc <= counts_b) {
Thijsjeee 0:0af507ea0d83 312 M2 = 1;
Thijsjeee 1:fafea1d00d0c 313 z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 314 PMW2.write(abs(z));
Thijsjeee 1:fafea1d00d0c 315 }
Thijsjeee 0:0af507ea0d83 316 }
Thijsjeee 0:0af507ea0d83 317
Thijsjeee 2:f68fd7b1c655 318
Thijsjeee 2:f68fd7b1c655 319 void change_wait()
Thijsjeee 2:f68fd7b1c655 320 {
Thijsjeee 2:f68fd7b1c655 321 waiting = waiting++;
Thijsjeee 2:f68fd7b1c655 322 }
Thijsjeee 2:f68fd7b1c655 323
Thijsjeee 2:f68fd7b1c655 324
Thijsjeee 2:f68fd7b1c655 325 void initializeren()
Thijsjeee 2:f68fd7b1c655 326 {
Thijsjeee 2:f68fd7b1c655 327 waiting = 1;
Thijsjeee 4:f16a18aa58bd 328 while(waiting <=2)
Thijsjeee 7:fcb20c3ccee9 329 if (bas == true) {
Thijsjeee 7:fcb20c3ccee9 330 if (waiting == 1) {
Thijsjeee 8:364ea64ae86b 331 Cxx = -17;
Thijsjeee 8:364ea64ae86b 332 Cyy = 3;
Thijsjeee 7:fcb20c3ccee9 333
Thijsjeee 7:fcb20c3ccee9 334 position_define();
Thijsjeee 7:fcb20c3ccee9 335 angle_define();
Thijsjeee 7:fcb20c3ccee9 336 motor_controler();
Thijsjeee 7:fcb20c3ccee9 337
Thijsjeee 7:fcb20c3ccee9 338 }
Thijsjeee 2:f68fd7b1c655 339
Thijsjeee 7:fcb20c3ccee9 340 if(waiting == 2) {
Thijsjeee 8:364ea64ae86b 341 Cxx = -45.8;
Thijsjeee 8:364ea64ae86b 342 Cyy = 7;
Thijsjeee 7:fcb20c3ccee9 343 position_define();
Thijsjeee 7:fcb20c3ccee9 344 angle_define();
Thijsjeee 7:fcb20c3ccee9 345 motor_controler();
Thijsjeee 7:fcb20c3ccee9 346 }
Thijsjeee 7:fcb20c3ccee9 347 }
Thijsjeee 7:fcb20c3ccee9 348 }
Thijsjeee 7:fcb20c3ccee9 349
Thijsjeee 7:fcb20c3ccee9 350 void setCalibration()
Thijsjeee 7:fcb20c3ccee9 351 {
Thijsjeee 7:fcb20c3ccee9 352 if (startCalc == false) {
Thijsjeee 2:f68fd7b1c655 353
Thijsjeee 7:fcb20c3ccee9 354 if (calpos1 == false) {
Thijsjeee 7:fcb20c3ccee9 355 while(abs(count2-count1) > 0) {
Thijsjeee 7:fcb20c3ccee9 356 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 357 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 358 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 359 count2 = count1;
Thijsjeee 7:fcb20c3ccee9 360 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 361 }
Thijsjeee 7:fcb20c3ccee9 362 Enc1.reset();
Thijsjeee 7:fcb20c3ccee9 363 bb= Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 364 calpos1 = true;
Thijsjeee 2:f68fd7b1c655 365 }
Thijsjeee 7:fcb20c3ccee9 366 if(calpos2 == false) {
Thijsjeee 7:fcb20c3ccee9 367 while(abs(count4-count3) > 0) {
Thijsjeee 7:fcb20c3ccee9 368 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 369 M1=0;
Thijsjeee 7:fcb20c3ccee9 370 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 371 wait(0.1);
Thijsjeee 8:364ea64ae86b 372 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 373 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 374 count4 = count3;
Thijsjeee 7:fcb20c3ccee9 375 count3 = Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 376 }
Thijsjeee 7:fcb20c3ccee9 377 Enc2.reset();
Thijsjeee 7:fcb20c3ccee9 378 bc= Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 379 calpos2 = true;
Thijsjeee 7:fcb20c3ccee9 380 }
Thijsjeee 2:f68fd7b1c655 381 }
Thijsjeee 2:f68fd7b1c655 382 }
Thijsjeee 2:f68fd7b1c655 383
Thijsjeee 2:f68fd7b1c655 384
Thijsjeee 0:0af507ea0d83 385 int main()
Thijsjeee 0:0af507ea0d83 386 {
Thijsjeee 3:40427c0157a0 387 Led = 1;
Thijsjeee 8:364ea64ae86b 388 Led2 = 0;
Thijsjeee 7:fcb20c3ccee9 389 M1 = 1;
Thijsjeee 7:fcb20c3ccee9 390 M2 = 1;
Thijsjeee 7:fcb20c3ccee9 391 startCalc = false;
Thijsjeee 7:fcb20c3ccee9 392 calpos1 = false;
Thijsjeee 7:fcb20c3ccee9 393 calpos2 = false;
Thijsjeee 7:fcb20c3ccee9 394 count2 = 10000;
Thijsjeee 7:fcb20c3ccee9 395 count4 = 10000;
Thijsjeee 8:364ea64ae86b 396 Led = 1;
Thijsjeee 7:fcb20c3ccee9 397 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 398 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 399 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 400 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 401 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 402 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 403 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 404 count3 = Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 405
Thijsjeee 7:fcb20c3ccee9 406 setCalibration();
Thijsjeee 8:364ea64ae86b 407 Led2 = 1;
Thijsjeee 7:fcb20c3ccee9 408 button.fall(&change_wait);
Thijsjeee 0:0af507ea0d83 409 PMW1.period_us(60);
Thijsjeee 7:fcb20c3ccee9 410 PMW2.period_us(60);
Thijsjeee 2:f68fd7b1c655 411 position_controll.attach(position_controll_void,0.002);
Thijsjeee 0:0af507ea0d83 412 X[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 413 X[1][0] = X0[1][0];
Thijsjeee 0:0af507ea0d83 414 Xold[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 415 Xold[1][0] = X0[1][0];
Thijsjeee 2:f68fd7b1c655 416 //pc.baud(115200);
Thijsjeee 2:f68fd7b1c655 417 initializeren();
Thijsjeee 1:fafea1d00d0c 418 x1 = Cxx;
Thijsjeee 1:fafea1d00d0c 419 y1= Cyy;
Thijsjeee 1:fafea1d00d0c 420 bqc.add( &bq1 ).add( &bq2 );
Thijsjeee 1:fafea1d00d0c 421 bqc2.add( &bq3 );
Thijsjeee 1:fafea1d00d0c 422 bqc3.add( &bq4 ).add( &bq5 );
Thijsjeee 1:fafea1d00d0c 423 bqc4.add( &bq6 );
Thijsjeee 4:f16a18aa58bd 424 Cxx = -35;
Thijsjeee 4:f16a18aa58bd 425 Cyy = 27;
Thijsjeee 6:e492bc8fc3fb 426 i = 251;
Thijsjeee 1:fafea1d00d0c 427 while(true) {
Thijsjeee 0:0af507ea0d83 428
Thijsjeee 1:fafea1d00d0c 429 if(bas == true) {
Thijsjeee 1:fafea1d00d0c 430 Led = 1;
Thijsjeee 1:fafea1d00d0c 431 filteren();
Thijsjeee 1:fafea1d00d0c 432 position_define();
Thijsjeee 1:fafea1d00d0c 433 angle_define();
Thijsjeee 1:fafea1d00d0c 434 motor_controler();
Thijsjeee 7:fcb20c3ccee9 435
Thijsjeee 8:364ea64ae86b 436 scope.set(0, ex); // filtered 1
Thijsjeee 8:364ea64ae86b 437 scope.set(1, Cxx); //filtered signal 2
Thijsjeee 8:364ea64ae86b 438 scope.set(2, ey);
Thijsjeee 8:364ea64ae86b 439 scope.set(3, Cyy);
Thijsjeee 5:d3031d082c22 440 scope.send();
Thijsjeee 7:fcb20c3ccee9 441
Thijsjeee 7:fcb20c3ccee9 442 if (i <= 250) {
Thijsjeee 6:e492bc8fc3fb 443 emgFiltered3 = 0;
Thijsjeee 6:e492bc8fc3fb 444 emgFiltered23 = 0;
Thijsjeee 7:fcb20c3ccee9 445 i++;
Thijsjeee 6:e492bc8fc3fb 446 }
Thijsjeee 7:fcb20c3ccee9 447
Thijsjeee 7:fcb20c3ccee9 448
Thijsjeee 8:364ea64ae86b 449 sw2.fall(change);
Thijsjeee 6:e492bc8fc3fb 450 Position1x(emgFiltered3);
Thijsjeee 6:e492bc8fc3fb 451 Position1y(emgFiltered23);
Thijsjeee 8:364ea64ae86b 452 Cxx = x1;
Thijsjeee 8:364ea64ae86b 453 Cyy = y1;
Thijsjeee 8:364ea64ae86b 454 Led2 = 1;
Thijsjeee 1:fafea1d00d0c 455 Led = 0;
Thijsjeee 1:fafea1d00d0c 456 bas= false;
Thijsjeee 1:fafea1d00d0c 457 }
Thijsjeee 0:0af507ea0d83 458 }
Thijsjeee 1:fafea1d00d0c 459
Thijsjeee 0:0af507ea0d83 460 }