Diff: main.cpp

Revision:

13:b35f3553f210

Parent:

12:5ea8de059fa1

Child:

14:e9ab81429271

--- a/main.cpp	Wed Oct 29 13:41:21 2014 +0000
+++ b/main.cpp	Wed Oct 29 15:36:59 2014 +0000
@@ -5,12 +5,12 @@
 #include <iostream>
 
 #define SAMP_TIME 0.01
-#define K_P (0.1)
-#define K_I (0.04 *SAMP_TIME)
-#define K_D (0.0001 /SAMP_TIME)
+#define K_P (0.001)
+#define K_I (0.000001 *SAMP_TIME)
+#define K_D (0.000005 /SAMP_TIME)
 #define I_LIMIT 1.
 
-#define MAXENCO 256
+#define MAXENCO 320
 #define MINENCO 0
 
 
@@ -20,31 +20,40 @@
 DigitalOut m1_dir(PTA4);
 
 //define functions
-void slam();
+void slam(int emg_value);
 void clamp(float * in, float min, float max);
 float pid(float rev_value, float mea_value);
 
 //define global variables
-int y1;
+//int y1;
 float new_speed;
 float max_speed;
 float enca;
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
 
 int main()
 {
+    int y1;
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,SAMP_TIME);
     while(1)
     {
         cin >> y1;
         cout << y1 << endl;
-        slam();
+        slam(y1);
     }
 }
 
-void slam()
+void slam(int emg_value)
 {
     cout << "Ik begin met de slafunctie" << endl;
     cout << encoderA.getPosition() << endl << "check" << endl;
-    switch (y1)
+    switch (emg_value)
     {
         case 1:
             cout << "zaak1" << endl;
@@ -73,13 +82,15 @@
             break;
     }
     cout << "Ik ben uit de switch" << endl;
+    while(!looptimerflag);
+    looptimerflag=false;
     new_speed=pid(MAXENCO,encoderA.getPosition());
     clamp(&new_speed,-max_speed,max_speed);
     if (new_speed>0)
     {
         m1_dir=1;
     }
-    else
+    else if (new_speed<0)
     {
         m1_dir=0;
     }
@@ -87,6 +98,8 @@
 
     while(new_speed<-0.1 || new_speed>0.1)
     {
+        while(!looptimerflag);
+        looptimerflag=false;
         //cout << "Eat, Sleep, Rave, Repeat" << endl;
         new_speed=pid(MAXENCO,encoderA.getPosition());
         clamp(&new_speed,-max_speed,max_speed);
@@ -94,7 +107,7 @@
         {
             m1_dir=1;
         }
-        else
+        else if (new_speed<0)
         {
             m1_dir=0;
         }
@@ -103,13 +116,15 @@
     }
     cout << "Ik ben uit de eerste whilelus" << endl;
     cout << encoderA.getPosition() << endl << "check" << endl;
+    while(!looptimerflag);
+    looptimerflag=false;
     new_speed=pid(MINENCO,encoderA.getPosition());
     clamp(&new_speed,-max_speed,max_speed);
     if (new_speed>0)
     {
         m1_dir=1;
     }
-    else
+    else if (new_speed<0)
     {
         m1_dir=0;
     }
@@ -117,13 +132,15 @@
     
     while(new_speed<-0.1 || new_speed>0.1)
     {
+        while(!looptimerflag);
+        looptimerflag=false;
         new_speed=pid(MINENCO,encoderA.getPosition());
         clamp(&new_speed,-0.5,0.5);
         if (new_speed>0)
         {
             m1_dir=1;    
         }
-        else
+        else if (new_speed<0)
         {
             m1_dir=0;
         }