Diff: NR_method_1.cpp

Revision:

9:4fc2659cfb26

Parent:

8:364ea64ae86b

Child:

10:86c810be889a

--- a/NR_method_1.cpp	Thu Nov 01 18:10:54 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 18:57:26 2018 +0000
@@ -9,11 +9,55 @@
 #include "PID_controler.h"
 #include <HIDScope.h>
 
-
-//#include  <MODSERIAL.h>
+//Define in/outputs
+Serial pc(USBTX, USBRX);
+AnalogIn   emg1(A0);// emg1 input
+AnalogIn   emg2(A1); // emg2 input
+InterruptIn sw2(SW2);
+InterruptIn button(SW3);
+DigitalOut Led(LED1);
+DigitalOut Led2(LED2);
+PwmOut PMW1(D5); // Motor 1
+DigitalOut M1(D4); // direction of motor 1
+PwmOut PMW2(D6); // Motor 2
+DigitalOut M2(D7); // direction of motor 2
 
 // hidscope
-HIDScope    scope( 4 );
+HIDScope    scope( 4 ); // 4 to check the different 
+
+// tickers
+Ticker sample_timer;
+Ticker position_controll;
+
+//Define Variables
+double pi = 3.14159265359;
+int bb;
+int bc;
+float z;
+int i;
+double angle_a = 0; //in rad
+double angle_b = 0.5 * pi;  //in rad
+double X0[2][1] = {{angle_a},{angle_b}};
+double X[2][1];
+double Xold[2][1];
+double fval[2][1];
+double J[2][2];
+double err[2][1];
+double MaxIter = 20;
+double tolX = 1e-4;
+double A = 20;
+double B = 30;
+double C = 20;
+double D = 27;
+double E = 35;
+double ex = -40; // current position
+double ey = -30; // current position
+double Cxx = -35; // Goal position
+double Cyy = 27; // Goal position
+double Kp = 1;
+double Ki = 1;
+double Kd = 0.3;
+double Ts = 0.001;
 bool startCalc;
 bool calpos1;
 bool calpos2;
@@ -23,40 +67,34 @@
 int count2;
 int count3;
 int count4;
-InterruptIn button(SW3);
-
-Serial pc(USBTX, USBRX);
-// emg signals input
-AnalogIn   emg1(A0);
-AnalogIn   emg2(A1);
-InterruptIn sw2(SW2);
-// tickers
-Ticker sample_timer;
-
+float counts_a;
+float counts_b;
+int counts = 8400;
 volatile float x1;
 volatile float y1;
 double emgFiltered3;
 double emgFiltered23;
 bool dir = true;
+
 // filtering
 //filter coeffiecents
 // highpass
-double b01h = 0.956543225556877;
+double b01h =  0.956543225556877;
 double b02h = -1.91308645111375;
 double b03h =  0.956543225556877;
 double a01h = -1.91119706742607;
-double a02h =   0.914975834801434;
+double a02h =  0.914975834801434;
 // notchfilter
-double b01n = 0.991103635646810;
-double b02n =   -1.60363936885013;
-double b03n = 0.991103635646810;
-double a01n =   -1.60363936885013;
+double b01n =  0.991103635646810;
+double b02n = -1.60363936885013;
+double b03n =  0.991103635646810;
+double a01n = -1.60363936885013;
 double a02n =  0.982207271293620;
 //lowpass 1
 double b01l =  0.000346041337639103;
 double b02l =  0.000692082675278205;
 double b03l =  0.000346041337639103;
-double a01l =  -1.94669754075618;
+double a01l = -1.94669754075618;
 double a02l =  0.948081706106740;
 BiQuadChain bqc;
 BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
@@ -75,62 +113,11 @@
 BiQuadChain bqc4;
 BiQuad bq6( b01l, b02l, b03l, a01l, a02l);  //lowpass
 
-
-//Define in/outputs
-
-int counts = 8400;
-DigitalOut Led(LED1);
-DigitalOut Led2(LED2);
-PwmOut PMW1(D5); // Motor 1
-DigitalOut M1(D4); // direction of motor 1
-PwmOut PMW2(D6); // Motor 2
-DigitalOut M2(D7); // direction of motor 2
-
 //initializing Encoders
 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
 
-double Kp = 1;
-double Ki = 1;
-double Kd = 0.3;
-double Ts = 0.001;
-
-float counts_a;
-float counts_b;
-
-
-//Define Variables
-
-double pi = 3.14159265359;
-int bb;
-int bc;
-float z;
-int i;
-
-double angle_a = 0; //in rad
-double angle_b = 0.5 * pi;  //in rad
-
-double X0[2][1] = {{angle_a},{angle_b}};
-double X[2][1];
-double Xold[2][1];
-double fval[2][1];
-double J[2][2];
-double err[2][1];
-
-double MaxIter = 20;
-double tolX = 1e-4;
-double A = 20;
-double B = 30;
-double C = 20;
-double D = 27;
-double E = 35;
-double ex = -40; // current position
-double ey = -30; // current position
-double Cxx = -35; // Goal position
-double Cyy = 27; // Goal position
-
-Ticker position_controll;
-
+//Functions
 void filteren ()
 {
     double emgSignal1 = emg1.read();