Diff: main.cpp

Revision:

7:9a1007e35bac

Parent:

6:14a9c4f30c86

Child:

8:bf5192a22c64

Child:

9:aa5d6636197b

diff -r 14a9c4f30c86 -r 9a1007e35bac main.cpp
--- a/main.cpp	Tue Mar 19 16:12:42 2019 +0000
+++ b/main.cpp	Wed Mar 20 13:21:56 2019 +0000
@@ -6,6 +6,8 @@
 // Algemeen
 DigitalIn button2(SW2);
 DigitalIn button3(SW3);
+AnalogIn But2(A5);
+AnalogIn But1(A3);
 DigitalOut led(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
@@ -36,6 +38,7 @@
 //Ticker
 Ticker Pwm;
 Ticker PotRead;
+Ticker Kdc;
 
 
 //Kinematica
@@ -54,14 +57,11 @@
 float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
 
 // VARIABLES PID CONTROLLER
-double Kp = Pot1*10; //
-double Ki = 5;   //
-double Kd = 5; //
-double Ts = 0.1; // Sample time in seconds
+double Kp = 6;          // Zonder arm: 6,0,1
+double Ki = 0;          //
+double Kd = 1;          //
+double Ts = 0.001;      // Sample time in seconds
 
-
-
-}
 float Kinematics(float KPot)
 {
 
@@ -110,7 +110,19 @@
     static double error_integral = 0;
     static double error_prev = error; // 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
+    Kp = 10 * Pot2;
+    Ki = 10 * Pot1;
+    
+    if (!But2) {
+        Kd = Kd + 0.01;
+    }
+    if (!But1){
+        Kd = Kd - 0.01;
+    }
+    */
+    
     // Proportional part:
     double u_k = Kp * error;
 
@@ -141,22 +153,16 @@
 
 void PwmMotor(void)
 {
+    // Reference hoek berekenen, in graden
     float Ellebooghoek1 = Kinematics(pwm2);
     float Ellebooghoek4 = Limits(Ellebooghoek1);
     ElbowReference = Ellebooghoek4;
 
-    //pc.printf("ElbowReference:%f\n", ElbowReference);
-    //pc.printf("pwm2=%f\r\n",pwm2);
-
+    // Positie motor berekenen, in graden
     Pulses2 = encoder2.getPulses();
     motor_position2 = -(Pulses2/8400)*360;
-    
-    double error = reference_rotation - motor_position2;
-    //double error = ElbowReference - motor_position2;
-    //pc.printf("Degrees is:%f, error = %f\n", motor_position2, error);
 
-    //pwmpin2 = fabs(pwm2);                 //zonder PID
-
+    double error = ElbowReference - motor_position2;
     double u = PID_controller(error);
     moter2_control(u);
 
@@ -178,12 +184,21 @@
 {
     Pot2 = pot.read();
     Pot1 = pot0.read();
-    pwm2 =(Pot2*2)-1;            //scaling
-
-    //pc.printf("%f\r\n",Pot2);
+    pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
 }
 
-
+/*
+void Kdcount (void)             // Voor het testen van de PID waardes
+{
+    int count = 0;
+    ElbowReference = ElbowReference + 10;
+    if (count == 7) {
+        ElbowReference = 0;
+        count = 0;
+    }
+    count ++;
+}
+*/
 
 int main()
 {
@@ -192,6 +207,7 @@
     int counter = 0;
     pwmpin2.period_us(60);
     PotRead.attach(ContinuousReader,Ts);
+    //Kdc.attach(Kdcount,5);                //Voor PID waarde testen
     pc.baud(115200);
     //pc.printf("start\r\n");
     led = 1;
@@ -214,7 +230,7 @@
         led = 0;
         if(counter==10) {
             float tmp = t.read();
-            printf("%f,%f,%f,%f\n\r",tmp,motor_position2,reference_rotation,Pot1);
+            printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Kp,Ki,Kd);
             counter = 0;
         }
         counter++;