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

Committer:
carlmaykel
Date:
Thu Nov 01 20:50:14 2018 +0000
Revision:
11:b1ad257e5647
Parent:
10:86c810be889a
Child:
12:771f3c73fe24
added some switch cases for letters.

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