Diff: main.cpp

Revision:

9:08a7a8e59a6a

Parent:

8:c6c94d55b088

Child:

10:b871d1b05787

--- a/main.cpp	Mon Oct 14 07:14:19 2019 +0000
+++ b/main.cpp	Mon Oct 14 09:06:08 2019 +0000
@@ -11,7 +11,7 @@
 AnalogIn potmeter1(A0);
 AnalogIn potmeter2(A1);
 AnalogIn potmeter3(A2);
-AnalogIn potmeter4(A4);
+AnalogIn potmeter4(A3);
 // Encoder
 DigitalIn encA(D13);
 DigitalIn encB(D12);
@@ -32,7 +32,7 @@
 //Motorcontrol
 bool motordir;
 double motorpwm;
-float u1;
+float u1= 0;
 double u2;
 double potValue;
 double pi2= 6.283185;
@@ -72,16 +72,18 @@
     static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
     
     //Proportional part:
-    Kp=2*potmeter2.read();
+    //Kp=2*potmeter1.read();
+    Kp=101;
     u_k=Kp*e;
     
     //Integral part
-    Ki=2*potmeter3.read();
+    Ki=10;
     error_integral=error_integral+e*Ts;
     u_i=Ki*error_integral;
     
     //Derivative part
-    Kd=2*potmeter4.read(); 
+    //Kd=2*potmeter2.read(); 
+    Kd=10;
     float error_derivative =(e-e_prev)/Ts;
     float filtered_error_derivative = LowPassFilter.step(error_derivative);
     u_d=Kd*filtered_error_derivative;
@@ -99,12 +101,18 @@
 
     countsPrev = counts;
 }
+
+void togglehoek(){
+    u1= 0.25*pi2+u1;
+    }
+    
 void motorControl()
 {
+    button1.fall(&togglehoek);
     angle = counts * factorin / gearratio; // Angle of motor shaft in rad
     omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter1.read();
-    u1= (potValue*2*pi2)-pi2;
+    //u1= (potValue*2*pi2)-pi2;
     e=u1-angle;
     
     u2=PID_controller(e);
@@ -143,6 +151,7 @@
     encoderTicker.attach(readEncoder, Ts);
     
     button2.fall(&toggleMotor);
+    
     while (true) {
 
         //pc.printf("Potmeter: %d \r\n", potValue,);