Diff: main.cpp

Revision:

20:f5091e29cd26

Parent:

19:9417d2011e8b

Child:

21:cd7eb62183da

Child:

22:56c3a5918bfc

Child:

24:d0af4b2be295

--- a/main.cpp	Fri Oct 02 07:27:12 2015 +0000
+++ b/main.cpp	Fri Oct 02 17:22:17 2015 +0000
@@ -5,24 +5,45 @@
 #include "biquadFilter.h"
  
 Serial pc(USBTX, USBRX); // tx, rx
-DigitalIn Button(SW2);
+QEI Encoder(D3, D2, NC, 128);
+HIDScope scope(3);
+
+//Ledjes
 DigitalOut LedR(LED_RED);
 DigitalOut LedG(LED_GREEN);
 DigitalOut LedB(LED_BLUE);
+
+//Motor
 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
 PwmOut motor2speed(D5);
-AnalogIn potmeter2(A5);
-QEI Encoder(D3, D2, NC, 128);
-HIDScope scope(3);
 
+//Tickers
 Ticker ScopeTime;
 Ticker myControllerTicker;
+Ticker PrintTime;
 
+//Startwaarden
 double reference;
 double position;
 double m2_ref = 0;
 int count = 0;
 
+//Sample time (motor2-step)
+const double m2_Ts = 0.01;
+
+//Controller gain Motor 2
+const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
+double m2_err_int = 0, m2_prev_err = 0;
+
+//Derivative filter coeffs Motor 2
+const double BiGain = 0.016955;
+const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
+
+// Filter variables
+double m2_f_v1 = 0, m2_f_v2 = 0;
+
+
+//HIDScope
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
     scope.set(0, motor2direction.read());
@@ -32,19 +53,12 @@
     scope.send();
     
 }
-// Sample time (motor2-step)
-const double m2_Ts = 0.01;
 
-// Controller gain
-const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
-double m2_err_int = 0, m2_prev_err = 0;
-
-//Derivative filter coeffs
-const double BiGain = 0.016955;
-const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
-
-// Filter variables
-double m2_f_v1 = 0, m2_f_v2 = 0;
+// 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,24 +101,45 @@
     {
         motor2direction = 1;
     }
-    pc.printf("position = %f aantal degs = %f, pulses = %f\n",reference,position, pulses);
+    
 }
 
 int main()
 {   
+    LedR.write(1);
+    LedB.write(1);
+    LedG.write(1);
     pc.baud(115200);
     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);
     while(true)
-    {      
-       switch (count)
-       {
-           case (0):
-           {
-                LedR.write(0);
-                char c = pc.getc();
+    {
+        char c = pc.getc();
+        if(c == 'e') //Ga 1 programma omhoog
+        {
+            count = count + 1;
+            if(count > 2)
+                {
+                    count = 2;
+                }
+
+        }
+        if(c == 'd') //Ga 1 programma omlaag
+        {
+            count = count - 1;
+            if(count < 0)
+                {
+                    count = 0;
+                }
+        }      
+        if(count == 0) //Motor 1 control
+        {
+                
+                LedR = LedB = 1;
+                LedG = 0;
                 if(c == 'r')
                 {
                     m2_ref = m2_ref + 5;
@@ -121,22 +156,18 @@
                         m2_ref = -90;
                     }
                 }
-                break;
-            }
-            
-            case (1):
-            {
-                LedG.write(0);
-                // code voor het besturen van de 2e motor
-                break;
-            }
-            
-            default:
-            {
-                LedB.write(1);
-                break;
-            }
-        } 
+        }
+        if(count == 1) //Motor 2 control
+        {
+                LedG = LedB = 1;
+                LedR = 0;
+        }
+        if(count == 2) //Vuur mechanisme
+        {
+
+                LedR = LedG = 1;
+                LedB = 0;   
+        }
     }
 
 }
\ No newline at end of file