Diff: main.cpp

Revision:

12:5ea8de059fa1

Parent:

11:a9f4f296d7ed

Child:

13:b35f3553f210

--- a/main.cpp	Wed Oct 29 11:01:55 2014 +0000
+++ b/main.cpp	Wed Oct 29 13:41:21 2014 +0000
@@ -6,11 +6,11 @@
 
 #define SAMP_TIME 0.01
 #define K_P (0.1)
-#define K_I (0.03 *SAMP_TIME)
-#define K_D (0.0005 /SAMP_TIME)
+#define K_I (0.04 *SAMP_TIME)
+#define K_D (0.0001 /SAMP_TIME)
 #define I_LIMIT 1.
 
-#define MAXENCO 300
+#define MAXENCO 256
 #define MINENCO 0
 
 
@@ -20,8 +20,7 @@
 DigitalOut m1_dir(PTA4);
 
 //define functions
-//void slam();
-void slam_II();
+void slam();
 void clamp(float * in, float min, float max);
 float pid(float rev_value, float mea_value);
 
@@ -37,87 +36,58 @@
     {
         cin >> y1;
         cout << y1 << endl;
-        //slam();
-        slam_II();
+        slam();
     }
 }
 
-/*void slam()
+void slam()
 {
-    float enca;
-    enca=encoderA.getPosition();
-    
-    //if (enca <= 0)
-    {
-        switch(y1)
-        {
-
-            case 1:
-                m1_dir=1;
-                m1_speed=0.33;
-                m1_speedout=m1_speed;
-                break;
-            case 2:
-                m1_dir=1;
-                m1_speed=0.66;
-                m1_speedout=m1_speed;
-                break;
-            case 3:
-                m1_dir=1;
-                m1_speed=1.0;
-                m1_speedout=m1_speed;
-                break;
-            default :
-                m1_speed=0;
-        }
-        while (enca < MAXENCO)
-        {
-            enca=encoderA.getPosition();
-            cout << enca << endl;
-        }
-        m1_speed=0;
-        m1_speedout=m1_speed;
-        m1_dir=0;
-        cout << "blieb!" << endl;
-        m1_speed=0.4;
-        m1_speedout=m1_speed;
-        while(enca > MINENCO)
-        {
-            enca=encoderA.getPosition();
-            cout << enca << endl;
-        }
-        m1_speed=0;
-        m1_speedout=m1_speed;
-    }
-}*/
-
-void slam_II()
-{
+    cout << "Ik begin met de slafunctie" << endl;
+    cout << encoderA.getPosition() << endl << "check" << endl;
     switch (y1)
     {
         case 1:
+            cout << "zaak1" << endl;
             m1_dir=1;
             max_speed=0.33;
-            m1_speed.write(0.33);
+            //m1_speed.write(0.33);
+            cout << "Motor 1 draait" << endl;
             break;
         case 2:
+            cout << "zaak2"<< endl;
             m1_dir=1;
             max_speed=0.66;
-            m1_speed.write(0.66);
+            //m1_speed.write(0.66);
+            cout << "Motor 1 draait" << endl;
             break;
         case 3:
+            cout << "zaak3"<< endl;
             m1_dir=1;
             max_speed=1;
-            m1_speed.write(1);
+            //m1_speed.write(1);
+            cout << "Motor 1 draait" << endl;
             break;
         default:
             m1_dir=1;
             m1_speed=0;
             break;
     }
+    cout << "Ik ben uit de switch" << endl;
     new_speed=pid(MAXENCO,encoderA.getPosition());
-    while(new_speed!=0)
+    clamp(&new_speed,-max_speed,max_speed);
+    if (new_speed>0)
+    {
+        m1_dir=1;
+    }
+    else
     {
+        m1_dir=0;
+    }
+    m1_speed.write(fabs(new_speed));
+
+    while(new_speed<-0.1 || new_speed>0.1)
+    {
+        //cout << "Eat, Sleep, Rave, Repeat" << endl;
         new_speed=pid(MAXENCO,encoderA.getPosition());
         clamp(&new_speed,-max_speed,max_speed);
         if (new_speed>0)
@@ -129,10 +99,23 @@
             m1_dir=0;
         }
         m1_speed.write(fabs(new_speed));
-        enca=encoderA.getPosition();
+        //cout << new_speed << endl;
     }
+    cout << "Ik ben uit de eerste whilelus" << endl;
+    cout << encoderA.getPosition() << endl << "check" << endl;
     new_speed=pid(MINENCO,encoderA.getPosition());
-    while(new_speed!=0)
+    clamp(&new_speed,-max_speed,max_speed);
+    if (new_speed>0)
+    {
+        m1_dir=1;
+    }
+    else
+    {
+        m1_dir=0;
+    }
+    m1_speed.write(fabs(new_speed));
+    
+    while(new_speed<-0.1 || new_speed>0.1)
     {
         new_speed=pid(MINENCO,encoderA.getPosition());
         clamp(&new_speed,-0.5,0.5);
@@ -146,6 +129,9 @@
         }
         m1_speed.write(fabs(new_speed));
     }
+    m1_speed.write(0);
+    cout << "Ik ben uit de tweede whilelus" << endl;
+    cout << encoderA.getPosition() << endl << "check" << endl;
 }
 
 void clamp(float * in, float min, float max)