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@13:5270b5921d03, 2018-11-02 (annotated)

Committer:
carlmaykel
Date:
Fri Nov 02 08:13:22 2018 +0000
Revision:
13:5270b5921d03
Parent:
12:771f3c73fe24 
added new margins;

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 }
carlmaykel 12:771f3c73fe24 136 void printboardpos()
carlmaykel 12:771f3c73fe24 137 {
carlmaykel 12:771f3c73fe24 138 float x = (abs(x1+17)/4.2);
carlmaykel 12:771f3c73fe24 139 float y = (abs(y1 -3)/4.2);
Thijsjeee 1:fafea1d00d0c 140
carlmaykel 12:771f3c73fe24 141 switch ((int)y) {
carlmaykel 12:771f3c73fe24 142 case 0:
carlmaykel 12:771f3c73fe24 143 lett = 'A';
carlmaykel 12:771f3c73fe24 144 break;
carlmaykel 12:771f3c73fe24 145 case 1:
carlmaykel 12:771f3c73fe24 146 lett = 'B';
carlmaykel 12:771f3c73fe24 147 break;
carlmaykel 12:771f3c73fe24 148 case 2:
carlmaykel 12:771f3c73fe24 149 lett = 'C';
carlmaykel 12:771f3c73fe24 150 break;
carlmaykel 12:771f3c73fe24 151 case 3:
carlmaykel 12:771f3c73fe24 152 lett = 'D';
carlmaykel 12:771f3c73fe24 153 break;
carlmaykel 12:771f3c73fe24 154 case 4:
carlmaykel 12:771f3c73fe24 155 lett = 'E';
carlmaykel 12:771f3c73fe24 156 break;
carlmaykel 12:771f3c73fe24 157 case 5:
carlmaykel 12:771f3c73fe24 158 lett = 'F';
carlmaykel 12:771f3c73fe24 159 break;
carlmaykel 12:771f3c73fe24 160 case 6:
carlmaykel 12:771f3c73fe24 161 lett = 'G';
carlmaykel 12:771f3c73fe24 162 break;
carlmaykel 12:771f3c73fe24 163 case 7:
carlmaykel 12:771f3c73fe24 164 lett = 'H';
carlmaykel 12:771f3c73fe24 165 break;
carlmaykel 12:771f3c73fe24 166 default:
carlmaykel 12:771f3c73fe24 167 break;
carlmaykel 12:771f3c73fe24 168 }
carlmaykel 12:771f3c73fe24 169 switch ((int)x) {
carlmaykel 12:771f3c73fe24 170 case 0:
carlmaykel 12:771f3c73fe24 171 num =8;
carlmaykel 12:771f3c73fe24 172 break;
carlmaykel 12:771f3c73fe24 173 case 1:
carlmaykel 12:771f3c73fe24 174 num =7;
carlmaykel 12:771f3c73fe24 175 break;
carlmaykel 12:771f3c73fe24 176 case 2:
carlmaykel 12:771f3c73fe24 177 num =6;
carlmaykel 12:771f3c73fe24 178 break;
carlmaykel 12:771f3c73fe24 179 case 3:
carlmaykel 12:771f3c73fe24 180 num =5;
carlmaykel 12:771f3c73fe24 181 break;
carlmaykel 12:771f3c73fe24 182 case 4:
carlmaykel 12:771f3c73fe24 183 num =4;
carlmaykel 12:771f3c73fe24 184 break;
carlmaykel 12:771f3c73fe24 185 case 5:
carlmaykel 12:771f3c73fe24 186 num =3;
carlmaykel 12:771f3c73fe24 187 break;
carlmaykel 12:771f3c73fe24 188 case 6:
carlmaykel 12:771f3c73fe24 189 num =2;
carlmaykel 12:771f3c73fe24 190 break;
carlmaykel 12:771f3c73fe24 191 case 7:
carlmaykel 12:771f3c73fe24 192 num =1;
carlmaykel 12:771f3c73fe24 193 break;
carlmaykel 12:771f3c73fe24 194 default:
carlmaykel 12:771f3c73fe24 195 break;
carlmaykel 12:771f3c73fe24 196 }
carlmaykel 12:771f3c73fe24 197 pc.printf("%c %i\r\n",lett,num);
carlmaykel 12:771f3c73fe24 198
carlmaykel 12:771f3c73fe24 199 }
Thijsjeee 1:fafea1d00d0c 200 void Position1x(double b)
Thijsjeee 1:fafea1d00d0c 201 {
Thijsjeee 1:fafea1d00d0c 202 if (b > 0.20) {
Thijsjeee 8:364ea64ae86b 203 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 204 i = 0;
Thijsjeee 1:fafea1d00d0c 205 Cxx =x1;
carlmaykel 12:771f3c73fe24 206
Thijsjeee 1:fafea1d00d0c 207 if (dir == true) {
carlmaykel 13:5270b5921d03 208 if(x1 > -46.9) {
Thijsjeee 8:364ea64ae86b 209 x1 = x1-4.2;
carlmaykel 12:771f3c73fe24 210 printboardpos();
Thijsjeee 1:fafea1d00d0c 211
carlmaykel 13:5270b5921d03 212 } else if ( x1 < -46.9) {
Thijsjeee 8:364ea64ae86b 213 x1 =-17;
carlmaykel 12:771f3c73fe24 214 printboardpos();
Thijsjeee 1:fafea1d00d0c 215 } else {
Thijsjeee 1:fafea1d00d0c 216 }
Thijsjeee 1:fafea1d00d0c 217 } else {
carlmaykel 13:5270b5921d03 218 if(x1 < -16.5) {
Thijsjeee 8:364ea64ae86b 219 x1 = x1+4.2;
carlmaykel 12:771f3c73fe24 220 printboardpos();
carlmaykel 13:5270b5921d03 221 } else if ( x1 > -16.5) {
carlmaykel 13:5270b5921d03 222 x1 = -46.4;
carlmaykel 12:771f3c73fe24 223 printboardpos();
Thijsjeee 1:fafea1d00d0c 224 } else {
Thijsjeee 1:fafea1d00d0c 225 }
Thijsjeee 1:fafea1d00d0c 226 }
carlmaykel 11:b1ad257e5647 227
carlmaykel 11:b1ad257e5647 228
Thijsjeee 1:fafea1d00d0c 229 }
Thijsjeee 1:fafea1d00d0c 230 }
Thijsjeee 1:fafea1d00d0c 231
Thijsjeee 1:fafea1d00d0c 232 void Position1y(double b)
Thijsjeee 1:fafea1d00d0c 233 {
Thijsjeee 8:364ea64ae86b 234 if (b > 0.18) {
Thijsjeee 8:364ea64ae86b 235 Led2 = 0;
Thijsjeee 6:e492bc8fc3fb 236 i = 0;
Thijsjeee 1:fafea1d00d0c 237 Cyy=y1;
carlmaykel 12:771f3c73fe24 238 //pc.printf("Moving in the y direction\r\n");
Thijsjeee 1:fafea1d00d0c 239 if(dir == true) {
carlmaykel 13:5270b5921d03 240 if(y1 < 32.9) {
Thijsjeee 8:364ea64ae86b 241 y1 = y1+4.2;
Thijsjeee 1:fafea1d00d0c 242 //return y1;
carlmaykel 12:771f3c73fe24 243 printboardpos();
carlmaykel 13:5270b5921d03 244 } else if ( y1 > 32.9) {
Thijsjeee 8:364ea64ae86b 245 y1 = 3;
carlmaykel 12:771f3c73fe24 246 printboardpos();
Thijsjeee 1:fafea1d00d0c 247 //return y1;
Thijsjeee 1:fafea1d00d0c 248 } else {
Thijsjeee 1:fafea1d00d0c 249 }
Thijsjeee 1:fafea1d00d0c 250 } else {
carlmaykel 13:5270b5921d03 251 if(y1 > 2.5) {
Thijsjeee 8:364ea64ae86b 252 y1 = y1-4.2;
carlmaykel 12:771f3c73fe24 253 printboardpos();
Thijsjeee 1:fafea1d00d0c 254 //return y1;
carlmaykel 13:5270b5921d03 255 } else if ( y1 < 2.5) {
Thijsjeee 8:364ea64ae86b 256 y1 = 32.4;
carlmaykel 12:771f3c73fe24 257 printboardpos();
Thijsjeee 1:fafea1d00d0c 258 //return y1;
Thijsjeee 1:fafea1d00d0c 259 } else {
Thijsjeee 1:fafea1d00d0c 260 }
Thijsjeee 1:fafea1d00d0c 261 }
Thijsjeee 1:fafea1d00d0c 262 }
Thijsjeee 1:fafea1d00d0c 263 }
Thijsjeee 1:fafea1d00d0c 264 void change()
Thijsjeee 1:fafea1d00d0c 265 {
Thijsjeee 1:fafea1d00d0c 266 dir = !dir;
Thijsjeee 1:fafea1d00d0c 267 }
Thijsjeee 0:0af507ea0d83 268 void NR() //Newton Rapshon Calculation
Thijsjeee 0:0af507ea0d83 269 {
Thijsjeee 0:0af507ea0d83 270 //Variables
Thijsjeee 1:fafea1d00d0c 271 double Hoa = X[0][0];
Thijsjeee 1:fafea1d00d0c 272 double Hob = X[1][0];
Thijsjeee 1:fafea1d00d0c 273
Thijsjeee 1:fafea1d00d0c 274 double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex;
Thijsjeee 1:fafea1d00d0c 275 double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey;
Thijsjeee 1:fafea1d00d0c 276
Thijsjeee 0:0af507ea0d83 277 //Define f(x)
Thijsjeee 1:fafea1d00d0c 278 fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2);
Thijsjeee 1:fafea1d00d0c 279 fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2);
Thijsjeee 0:0af507ea0d83 280 //Jacobian
Thijsjeee 1:fafea1d00d0c 281
Thijsjeee 1:fafea1d00d0c 282
Thijsjeee 1:fafea1d00d0c 283
Thijsjeee 1:fafea1d00d0c 284 J[0][0]= 0;
Thijsjeee 1:fafea1d00d0c 285 J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob));
Thijsjeee 1:fafea1d00d0c 286 J[1][0]= - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2;
Thijsjeee 1:fafea1d00d0c 287 J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2;
Thijsjeee 0:0af507ea0d83 288 }
Thijsjeee 0:0af507ea0d83 289
Thijsjeee 1:fafea1d00d0c 290 void angle_define() //define the angle needed.
Thijsjeee 1:fafea1d00d0c 291 {
Thijsjeee 1:fafea1d00d0c 292 for(int i=1 ; i <= MaxIter; i++) {
Thijsjeee 1:fafea1d00d0c 293 NR();
Thijsjeee 0:0af507ea0d83 294
Thijsjeee 1:fafea1d00d0c 295 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 296 X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]);
Thijsjeee 0:0af507ea0d83 297
Thijsjeee 0:0af507ea0d83 298 err[0][0] = abs(X[0][0] - Xold[0][0]);
Thijsjeee 0:0af507ea0d83 299 err[1][0] = abs(X[1][0] - Xold[1][0]);
Thijsjeee 0:0af507ea0d83 300
Thijsjeee 0:0af507ea0d83 301 Xold[0][0] = X[0][0];
Thijsjeee 0:0af507ea0d83 302 Xold[1][0] = X[1][0];
Thijsjeee 1:fafea1d00d0c 303
Thijsjeee 0:0af507ea0d83 304 counts_a = ((X[0][0]) / (2* pi)) * 8400;
Thijsjeee 1:fafea1d00d0c 305 counts_b = ((X[1][0]) / (2* pi)) * 8400;
Thijsjeee 1:fafea1d00d0c 306
Thijsjeee 1:fafea1d00d0c 307 if(err[0][0] <= tolX) {
Thijsjeee 1:fafea1d00d0c 308 if(err[1][0] <= tolX) {
Thijsjeee 0:0af507ea0d83 309 break;
Thijsjeee 0:0af507ea0d83 310 }
Thijsjeee 1:fafea1d00d0c 311 }
Thijsjeee 1:fafea1d00d0c 312 }
Thijsjeee 0:0af507ea0d83 313 }
Thijsjeee 0:0af507ea0d83 314
Thijsjeee 0:0af507ea0d83 315 void position_define()
Thijsjeee 0:0af507ea0d83 316 {
Thijsjeee 1:fafea1d00d0c 317 if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) {
Thijsjeee 1:fafea1d00d0c 318 if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
Thijsjeee 1:fafea1d00d0c 319 } else {
Thijsjeee 1:fafea1d00d0c 320 if (ey > Cyy) {
Thijsjeee 8:364ea64ae86b 321 ey = ey - 0.008;
Thijsjeee 1:fafea1d00d0c 322 }
Thijsjeee 1:fafea1d00d0c 323 if (ey < Cyy) {
Thijsjeee 8:364ea64ae86b 324 ey = ey + 0.008;
Thijsjeee 1:fafea1d00d0c 325 }
Thijsjeee 0:0af507ea0d83 326 }
Thijsjeee 1:fafea1d00d0c 327 } else {
Thijsjeee 1:fafea1d00d0c 328 if (ex > Cxx) {
Thijsjeee 8:364ea64ae86b 329 ex = ex - 0.008;
Thijsjeee 1:fafea1d00d0c 330 }
Thijsjeee 1:fafea1d00d0c 331 if (ex < Cxx) {
Thijsjeee 8:364ea64ae86b 332 ex = ex + 0.008;
Thijsjeee 0:0af507ea0d83 333 }
Thijsjeee 0:0af507ea0d83 334 }
Thijsjeee 0:0af507ea0d83 335 }
Thijsjeee 0:0af507ea0d83 336
Thijsjeee 1:fafea1d00d0c 337
Thijsjeee 1:fafea1d00d0c 338
Thijsjeee 1:fafea1d00d0c 339
Thijsjeee 1:fafea1d00d0c 340
Thijsjeee 1:fafea1d00d0c 341
Thijsjeee 1:fafea1d00d0c 342
Thijsjeee 1:fafea1d00d0c 343 void position_controll_void()
Thijsjeee 0:0af507ea0d83 344 {
Thijsjeee 1:fafea1d00d0c 345 bas = true;
Thijsjeee 0:0af507ea0d83 346 }
Thijsjeee 0:0af507ea0d83 347
Thijsjeee 1:fafea1d00d0c 348
Thijsjeee 0:0af507ea0d83 349 void motor_controler()
Thijsjeee 0:0af507ea0d83 350 {
Thijsjeee 7:fcb20c3ccee9 351 bb = -(Enc1.getPulses()) - 816;
Thijsjeee 7:fcb20c3ccee9 352 bc = Enc2.getPulses() + 4316;
Thijsjeee 0:0af507ea0d83 353
Thijsjeee 1:fafea1d00d0c 354 if (bb >= counts_a) {
Thijsjeee 1:fafea1d00d0c 355 z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 356 PMW1.write(abs(z));
Thijsjeee 0:0af507ea0d83 357 M1 = 1;
Thijsjeee 0:0af507ea0d83 358 }
Thijsjeee 1:fafea1d00d0c 359 if (bb <= counts_a) {
Thijsjeee 1:fafea1d00d0c 360 z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
Thijsjeee 1:fafea1d00d0c 361 PMW1.write(abs(z));
Thijsjeee 1:fafea1d00d0c 362 M1 = 0;
Thijsjeee 1:fafea1d00d0c 363 }
Thijsjeee 1:fafea1d00d0c 364 if (bc >= counts_b) {
Thijsjeee 0:0af507ea0d83 365 M2 = 0;
Thijsjeee 1:fafea1d00d0c 366 z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 367 PMW2.write(abs(z));
Thijsjeee 1:fafea1d00d0c 368 }
Thijsjeee 1:fafea1d00d0c 369 if (bc <= counts_b) {
Thijsjeee 0:0af507ea0d83 370 M2 = 1;
Thijsjeee 1:fafea1d00d0c 371 z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
Thijsjeee 0:0af507ea0d83 372 PMW2.write(abs(z));
Thijsjeee 1:fafea1d00d0c 373 }
Thijsjeee 0:0af507ea0d83 374 }
Thijsjeee 0:0af507ea0d83 375
Thijsjeee 2:f68fd7b1c655 376
Thijsjeee 2:f68fd7b1c655 377 void change_wait()
Thijsjeee 2:f68fd7b1c655 378 {
Thijsjeee 2:f68fd7b1c655 379 waiting = waiting++;
Thijsjeee 2:f68fd7b1c655 380 }
Thijsjeee 2:f68fd7b1c655 381
Thijsjeee 2:f68fd7b1c655 382
Thijsjeee 2:f68fd7b1c655 383 void initializeren()
Thijsjeee 2:f68fd7b1c655 384 {
Thijsjeee 2:f68fd7b1c655 385 waiting = 1;
Thijsjeee 4:f16a18aa58bd 386 while(waiting <=2)
Thijsjeee 7:fcb20c3ccee9 387 if (bas == true) {
Thijsjeee 7:fcb20c3ccee9 388 if (waiting == 1) {
Thijsjeee 8:364ea64ae86b 389 Cxx = -17;
Thijsjeee 8:364ea64ae86b 390 Cyy = 3;
Thijsjeee 7:fcb20c3ccee9 391
Thijsjeee 7:fcb20c3ccee9 392 position_define();
Thijsjeee 7:fcb20c3ccee9 393 angle_define();
Thijsjeee 7:fcb20c3ccee9 394 motor_controler();
Thijsjeee 7:fcb20c3ccee9 395
Thijsjeee 7:fcb20c3ccee9 396 }
Thijsjeee 2:f68fd7b1c655 397
Thijsjeee 7:fcb20c3ccee9 398 if(waiting == 2) {
Thijsjeee 8:364ea64ae86b 399 Cxx = -45.8;
Thijsjeee 8:364ea64ae86b 400 Cyy = 7;
Thijsjeee 7:fcb20c3ccee9 401 position_define();
Thijsjeee 7:fcb20c3ccee9 402 angle_define();
Thijsjeee 7:fcb20c3ccee9 403 motor_controler();
Thijsjeee 7:fcb20c3ccee9 404 }
Thijsjeee 7:fcb20c3ccee9 405 }
Thijsjeee 7:fcb20c3ccee9 406 }
Thijsjeee 7:fcb20c3ccee9 407
carlmaykel 12:771f3c73fe24 408
Thijsjeee 7:fcb20c3ccee9 409 void setCalibration()
Thijsjeee 7:fcb20c3ccee9 410 {
Thijsjeee 7:fcb20c3ccee9 411 if (startCalc == false) {
Thijsjeee 2:f68fd7b1c655 412
Thijsjeee 7:fcb20c3ccee9 413 if (calpos1 == false) {
Thijsjeee 7:fcb20c3ccee9 414 while(abs(count2-count1) > 0) {
Thijsjeee 7:fcb20c3ccee9 415 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 416 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 417 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 418 count2 = count1;
Thijsjeee 7:fcb20c3ccee9 419 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 420 }
Thijsjeee 7:fcb20c3ccee9 421 Enc1.reset();
Thijsjeee 7:fcb20c3ccee9 422 bb= Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 423 calpos1 = true;
Thijsjeee 2:f68fd7b1c655 424 }
Thijsjeee 7:fcb20c3ccee9 425 if(calpos2 == false) {
Thijsjeee 7:fcb20c3ccee9 426 while(abs(count4-count3) > 0) {
Thijsjeee 7:fcb20c3ccee9 427 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 428 M1=0;
Thijsjeee 7:fcb20c3ccee9 429 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 430 wait(0.1);
Thijsjeee 8:364ea64ae86b 431 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 432 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 433 count4 = count3;
Thijsjeee 7:fcb20c3ccee9 434 count3 = Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 435 }
Thijsjeee 7:fcb20c3ccee9 436 Enc2.reset();
Thijsjeee 7:fcb20c3ccee9 437 bc= Enc2.getPulses();
Thijsjeee 7:fcb20c3ccee9 438 calpos2 = true;
Thijsjeee 7:fcb20c3ccee9 439 }
Thijsjeee 2:f68fd7b1c655 440 }
Thijsjeee 2:f68fd7b1c655 441 }
Thijsjeee 2:f68fd7b1c655 442
Thijsjeee 2:f68fd7b1c655 443
Thijsjeee 0:0af507ea0d83 444 int main()
Thijsjeee 0:0af507ea0d83 445 {
carlmaykel 10:86c810be889a 446 pc.baud(115200);
carlmaykel 10:86c810be889a 447 pc.printf("Hello World!\r\n");
Thijsjeee 3:40427c0157a0 448 Led = 1;
Thijsjeee 8:364ea64ae86b 449 Led2 = 0;
Thijsjeee 7:fcb20c3ccee9 450 M1 = 1;
Thijsjeee 7:fcb20c3ccee9 451 M2 = 1;
Thijsjeee 7:fcb20c3ccee9 452 startCalc = false;
Thijsjeee 7:fcb20c3ccee9 453 calpos1 = false;
Thijsjeee 7:fcb20c3ccee9 454 calpos2 = false;
Thijsjeee 7:fcb20c3ccee9 455 count2 = 10000;
Thijsjeee 7:fcb20c3ccee9 456 count4 = 10000;
Thijsjeee 8:364ea64ae86b 457 Led = 1;
Thijsjeee 7:fcb20c3ccee9 458 PMW1.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 459 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 460 PMW1.write(0);
Thijsjeee 7:fcb20c3ccee9 461 count1 = Enc1.getPulses();
Thijsjeee 7:fcb20c3ccee9 462 PMW2.write(0.1f);
Thijsjeee 7:fcb20c3ccee9 463 wait(0.1);
Thijsjeee 7:fcb20c3ccee9 464 PMW2.write(0);
Thijsjeee 7:fcb20c3ccee9 465 count3 = Enc2.getPulses();
carlmaykel 11:b1ad257e5647 466
Thijsjeee 7:fcb20c3ccee9 467 setCalibration();
carlmaykel 10:86c810be889a 468 pc.printf("Calibration is done\r\n");
carlmaykel 10:86c810be889a 469 pc.printf("Please press button SW3\n\r");
Thijsjeee 8:364ea64ae86b 470 Led2 = 1;
Thijsjeee 7:fcb20c3ccee9 471 button.fall(&change_wait);
Thijsjeee 0:0af507ea0d83 472 PMW1.period_us(60);
Thijsjeee 7:fcb20c3ccee9 473 PMW2.period_us(60);
Thijsjeee 2:f68fd7b1c655 474 position_controll.attach(position_controll_void,0.002);
Thijsjeee 0:0af507ea0d83 475 X[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 476 X[1][0] = X0[1][0];
Thijsjeee 0:0af507ea0d83 477 Xold[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 478 Xold[1][0] = X0[1][0];
Thijsjeee 2:f68fd7b1c655 479 initializeren();
carlmaykel 11:b1ad257e5647 480 pc.printf("Initialization step done\n\r");
Thijsjeee 1:fafea1d00d0c 481 x1 = Cxx;
Thijsjeee 1:fafea1d00d0c 482 y1= Cyy;
Thijsjeee 1:fafea1d00d0c 483 bqc.add( &bq1 ).add( &bq2 );
Thijsjeee 1:fafea1d00d0c 484 bqc2.add( &bq3 );
Thijsjeee 1:fafea1d00d0c 485 bqc3.add( &bq4 ).add( &bq5 );
Thijsjeee 1:fafea1d00d0c 486 bqc4.add( &bq6 );
Thijsjeee 4:f16a18aa58bd 487 Cxx = -35;
Thijsjeee 4:f16a18aa58bd 488 Cyy = 27;
Thijsjeee 6:e492bc8fc3fb 489 i = 251;
Thijsjeee 1:fafea1d00d0c 490 while(true) {
Thijsjeee 0:0af507ea0d83 491
Thijsjeee 1:fafea1d00d0c 492 if(bas == true) {
Thijsjeee 1:fafea1d00d0c 493 Led = 1;
Thijsjeee 1:fafea1d00d0c 494 filteren();
Thijsjeee 1:fafea1d00d0c 495 position_define();
Thijsjeee 1:fafea1d00d0c 496 angle_define();
Thijsjeee 1:fafea1d00d0c 497 motor_controler();
Thijsjeee 7:fcb20c3ccee9 498
Thijsjeee 8:364ea64ae86b 499 scope.set(0, ex); // filtered 1
Thijsjeee 8:364ea64ae86b 500 scope.set(1, Cxx); //filtered signal 2
Thijsjeee 8:364ea64ae86b 501 scope.set(2, ey);
Thijsjeee 8:364ea64ae86b 502 scope.set(3, Cyy);
Thijsjeee 5:d3031d082c22 503 scope.send();
Thijsjeee 7:fcb20c3ccee9 504
Thijsjeee 7:fcb20c3ccee9 505 if (i <= 250) {
Thijsjeee 6:e492bc8fc3fb 506 emgFiltered3 = 0;
Thijsjeee 6:e492bc8fc3fb 507 emgFiltered23 = 0;
Thijsjeee 7:fcb20c3ccee9 508 i++;
Thijsjeee 6:e492bc8fc3fb 509 }
Thijsjeee 7:fcb20c3ccee9 510
Thijsjeee 7:fcb20c3ccee9 511
Thijsjeee 8:364ea64ae86b 512 sw2.fall(change);
Thijsjeee 6:e492bc8fc3fb 513 Position1x(emgFiltered3);
Thijsjeee 6:e492bc8fc3fb 514 Position1y(emgFiltered23);
Thijsjeee 8:364ea64ae86b 515 Cxx = x1;
Thijsjeee 8:364ea64ae86b 516 Cyy = y1;
Thijsjeee 8:364ea64ae86b 517 Led2 = 1;
Thijsjeee 1:fafea1d00d0c 518 Led = 0;
Thijsjeee 1:fafea1d00d0c 519 bas= false;
Thijsjeee 1:fafea1d00d0c 520 }
Thijsjeee 0:0af507ea0d83 521 }
Thijsjeee 1:fafea1d00d0c 522
Thijsjeee 0:0af507ea0d83 523 }