Thijs Rakels / Mbed 2 deprecated NR_method

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:   QEI biquadFilter mbed HIDScope

NR_method_1.cpp@6:e492bc8fc3fb, 2018-11-01 (annotated)

Committer:
Thijsjeee
Date:
Thu Nov 01 12:59:27 2018 +0000
Revision:
6:e492bc8fc3fb
Parent:
5:d3031d082c22
Child:
7:fcb20c3ccee9
met nieuwe filters 500 Hz;

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