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@9:4fc2659cfb26, 2018-11-01 (annotated)

Committer:
carlmaykel
Date:
Thu Nov 01 18:57:26 2018 +0000
Revision:
9:4fc2659cfb26
Parent:
8:364ea64ae86b
Child:
10:86c810be889a
Cleaned up the head of the file

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
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);
carlmaykel 9:4fc2659cfb26 18 DigitalOut Led(LED1);
carlmaykel 9:4fc2659cfb26 19 DigitalOut Led2(LED2);
carlmaykel 9:4fc2659cfb26 20 PwmOut PMW1(D5); // Motor 1
carlmaykel 9:4fc2659cfb26 21 DigitalOut M1(D4); // direction of motor 1
carlmaykel 9:4fc2659cfb26 22 PwmOut PMW2(D6); // Motor 2
carlmaykel 9:4fc2659cfb26 23 DigitalOut M2(D7); // direction of motor 2
Thijsjeee 1:fafea1d00d0c 24
Thijsjeee 5:d3031d082c22 25 // hidscope
carlmaykel 9:4fc2659cfb26 26 HIDScope scope( 4 ); // 4 to check the different
carlmaykel 9:4fc2659cfb26 27
carlmaykel 9:4fc2659cfb26 28 // tickers
carlmaykel 9:4fc2659cfb26 29 Ticker sample_timer;
carlmaykel 9:4fc2659cfb26 30 Ticker position_controll;
carlmaykel 9:4fc2659cfb26 31
carlmaykel 9:4fc2659cfb26 32 //Define Variables
carlmaykel 9:4fc2659cfb26 33 double pi = 3.14159265359;
carlmaykel 9:4fc2659cfb26 34 int bb;
carlmaykel 9:4fc2659cfb26 35 int bc;
carlmaykel 9:4fc2659cfb26 36 float z;
carlmaykel 9:4fc2659cfb26 37 int i;
carlmaykel 9:4fc2659cfb26 38 double angle_a = 0; //in rad
carlmaykel 9:4fc2659cfb26 39 double angle_b = 0.5 * pi; //in rad
carlmaykel 9:4fc2659cfb26 40 double X0[2][1] = {{angle_a},{angle_b}};
carlmaykel 9:4fc2659cfb26 41 double X[2][1];
carlmaykel 9:4fc2659cfb26 42 double Xold[2][1];
carlmaykel 9:4fc2659cfb26 43 double fval[2][1];
carlmaykel 9:4fc2659cfb26 44 double J[2][2];
carlmaykel 9:4fc2659cfb26 45 double err[2][1];
carlmaykel 9:4fc2659cfb26 46 double MaxIter = 20;
carlmaykel 9:4fc2659cfb26 47 double tolX = 1e-4;
carlmaykel 9:4fc2659cfb26 48 double A = 20;
carlmaykel 9:4fc2659cfb26 49 double B = 30;
carlmaykel 9:4fc2659cfb26 50 double C = 20;
carlmaykel 9:4fc2659cfb26 51 double D = 27;
carlmaykel 9:4fc2659cfb26 52 double E = 35;
carlmaykel 9:4fc2659cfb26 53 double ex = -40; // current position
carlmaykel 9:4fc2659cfb26 54 double ey = -30; // current position
carlmaykel 9:4fc2659cfb26 55 double Cxx = -35; // Goal position
carlmaykel 9:4fc2659cfb26 56 double Cyy = 27; // Goal position
carlmaykel 9:4fc2659cfb26 57 double Kp = 1;
carlmaykel 9:4fc2659cfb26 58 double Ki = 1;
carlmaykel 9:4fc2659cfb26 59 double Kd = 0.3;
carlmaykel 9:4fc2659cfb26 60 double Ts = 0.001;
Thijsjeee 7:fcb20c3ccee9 61 bool startCalc;
Thijsjeee 7:fcb20c3ccee9 62 bool calpos1;
Thijsjeee 7:fcb20c3ccee9 63 bool calpos2;
Thijsjeee 1:fafea1d00d0c 64 bool bas;
Thijsjeee 2:f68fd7b1c655 65 int waiting;
Thijsjeee 7:fcb20c3ccee9 66 int count1;
Thijsjeee 7:fcb20c3ccee9 67 int count2;
Thijsjeee 7:fcb20c3ccee9 68 int count3;
Thijsjeee 7:fcb20c3ccee9 69 int count4;
carlmaykel 9:4fc2659cfb26 70 float counts_a;
carlmaykel 9:4fc2659cfb26 71 float counts_b;
carlmaykel 9:4fc2659cfb26 72 int counts = 8400;
Thijsjeee 8:364ea64ae86b 73 volatile float x1;
Thijsjeee 8:364ea64ae86b 74 volatile float y1;
Thijsjeee 1:fafea1d00d0c 75 double emgFiltered3;
Thijsjeee 1:fafea1d00d0c 76 double emgFiltered23;
Thijsjeee 1:fafea1d00d0c 77 bool dir = true;
carlmaykel 9:4fc2659cfb26 78
Thijsjeee 1:fafea1d00d0c 79 // filtering
Thijsjeee 1:fafea1d00d0c 80 //filter coeffiecents
Thijsjeee 1:fafea1d00d0c 81 // highpass
carlmaykel 9:4fc2659cfb26 82 double b01h = 0.956543225556877;
Thijsjeee 6:e492bc8fc3fb 83 double b02h = -1.91308645111375;
Thijsjeee 6:e492bc8fc3fb 84 double b03h = 0.956543225556877;
Thijsjeee 6:e492bc8fc3fb 85 double a01h = -1.91119706742607;
carlmaykel 9:4fc2659cfb26 86 double a02h = 0.914975834801434;
Thijsjeee 1:fafea1d00d0c 87 // notchfilter
carlmaykel 9:4fc2659cfb26 88 double b01n = 0.991103635646810;
carlmaykel 9:4fc2659cfb26 89 double b02n = -1.60363936885013;
carlmaykel 9:4fc2659cfb26 90 double b03n = 0.991103635646810;
carlmaykel 9:4fc2659cfb26 91 double a01n = -1.60363936885013;
Thijsjeee 6:e492bc8fc3fb 92 double a02n = 0.982207271293620;
Thijsjeee 1:fafea1d00d0c 93 //lowpass 1
Thijsjeee 6:e492bc8fc3fb 94 double b01l = 0.000346041337639103;
Thijsjeee 6:e492bc8fc3fb 95 double b02l = 0.000692082675278205;
Thijsjeee 6:e492bc8fc3fb 96 double b03l = 0.000346041337639103;
carlmaykel 9:4fc2659cfb26 97 double a01l = -1.94669754075618;
Thijsjeee 6:e492bc8fc3fb 98 double a02l = 0.948081706106740;
Thijsjeee 1:fafea1d00d0c 99 BiQuadChain bqc;
Thijsjeee 1:fafea1d00d0c 100 BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
Thijsjeee 1:fafea1d00d0c 101 BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
Thijsjeee 1:fafea1d00d0c 102 // than we need to rectifie
Thijsjeee 1:fafea1d00d0c 103 // and lowpass afterwards
Thijsjeee 1:fafea1d00d0c 104 BiQuadChain bqc2;
Thijsjeee 1:fafea1d00d0c 105 BiQuad bq3( b01l, b02l, b03l, a01l, a02l); //lowpass
Thijsjeee 1:fafea1d00d0c 106 // optional is doing a movingaverage after
Thijsjeee 1:fafea1d00d0c 107
Thijsjeee 1:fafea1d00d0c 108 BiQuadChain bqc3;
Thijsjeee 1:fafea1d00d0c 109 BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass
Thijsjeee 1:fafea1d00d0c 110 BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch
Thijsjeee 1:fafea1d00d0c 111 // than we need to rectifie
Thijsjeee 1:fafea1d00d0c 112 // and lowpass afterwards
Thijsjeee 1:fafea1d00d0c 113 BiQuadChain bqc4;
Thijsjeee 1:fafea1d00d0c 114 BiQuad bq6( b01l, b02l, b03l, a01l, a02l); //lowpass
Thijsjeee 1:fafea1d00d0c 115
Thijsjeee 0:0af507ea0d83 116 //initializing Encoders
Thijsjeee 0:0af507ea0d83 117 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
Thijsjeee 0:0af507ea0d83 118 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
Thijsjeee 0:0af507ea0d83 119
carlmaykel 9:4fc2659cfb26 120 //Functions
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 8:364ea64ae86b 139 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 140 i = 0;
Thijsjeee 1:fafea1d00d0c 141 Cxx =x1;
Thijsjeee 1:fafea1d00d0c 142 if (dir == true) {
Thijsjeee 8:364ea64ae86b 143 if(x1 > -46.3) {
Thijsjeee 8:364ea64ae86b 144 x1 = x1-4.2;
Thijsjeee 1:fafea1d00d0c 145 //return x1;
Thijsjeee 1:fafea1d00d0c 146
Thijsjeee 8:364ea64ae86b 147 } else if ( x1 <= -46.3) {
Thijsjeee 8:364ea64ae86b 148 x1 =-17;
Thijsjeee 1:fafea1d00d0c 149 //return x1;
Thijsjeee 1:fafea1d00d0c 150 } else {
Thijsjeee 1:fafea1d00d0c 151 }
Thijsjeee 1:fafea1d00d0c 152 } else {
Thijsjeee 8:364ea64ae86b 153 if(x1 < -17) {
Thijsjeee 8:364ea64ae86b 154 x1 = x1+4.2;
Thijsjeee 1:fafea1d00d0c 155 //return x1;
Thijsjeee 1:fafea1d00d0c 156
Thijsjeee 8:364ea64ae86b 157 } else if ( x1 >= -17) {
Thijsjeee 8:364ea64ae86b 158 x1 = -46.3;
Thijsjeee 1:fafea1d00d0c 159 //return x1;
Thijsjeee 1:fafea1d00d0c 160 } else {
Thijsjeee 1:fafea1d00d0c 161 }
Thijsjeee 1:fafea1d00d0c 162 }
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 8:364ea64ae86b 168 if (b > 0.18) {
Thijsjeee 8:364ea64ae86b 169 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 170 i = 0;
Thijsjeee 1:fafea1d00d0c 171 Cyy=y1;
Thijsjeee 1:fafea1d00d0c 172 if(dir == true) {
Thijsjeee 8:364ea64ae86b 173 if(y1 < 32.4) {
Thijsjeee 8:364ea64ae86b 174 y1 = y1+4.2;
Thijsjeee 1:fafea1d00d0c 175 //return y1;
Thijsjeee 8:364ea64ae86b 176 } else if ( y1 >= 32.4) {
Thijsjeee 8:364ea64ae86b 177 y1 = 3;
Thijsjeee 1:fafea1d00d0c 178 //return y1;
Thijsjeee 1:fafea1d00d0c 179 } else {
Thijsjeee 1:fafea1d00d0c 180 }
Thijsjeee 1:fafea1d00d0c 181 } else {
Thijsjeee 8:364ea64ae86b 182 if(y1 > 3) {
Thijsjeee 8:364ea64ae86b 183 y1 = y1-4.2;
Thijsjeee 1:fafea1d00d0c 184 //return y1;
Thijsjeee 8:364ea64ae86b 185 } else if ( y1 <= 3) {
Thijsjeee 8:364ea64ae86b 186 y1 = 32.4;
Thijsjeee 1:fafea1d00d0c 187 //return y1;
Thijsjeee 1:fafea1d00d0c 188 } else {
Thijsjeee 1:fafea1d00d0c 189 }
Thijsjeee 1:fafea1d00d0c 190 }
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 8:364ea64ae86b 250 ey = ey - 0.008;
Thijsjeee 1:fafea1d00d0c 251 }
Thijsjeee 1:fafea1d00d0c 252 if (ey < Cyy) {
Thijsjeee 8:364ea64ae86b 253 ey = ey + 0.008;
Thijsjeee 1:fafea1d00d0c 254 }
Thijsjeee 0:0af507ea0d83 255 }
Thijsjeee 1:fafea1d00d0c 256 } else {
Thijsjeee 1:fafea1d00d0c 257 if (ex > Cxx) {
Thijsjeee 8:364ea64ae86b 258 ex = ex - 0.008;
Thijsjeee 1:fafea1d00d0c 259 }
Thijsjeee 1:fafea1d00d0c 260 if (ex < Cxx) {
Thijsjeee 8:364ea64ae86b 261 ex = ex + 0.008;
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 7:fcb20c3ccee9 280 bb = -(Enc1.getPulses()) - 816;
Thijsjeee 7:fcb20c3ccee9 281 bc = Enc2.getPulses() + 4316;
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 2:f68fd7b1c655 305
Thijsjeee 2:f68fd7b1c655 306 void change_wait()
Thijsjeee 2:f68fd7b1c655 307 {
Thijsjeee 2:f68fd7b1c655 308 waiting = waiting++;
Thijsjeee 2:f68fd7b1c655 309 }
Thijsjeee 2:f68fd7b1c655 310
Thijsjeee 2:f68fd7b1c655 311
Thijsjeee 2:f68fd7b1c655 312 void initializeren()
Thijsjeee 2:f68fd7b1c655 313 {
Thijsjeee 2:f68fd7b1c655 314 waiting = 1;
Thijsjeee 4:f16a18aa58bd 315 while(waiting <=2)
Thijsjeee 7:fcb20c3ccee9 316 if (bas == true) {
Thijsjeee 7:fcb20c3ccee9 317 if (waiting == 1) {
Thijsjeee 8:364ea64ae86b 318 Cxx = -17;
Thijsjeee 8:364ea64ae86b 319 Cyy = 3;
Thijsjeee 7:fcb20c3ccee9 320
Thijsjeee 7:fcb20c3ccee9 321 position_define();
Thijsjeee 7:fcb20c3ccee9 322 angle_define();
Thijsjeee 7:fcb20c3ccee9 323 motor_controler();
Thijsjeee 7:fcb20c3ccee9 324
Thijsjeee 7:fcb20c3ccee9 325 }
Thijsjeee 2:f68fd7b1c655 326
Thijsjeee 7:fcb20c3ccee9 327 if(waiting == 2) {
Thijsjeee 8:364ea64ae86b 328 Cxx = -45.8;
Thijsjeee 8:364ea64ae86b 329 Cyy = 7;
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 7:fcb20c3ccee9 335 }
Thijsjeee 7:fcb20c3ccee9 336
Thijsjeee 7:fcb20c3ccee9 337 void setCalibration()
Thijsjeee 7:fcb20c3ccee9 338 {
Thijsjeee 7:fcb20c3ccee9 339 if (startCalc == false) {
Thijsjeee 2:f68fd7b1c655 340
Thijsjeee 7:fcb20c3ccee9 341 if (calpos1 == false) {
Thijsjeee 7:fcb20c3ccee9 342 while(abs(count2-count1) > 0) {
Thijsjeee 7:fcb20c3ccee9 343 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 344 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 345 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 346 count2 = count1;
Thijsjeee 7:fcb20c3ccee9 347 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 348 }
Thijsjeee 7:fcb20c3ccee9 349 Enc1.reset();
Thijsjeee 7:fcb20c3ccee9 350 bb= Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 351 calpos1 = true;
Thijsjeee 2:f68fd7b1c655 352 }
Thijsjeee 7:fcb20c3ccee9 353 if(calpos2 == false) {
Thijsjeee 7:fcb20c3ccee9 354 while(abs(count4-count3) > 0) {
Thijsjeee 7:fcb20c3ccee9 355 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 356 M1=0;
Thijsjeee 7:fcb20c3ccee9 357 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 358 wait(0.1);
Thijsjeee 8:364ea64ae86b 359 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 360 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 361 count4 = count3;
Thijsjeee 7:fcb20c3ccee9 362 count3 = Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 363 }
Thijsjeee 7:fcb20c3ccee9 364 Enc2.reset();
Thijsjeee 7:fcb20c3ccee9 365 bc= Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 366 calpos2 = true;
Thijsjeee 7:fcb20c3ccee9 367 }
Thijsjeee 2:f68fd7b1c655 368 }
Thijsjeee 2:f68fd7b1c655 369 }
Thijsjeee 2:f68fd7b1c655 370
Thijsjeee 2:f68fd7b1c655 371
Thijsjeee 0:0af507ea0d83 372 int main()
Thijsjeee 0:0af507ea0d83 373 {
Thijsjeee 3:40427c0157a0 374 Led = 1;
Thijsjeee 8:364ea64ae86b 375 Led2 = 0;
Thijsjeee 7:fcb20c3ccee9 376 M1 = 1;
Thijsjeee 7:fcb20c3ccee9 377 M2 = 1;
Thijsjeee 7:fcb20c3ccee9 378 startCalc = false;
Thijsjeee 7:fcb20c3ccee9 379 calpos1 = false;
Thijsjeee 7:fcb20c3ccee9 380 calpos2 = false;
Thijsjeee 7:fcb20c3ccee9 381 count2 = 10000;
Thijsjeee 7:fcb20c3ccee9 382 count4 = 10000;
Thijsjeee 8:364ea64ae86b 383 Led = 1;
Thijsjeee 7:fcb20c3ccee9 384 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 385 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 386 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 387 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 388 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 389 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 390 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 391 count3 = Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 392
Thijsjeee 7:fcb20c3ccee9 393 setCalibration();
Thijsjeee 8:364ea64ae86b 394 Led2 = 1;
Thijsjeee 7:fcb20c3ccee9 395 button.fall(&change_wait);
Thijsjeee 0:0af507ea0d83 396 PMW1.period_us(60);
Thijsjeee 7:fcb20c3ccee9 397 PMW2.period_us(60);
Thijsjeee 2:f68fd7b1c655 398 position_controll.attach(position_controll_void,0.002);
Thijsjeee 0:0af507ea0d83 399 X[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 400 X[1][0] = X0[1][0];
Thijsjeee 0:0af507ea0d83 401 Xold[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 402 Xold[1][0] = X0[1][0];
Thijsjeee 2:f68fd7b1c655 403 //pc.baud(115200);
Thijsjeee 2:f68fd7b1c655 404 initializeren();
Thijsjeee 1:fafea1d00d0c 405 x1 = Cxx;
Thijsjeee 1:fafea1d00d0c 406 y1= Cyy;
Thijsjeee 1:fafea1d00d0c 407 bqc.add( &bq1 ).add( &bq2 );
Thijsjeee 1:fafea1d00d0c 408 bqc2.add( &bq3 );
Thijsjeee 1:fafea1d00d0c 409 bqc3.add( &bq4 ).add( &bq5 );
Thijsjeee 1:fafea1d00d0c 410 bqc4.add( &bq6 );
Thijsjeee 4:f16a18aa58bd 411 Cxx = -35;
Thijsjeee 4:f16a18aa58bd 412 Cyy = 27;
Thijsjeee 6:e492bc8fc3fb 413 i = 251;
Thijsjeee 1:fafea1d00d0c 414 while(true) {
Thijsjeee 0:0af507ea0d83 415
Thijsjeee 1:fafea1d00d0c 416 if(bas == true) {
Thijsjeee 1:fafea1d00d0c 417 Led = 1;
Thijsjeee 1:fafea1d00d0c 418 filteren();
Thijsjeee 1:fafea1d00d0c 419 position_define();
Thijsjeee 1:fafea1d00d0c 420 angle_define();
Thijsjeee 1:fafea1d00d0c 421 motor_controler();
Thijsjeee 7:fcb20c3ccee9 422
Thijsjeee 8:364ea64ae86b 423 scope.set(0, ex); // filtered 1
Thijsjeee 8:364ea64ae86b 424 scope.set(1, Cxx); //filtered signal 2
Thijsjeee 8:364ea64ae86b 425 scope.set(2, ey);
Thijsjeee 8:364ea64ae86b 426 scope.set(3, Cyy);
Thijsjeee 5:d3031d082c22 427 scope.send();
Thijsjeee 7:fcb20c3ccee9 428
Thijsjeee 7:fcb20c3ccee9 429 if (i <= 250) {
Thijsjeee 6:e492bc8fc3fb 430 emgFiltered3 = 0;
Thijsjeee 6:e492bc8fc3fb 431 emgFiltered23 = 0;
Thijsjeee 7:fcb20c3ccee9 432 i++;
Thijsjeee 6:e492bc8fc3fb 433 }
Thijsjeee 7:fcb20c3ccee9 434
Thijsjeee 7:fcb20c3ccee9 435
Thijsjeee 8:364ea64ae86b 436 sw2.fall(change);
Thijsjeee 6:e492bc8fc3fb 437 Position1x(emgFiltered3);
Thijsjeee 6:e492bc8fc3fb 438 Position1y(emgFiltered23);
Thijsjeee 8:364ea64ae86b 439 Cxx = x1;
Thijsjeee 8:364ea64ae86b 440 Cyy = y1;
Thijsjeee 8:364ea64ae86b 441 Led2 = 1;
Thijsjeee 1:fafea1d00d0c 442 Led = 0;
Thijsjeee 1:fafea1d00d0c 443 bas= false;
Thijsjeee 1:fafea1d00d0c 444 }
Thijsjeee 0:0af507ea0d83 445 }
Thijsjeee 1:fafea1d00d0c 446
Thijsjeee 0:0af507ea0d83 447 }