Diff: main.cpp

Revision:

3:5b5b8ebdc46c

Parent:

2:f5bb7fcfc7a2

--- a/main.cpp	Thu Jun 09 15:34:51 2016 +0000
+++ b/main.cpp	Fri Jun 10 12:31:08 2016 +0000
@@ -39,6 +39,7 @@
 double Force_nieuw=0;
 double Ref_der_prev;
 double Plaats_der1;
+double Plaats_der2;
 
 //LowPass filter 2 Hz
 const double a1_LP =  -1.940778135263835, a2_LP = 0.942482022027066;
@@ -51,12 +52,18 @@
 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
 
 
-biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);        // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
-biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);        // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10 
+biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);        // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
+biquadFilter Filter2(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);        // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10 
 biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor signaal positie-potmeter, 300 Hz
-biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz 
+biquadFilter Filter4(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);   // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz 
 biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz
 
+/* Aanpasmogelijkheden die succes kunnen geven: 
+-Positiesignaal ook door highpassfilter gooien, filters voor krachtsensor en positie gelijkstellen. 
+-Lowpassfilter van geschaalde signaal weghalen. 
+- Signalen bekijken van referentie, ref_der en de tweede afgeleide. Wellicht ook derde afgeleide bekijken. 
+- 
+*/
 
 double Y_filt;
 double Y_filt2;
@@ -69,11 +76,11 @@
 // The data read and send function
 void scopeSend()
 {
-    scope.set(0,Force_nieuw);
-    scope.set(1,Y_filtder);
-    scope.set(2,Plaats_der);
-    scope.set(3,Input);
-    scope.set(4,A);
+    scope.set(0,Force_nieuw); //Y_filt);
+    scope.set(1,Y_filt2);
+    scope.set(2,Plaats_der1);
+    scope.set(3,Input);//Referentie2);// 
+    scope.set(4,A);//Y_filtder);//
     scope.send();
 
 }
@@ -172,20 +179,20 @@
 
 void Afgeleide_Force()
 {
-    Y_filtder = (Y_filt-Y_filtprev)/0.01;
-    Y_filtprev = Y_filt;
+    Y_filtder = (Force_nieuw-Y_filtprev)/0.01;
+    Y_filtprev = Force_nieuw;
 }
 
 void Afgeleide_Plaats()
 {
     LoopTimerFlag2 = 0;
-    Plaats_der = Ref_der/0.01;
-    Plaats_der1 = Filter6.step(Plaats_der);
-    Plaats_der = (Plaats_der1-Ref_der_prev)/0.1;
-    if (Plaats_der>=0) {
-        Plaats_der = Plaats_der;
-    } else if (Plaats_der<0) {
-        Plaats_der = 0.5*Plaats_der;
+    Plaats_der = Ref_der/0.1;
+    Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))-1;
+    Plaats_der2 = (Plaats_der1-Ref_der_prev);
+    if (Plaats_der1>=-1) {
+        Plaats_der1 = Plaats_der1;
+    } else if (Plaats_der1<-1) {
+        Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))*0.5-1;
     }
     Ref_der_prev= Plaats_der1;
 }
@@ -212,7 +219,7 @@
         while(LoopTimerFlag !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
         LoopTimerFlag = 0;    // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
         Led = 1;
-        Y_filt = 3*Filter1.step(a_in3.read());
+        Y_filt = 5*Filter1.step(a_in3.read())+1;
         Y_filt2 = Filter2.step(Y_filt);
         Input = Inputberekening3(Force_nieuw); 
         time_ += time_increment;
@@ -225,7 +232,7 @@
         if (LoopTimerFlag2==1) {
             Afgeleide_Force();
             Afgeleide_Plaats();
-            Force_nieuw = Y_filtder-Boardpotmeter2.read()*Plaats_der;
+            Force_nieuw = Y_filtder-Plaats_der2;
         }
         scopeSend();
     }