Diff: main.cpp

Revision:

14:30d32f51dd9f

Parent:

13:b7a1a4245f37

Child:

16:75884d07334e

Child:

17:190c86f4828f

--- a/main.cpp	Thu Apr 04 12:07:14 2019 +0000
+++ b/main.cpp	Thu Apr 04 12:25:55 2019 +0000
@@ -70,7 +70,7 @@
 float upperlim2 = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
 
 // VARIABLES PID CONTROLLER
-double Kp1 = 5;          
+double Kp1 = 0;          
 double Ki1 = 0;          
 double Kd1 = 1;
 double Kp2 = 6;          // Zonder arm: 6,0,1
@@ -164,17 +164,17 @@
     static double error1_prev = error1; // initialization with this value only done once!
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
     
-    /* PID testing
-    Kp1 = 10 * Pot2;
+    //PID testing
+    Kp1 = 30 * Pot2;
     Ki1 = 10 * Pot1;
     
     if (!But2) {
-        Kd1 = Kd1 + 0.01;
+        Kd1 = Kd1 + 0.001;
     }
     if (!But1){
-        Kd1 = Kd1 - 0.01;
+        Kd1 = Kd1 - 0.001;
     }
-    */
+    
     
     // Proportional part:
     double u_k1 = Kp1 * error1;
@@ -251,14 +251,14 @@
 
 void PwmMotor(void)
 {
-    // Reference hoek berekenen, in graden
+    //Reference hoek berekenen, in graden
     float Ellebooghoek1 = Kinematics2(pwm2);
     float Ellebooghoek4 = Limits2(Ellebooghoek1);
-    ElbowReference = Ellebooghoek4;
+    //ElbowReference = Ellebooghoek4;
     
-    float Polshoek1 = Kinematics1(pwm2);
-    float Polshoek4 = Limits1(Polshoek1);
-    PolsReference = Polshoek4;
+    //float Polshoek1 = Kinematics1(pwm2);
+    //float Polshoek4 = Limits1(Polshoek1);
+    //PolsReference = Polshoek4;
 
     // Positie motor berekenen, in graden
     Pulses1 = encoder1.getPulses();
@@ -298,18 +298,18 @@
     pwm1 =(Pot1*2)-1;
 }
 
-/*
+
 void Kdcount (void)             // Voor het testen van de PID waardes
 {
     int count = 0;
-    PolsReference = PolsReference + 10;
+    PolsReference = PolsReference + 50;
     if (count == 7) {
         PolsReference = 0;
         count = 0;
     }
     count ++;
 }
-*/
+
 
 int main()
 {
@@ -318,7 +318,7 @@
     int counter = 0;
     pwmpin2.period_us(60);
     PotRead.attach(ContinuousReader,Ts);
-    //Kdc.attach(Kdcount,5);                //Voor PID waarde testen
+    Kdc.attach(Kdcount,5);                //Voor PID waarde testen
     pc.baud(115200);
     //pc.printf("start\r\n");
     led = 1;