Diff: main.cpp

Revision:

8:bf5192a22c64

Parent:

7:9a1007e35bac

--- a/main.cpp	Wed Mar 20 13:21:56 2019 +0000
+++ b/main.cpp	Thu Mar 21 10:12:46 2019 +0000
@@ -14,18 +14,22 @@
 MODSERIAL pc(USBTX, USBRX);
 
 //Motoren
-DigitalOut direction1(D4);
+DigitalOut direction1(D4);  //Motor 1 = rotatie gewricht
 PwmOut pwmpin1(D5);
-PwmOut pwmpin2(D6);
+PwmOut pwmpin2(D6);         //Motor 2 = elleboog gewricht
 DigitalOut direction2(D7);
 volatile float PWM1;
 volatile float PWM2;
 volatile float pwm2;
+volatile float pwm1;
 
 //Encoder
 DigitalIn EncoderA(D13);
 DigitalIn EncoderB(D12);
 QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
+QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
+double Pulses1;
+double motor_position1;
 double Pulses2;
 double motor_position2;
 
@@ -57,8 +61,8 @@
 float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
 
 // VARIABLES PID CONTROLLER
-double Kp = 6;          // Zonder arm: 6,0,1
-double Ki = 0;          //
+double Kp = 10 * Pot1;          // Zonder arm: 6,0,1
+double Ki = 10 * Pot2;          //
 double Kd = 1;          //
 double Ts = 0.001;      // Sample time in seconds
 
@@ -111,7 +115,7 @@
     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
+    // PID testing
     Kp = 10 * Pot2;
     Ki = 10 * Pot1;
     
@@ -121,7 +125,7 @@
     if (!But1){
         Kd = Kd - 0.01;
     }
-    */
+    
     
     // Proportional part:
     double u_k = Kp * error;
@@ -140,15 +144,15 @@
     return u_k + u_i + u_d;
 }
 
-void moter2_control(double u)
+void moter1_control(double u1)
 {
-    direction2= u < 0.0f; //positief = CW
-    if (fabs(u)> 0.7f) {
-        u = 0.7f;
+    direction1= u1 < 0.0f; //positief = CW
+    if (fabs(u1)> 0.7f) {
+        u1 = 0.7f;
     } else {
-        u= u;
+        u1= u1;
     }
-    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+    pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void PwmMotor(void)
@@ -159,17 +163,24 @@
     ElbowReference = Ellebooghoek4;
 
     // Positie motor berekenen, in graden
+    Pulses1 = encoder1.getPulses();
     Pulses2 = encoder2.getPulses();
+    motor_position1 = -(Pulses1/1200)*360;
     motor_position2 = -(Pulses2/8400)*360;
 
-    double error = ElbowReference - motor_position2;
-    double u = PID_controller(error);
-    moter2_control(u);
+    //double error = ElbowReference - motor_position2;
+    double error1 = ElbowReference - motor_position1;
+    
+    //double u = PID_controller(error);
+    double u1 = PID_controller(error1);
+    moter1_control(u1);
+    //moter2_control(u);
 
 }
 
 void MotorOn(void)
 {
+    pwmpin1 = 0;
     pwmpin2 = 0;
     Pwm.attach (PwmMotor, Ts);
 
@@ -177,6 +188,7 @@
 void MotorOff(void)
 {
     Pwm.detach ();
+    pwmpin1 = 0;
     pwmpin2 = 0;
 }
 
@@ -184,10 +196,11 @@
 {
     Pot2 = pot.read();
     Pot1 = pot0.read();
+    pwm1 =(Pot1*2)-1;
     pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
 }
 
-/*
+
 void Kdcount (void)             // Voor het testen van de PID waardes
 {
     int count = 0;
@@ -198,16 +211,17 @@
     }
     count ++;
 }
-*/
+
 
 int main()
 {
     Timer t;
     t.start();
     int counter = 0;
+    pwmpin1.period_us(60);
     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;
@@ -230,7 +244,7 @@
         led = 0;
         if(counter==10) {
             float tmp = t.read();
-            printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Kp,Ki,Kd);
+            printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position1,ElbowReference,Kp,Ki,Kd);
             counter = 0;
         }
         counter++;