Diff: main.cpp

Revision:

23:0609a43076ff

Parent:

19:1353ba4d94db

--- a/main.cpp	Mon Oct 22 08:54:04 2018 +0000
+++ b/main.cpp	Wed Oct 31 11:23:37 2018 +0000
@@ -4,12 +4,12 @@
 #include "QEI.h"
 #include <math.h>
 MODSERIAL pc(USBTX, USBRX);
-DigitalOut motor1DirectionPin(D7);
-FastPWM motor1MagnitudePin(D6); 
+DigitalOut motor1DirectionPin(D4);
+FastPWM motor1MagnitudePin(D5); 
 AnalogIn potMeter1(A4);
 AnalogIn potMeter2(A5);
 InterruptIn button2(D3);
-QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING);
+QEI Encoder (D10, D11, NC, 64, QEI::X4_ENCODING);
 
 //Tickers
 Ticker MeasureControl;
@@ -19,22 +19,17 @@
 volatile double measuredPosition = 0.0;
 volatile double referencePosition = 0.0; 
 volatile double motorValue= 0.01;
-volatile double Kp = 5.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
-volatile double Ki = 1.0; //dit moeten we bepalen met een plot bijvoorbeeld
+volatile double Kp = 0.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
+volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
 volatile double Kd = 0.0;
 volatile double Ts = 0.01;
 
 //------------------------------------------------------------------------------
 // Functions
-double Filters()
-{
-    static BiQuad Notchfilter(1.0000, -1.6180, 1.0000, 1.0000, -1.5687, 0.9391);
-    static BiQuad HighPassFilter (1.0000, -2.0000, 1.0000, 1.0000, -1.8268, 0.8407);
-    static BiQuad LowPassFilter (1.0000, 2.0000, 1.0000, 1.0000, 0.3172, 0.1894);
-}
+
 double GetReferencePosition()
 {
-   double potMeterIn = potMeter1.read();
+   double potMeterIn = potMeter2.read();
    referencePosition = 4.0*3.14*potMeterIn - 2.0*3.14 ; // Reference value y, scaled to -2 to 2 revolutions (or 0 to 100 pi) WAAROM?
    return referencePosition;
 }
@@ -53,13 +48,13 @@
     //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
     // Proportional part: 
     //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
+    Kp = 20*potMeter1.read();
     double u_k = Kp * Error;
     // Integral part:
     Error_integral = Error_integral + Error * Ts; 
     double u_i = Ki * Error_integral;
     // Derivative part
     double Error_derivative = (Error - Error_prev)/Ts;
-    Kd = 20*potMeter2.read();
     double u_d = Kd * Error_derivative;
     Error_prev = Error;
     // Sum all parts and return it