Diff: main.cpp

Revision:

1:a010e434a360

Parent:

0:25a2e7ea29f3

Child:

2:f3e8a27d376c

--- a/main.cpp	Fri Oct 31 11:28:10 2014 +0000
+++ b/main.cpp	Fri Oct 31 12:03:55 2014 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
-#include <iostream>
+
 #include "encoder.h"
 #define K_P (0.1)
 #define K_I (0.1)
@@ -22,6 +22,11 @@
 float s_speed2;
 float s_dir2;
 
+//lampjes
+
+DigitalOut rood(LED1);
+DigitalOut blauw(LED3);
+DigitalOut groen(LED2);
 // Alle constantes voor de filters definiëren
 // Constantes voor de Low Pass filter
 #define A1LP    0.018180963222803
@@ -270,6 +275,8 @@
     if (y1>0 && y2>0 && check>0) {
         badjedone=1;
         check=0;
+        rood = 1;
+        groen = 0;
     } else if (y1>0 && y2>0) {
         check=1;
     } else if (y1>0) {
@@ -294,6 +301,8 @@
     if (y1>0 && y2>0 && check>0) {
         speeddone=1;
         check=0;
+        rood = 0;
+        groen = 1;
     } else if (y1>0 && y2>0) {
         check=1;
     } else if (y1>0) {
@@ -343,6 +352,9 @@
 // Main code
 int main()
 {
+    rood = 0;
+    blauw = 1;
+    groen = 1;
     // Startcondities
     pc.baud(115200);
     timer.attach(setlooptimerflag,TSAMP);
@@ -378,9 +390,9 @@
             // Als de hoek voor het badje nog niet klaar is, zet badje in de huidig aangegeven stand
             if (badjedone==0) {
                 badjestand=badje(y1,y2);
-                cout<<"ben voor batjes"<<endl;
+                
                 batposition(badjestand);
-                cout<<"batjes gedaan"<<endl;
+              
                 // Als de hoek voor het badje wel klaar is:
             } else if (badjedone==1) {
                 // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt