Diff: main.cpp

Revision:

0:921402a95675

Child:

1:7969189824ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 01 16:16:04 2017 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "math.h"
+#include "QEI.h"
+
+Serial      pc(USBTX, USBRX);        //Serial PC connectie
+DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
+DigitalOut  motor2DirectionPin(D7);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor2MagnitudePin(D6);  //Motorkracht op D5 (connected op het bord)
+QEI         q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
+QEI         q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
+const double pi = 3.1415926535897;  // waarde voor pi aanmaken
+
+Timer t;
+Ticker aansturing;
+Ticker stepres;
+
+double ref1 = 0;
+double refrad1;
+double refrad2;
+double ref2 = 0;
+double Kp1 = 0.002;    //kp motor 2 = 0.0001
+double Ki1 = 0;
+double Kp2 = 0.0001;
+double Ki2 = 0;
+int q1_puls;
+int q2_puls;
+double rad2pulses=4200/(2*pi);
+double ts = 0.001;
+double PI1;
+int n;
+
+double error1_1;
+double error2_1 = 0;
+double error_I_1;
+double error_I2_1 = 0;
+double error1_2;
+double error2_2 = 0;
+double error_I_2;
+double error_I2_2 = 0;
+double PI2;
+
+void controller()
+{   
+    refrad2 =  1.57f*sin(t.read()); //motor 2
+    ref2 = refrad2 * rad2pulses;   
+    refrad1 = -0.6f*cos(t.read()) + 0.2f; //motor 1
+    ref1 = refrad2 * rad2pulses;
+    
+    // encoders uitlezen    
+    q1_puls = q1_enc.getPulses();
+    q2_puls = q2_enc.getPulses();
+
+    //PID 1
+error1_1 = ref1 - q1_puls;
+error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
+
+PI1 = Kp1*error1_1 + Ki1*error_I_1;
+
+error2_1   = error1_1; // opslaan variabelen voor integraal onderdeel
+error_I2_1 = error_I_1;
+    
+    //PID 2
+error1_2 = ref2 - q2_puls;
+error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
+
+PI2 = Kp2*error1_2 + Ki2*(error_I_2);
+
+
+    if (PI1<=0)
+    {
+    motor1DirectionPin = 1;
+    motor1MagnitudePin = fabs(PI1);
+    }
+    else if (PI1>0)
+    {
+    motor1DirectionPin = 0;
+    motor1MagnitudePin = fabs(PI1);        
+    }
+    
+    if (PI2>=0)
+    {
+    motor2DirectionPin = 1;
+    motor2MagnitudePin = fabs(PI2);   
+    }
+    else if(PI2<0)
+    {
+    motor2DirectionPin = 0;
+    motor2MagnitudePin = fabs(PI2);               
+    }
+    
+    if(n==500){         
+    printf("PI1 = %f\n\r", PI1);
+    n=0;
+    }
+    else{
+             n=n+1;
+             }     
+
+}
+
+void stapfunc()
+{
+ if (ref1==0){
+     ref1 = 1000;
+     }
+     else if (ref1==1000)
+     {
+         ref1=0; 
+     }  
+}
+
+int main()
+{
+    pc.baud(115200);
+    t.start();
+    aansturing.attach_us(&controller, 1000);
+    //stepres.attach(&stapfunc, 3);
+    
+    while(true){
+
+        }
+
+
+}
\ No newline at end of file