Diff: main.cpp

Revision:

19:9417d2011e8b

Parent:

18:6f71bb91b8bd

--- a/main.cpp	Thu Oct 01 15:46:34 2015 +0000
+++ b/main.cpp	Fri Oct 02 07:27:12 2015 +0000
@@ -5,6 +5,10 @@
 #include "biquadFilter.h"
  
 Serial pc(USBTX, USBRX); // tx, rx
+DigitalIn Button(SW2);
+DigitalOut LedR(LED_RED);
+DigitalOut LedG(LED_GREEN);
+DigitalOut LedB(LED_BLUE);
 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
 PwmOut motor2speed(D5);
 AnalogIn potmeter2(A5);
@@ -17,6 +21,7 @@
 double reference;
 double position;
 double m2_ref = 0;
+int count = 0;
 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
@@ -70,7 +75,7 @@
 {
     reference = m2_ref; // Setpoint
     double pulses = Encoder.getPulses();
-    position = pulses*360/(0.5*128*131); // Aantal Degs
+    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, 
     m2_f_b0, m2_f_b1, m2_f_b2);
     motor2speed = abs(P2); // Speed control
@@ -82,7 +87,7 @@
     {
         motor2direction = 1;
     }
-    pc.printf("position = %f aantal degs = %f, pulses = %d\n",reference,position, pulses);
+    pc.printf("position = %f aantal degs = %f, pulses = %f\n",reference,position, pulses);
 }
 
 int main()
@@ -93,24 +98,45 @@
     ScopeTime.attach_us(&ScopeSend, 10e4);
     myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
     while(true)
-    {
-       char c = pc.getc();
-       if(c == 'r')
-        {
-           m2_ref = m2_ref + 5;
-           if (m2_ref > 90)
+    {      
+       switch (count)
+       {
+           case (0):
            {
-               m2_ref = 90;
+                LedR.write(0);
+                char c = pc.getc();
+                if(c == 'r')
+                {
+                    m2_ref = m2_ref + 5;
+                    if (m2_ref > 90)
+                    {
+                        m2_ref = 90;
+                    }
+                }
+                if(c == 'f')
+                {
+                    m2_ref = m2_ref - 5;
+                    if (m2_ref < -90)
+                    {
+                        m2_ref = -90;
+                    }
+                }
+                break;
             }
-        }
-        if(c == 'f')
-        {
-            m2_ref = m2_ref - 5;
-            if (m2_ref < -90)
+            
+            case (1):
             {
-                m2_ref = -90;
+                LedG.write(0);
+                // code voor het besturen van de 2e motor
+                break;
             }
-        }
+            
+            default:
+            {
+                LedB.write(1);
+                break;
+            }
+        } 
     }
 
 }
\ No newline at end of file