Carlmaykel Orman / Mbed 2 deprecated NR_method_1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of NR_method by Thijs Rakels

NR_method_1.cpp@16:fb52c8d7f242, 2018-11-02 (annotated)

Committer:
fabuled
Date:
Fri Nov 02 09:47:03 2018 +0000
Revision:
16:fb52c8d7f242
Parent:
15:56775c151fee 
Finished code with hidscope commented out;

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