Diff: main.cpp

Revision:

24:d0af4b2be295

Parent:

20:f5091e29cd26

Child:

25:ae908de29943

--- a/main.cpp	Fri Oct 02 17:22:17 2015 +0000
+++ b/main.cpp	Sat Oct 03 15:59:25 2015 +0000
@@ -5,8 +5,9 @@
 #include "biquadFilter.h"
  
 Serial pc(USBTX, USBRX); // tx, rx
-QEI Encoder(D3, D2, NC, 128);
-HIDScope scope(3);
+QEI Encoder2(D3, D2, NC, 32);
+QEI Encoder1(D13,D12,NC, 32);
+HIDScope scope(4);
 
 //Ledjes
 DigitalOut LedR(LED_RED);
@@ -14,26 +15,32 @@
 DigitalOut LedB(LED_BLUE);
 
 //Motor
-DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
+DigitalOut motor1direction(D6);
+PwmOut motor1speed(D7);
+DigitalOut motor2direction(D4);
 PwmOut motor2speed(D5);
 
 //Tickers
 Ticker ScopeTime;
-Ticker myControllerTicker;
-Ticker PrintTime;
+Ticker myControllerTicker2;
 
 //Startwaarden
-double reference;
-double position;
-double m2_ref = 0;
+double reference2, reference1;
+double position2 = 0, position1 = 0;
+double m2_ref = 0, m1_ref = 0;
 int count = 0;
+double Grens2 = 90, Grens1 = 90;
+double Stapgrootte = 5;
 
 //Sample time (motor2-step)
 const double m2_Ts = 0.01;
+const double m1_Ts = 0.01;
 
-//Controller gain Motor 2
-const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
+//Controller gain Motor 
+const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
+const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
 double m2_err_int = 0, m2_prev_err = 0;
+double m1_err_int = 0, m1_prev_err = 0;
 
 //Derivative filter coeffs Motor 2
 const double BiGain = 0.016955;
@@ -46,20 +53,14 @@
 //HIDScope
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
-    scope.set(0, motor2direction.read());
-    scope.set(1, motor2speed.read());
-    scope.set(2, position);
-
+    scope.set(0, reference2 - position2);
+    scope.set(1, position2);
+    scope.set(2, reference1 - position1);    
+    scope.set(3, position1);
     scope.send();
     
 }
 
-// PC Printf
-void PCPrintf()
-{
-    pc.printf("position = %f aantal degs = %f, count = %i\n",reference,position,count);
-}
-
 // Biquad filter
 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
 {
@@ -87,13 +88,13 @@
 // Motor2 control
 void motor2_Controller() 
 {
-    reference = m2_ref; // Setpoint
-    double pulses = Encoder.getPulses();
-    position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
-    double P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
+    reference2 = m2_ref; // Setpoint motor 2
+    position2 = Encoder2.getPulses()*360/(32*131); // Aantal Degs motor 2
+    double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
     m2_f_b0, m2_f_b1, m2_f_b2);
-    motor2speed = abs(P2); // Speed control
-    if(P2 > 0) // Direction control
+    double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2);
+    motor2speed = abs(m2_P2); // Speed control
+    if(m2_P2 > 0) // Direction control
     {  
         motor2direction = 0;
     }
@@ -101,9 +102,25 @@
     {
         motor2direction = 1;
     }
-    
+    reference1 = m1_ref; // Setpoint
+    position1 = Encoder1.getPulses()*360/(32*131); // Aantal Degs
+    double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
+    m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilerd met dezelfde coeffs als motor 2
+    double m1_P2 = biquad(m1_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilterd met dezelfde coeffs als motor 2
+    motor2speed = abs(m1_P2); // Speed control
+    if(m1_P2 > 0) // Direction control
+    {  
+        motor1direction = 0;
+    }
+    else
+    {
+        motor1direction = 1;
+    }
 }
 
+
+
+
 int main()
 {   
     LedR.write(1);
@@ -113,8 +130,8 @@
     pc.printf("Tot aan loop werkt\n");
     
     ScopeTime.attach_us(&ScopeSend, 10e4);
-    myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
-    PrintTime.attach(&PCPrintf,0.1f);
+    myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz
+
     while(true)
     {
         char c = pc.getc();
@@ -135,38 +152,58 @@
                     count = 0;
                 }
         }      
-        if(count == 0) //Motor 1 control
+        if(count == 0) //Motor 2 control, Groene LED
         {
                 
                 LedR = LedB = 1;
                 LedG = 0;
                 if(c == 'r')
                 {
-                    m2_ref = m2_ref + 5;
-                    if (m2_ref > 90)
+                    m2_ref = m2_ref + Stapgrootte;
+                    if (m2_ref > Grens2)
                     {
-                        m2_ref = 90;
+                        m2_ref = Grens2;
                     }
                 }
                 if(c == 'f')
                 {
-                    m2_ref = m2_ref - 5;
-                    if (m2_ref < -90)
+                    m2_ref = m2_ref - Stapgrootte;
+                    if (m2_ref < -1*Grens2)
                     {
-                        m2_ref = -90;
+                        m2_ref = -1*Grens2;
                     }
                 }
         }
-        if(count == 1) //Motor 2 control
+        if(count == 1) //Motor 1 control, Rode LED
         {
                 LedG = LedB = 1;
                 LedR = 0;
+                if(c == 't')
+                {
+                    m1_ref = m1_ref + Stapgrootte;
+                    if (m1_ref > Grens1)
+                    {
+                        m1_ref = Grens1;
+                    }
+                }
+                if(c == 'g')
+                {
+                    m1_ref = m1_ref - Stapgrootte;
+                    if (m1_ref < -1*Grens1)
+                    {
+                        m1_ref = -1*Grens1;
+                    }
+                }
         }
-        if(count == 2) //Vuur mechanisme
+        if(count == 2) //Vuur mechanisme & Reset, Blauwe LED
         {
 
                 LedR = LedG = 1;
-                LedB = 0;   
+                LedB = 0;
+                //VUUUR!
+                wait(1);
+                m2_ref = 0;
+                m1_ref = 0;   
         }
     }