Diff: main.cpp

Revision:

8:c6c94d55b088

Parent:

7:20a802dfe664

Child:

9:08a7a8e59a6a

--- a/main.cpp	Fri Oct 11 13:02:43 2019 +0000
+++ b/main.cpp	Mon Oct 14 07:14:19 2019 +0000
@@ -10,6 +10,8 @@
 InterruptIn button2(D10);
 AnalogIn potmeter1(A0);
 AnalogIn potmeter2(A1);
+AnalogIn potmeter3(A2);
+AnalogIn potmeter4(A4);
 // Encoder
 DigitalIn encA(D13);
 DigitalIn encB(D12);
@@ -35,8 +37,8 @@
 double potValue;
 double pi2= 6.283185;
 float e; //e = error
-float Kp=0.45;
-float Ki=0;
+float Kp;
+float Ki;
 float Kd;
 float u_k;
 float u_i;
@@ -70,14 +72,16 @@
     static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
     
     //Proportional part:
-    Kd=0.5*potmeter2.read();
+    Kp=2*potmeter2.read();
     u_k=Kp*e;
     
     //Integral part
+    Ki=2*potmeter3.read();
     error_integral=error_integral+e*Ts;
     u_i=Ki*error_integral;
     
     //Derivative part
+    Kd=2*potmeter4.read(); 
     float error_derivative =(e-e_prev)/Ts;
     float filtered_error_derivative = LowPassFilter.step(error_derivative);
     u_d=Kd*filtered_error_derivative;