Diff: main.cpp

Revision:

6:3c4a2afb11e5

Parent:

5:daf561abb67d

Child:

7:36a7a7136be3

--- a/main.cpp	Wed Oct 29 10:05:43 2014 +0000
+++ b/main.cpp	Wed Oct 29 10:36:41 2014 +0000
@@ -4,9 +4,10 @@
 #include "encoder.h"
 #include <iostream>
 
-#define K_P 0
-#define K_I 0
-#define K_D 0
+#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 I_LIMIT 1.
 
 #define MAXENCO 300
@@ -15,17 +16,17 @@
 
 //define in and output
 Encoder encoderA(PTD0,PTD2);
-PwmOut m1_speedout(PTA5);
+PwmOut m1_speed(PTA5);
 DigitalOut m1_dir(PTA4);
 
 //define functions
-void slam();
+//void slam();
+void slam_II();
 void clamp(float * in, float min, float max);
 float pid(float rev_value, float mea_value);
 
 //define global variables
 int y1;
-float m1_speed;
 
 int main()
 {
@@ -33,11 +34,12 @@
     {
         cin >> y1;
         cout << y1 << endl;
-        slam();
+        //slam();
+        slam_II();
     }
 }
 
-void slam()
+/*void slam()
 {
     float enca;
     enca=encoderA.getPosition();
@@ -84,6 +86,66 @@
         m1_speed=0;
         m1_speedout=m1_speed;
     }
+}*/
+
+void slam_II()
+{
+    float new_speed;
+    float max_speed;
+    float enca;
+    switch (y1)
+    {
+        case 1:
+            m1_dir=1;
+            max_speed=0.33;
+            m1_speed.write(0.33);
+            return;
+        case 2:
+            m1_dir=1;
+            max_speed=0.66;
+            m1_speed.write(0.66);
+            return;
+        case 3:
+            m1_dir=1;
+            max_speed=1;
+            m1_speed.write(1);
+            return;
+        default:
+            m1_dir=1;
+            m1_speed=0;
+            return;
+    }
+    enca=encoderA.getPosition();
+    while(enca!=MAXENCO)
+    {
+        new_speed=pid(MAXENCO,encoderA.getPosition());
+        clamp(&new_speed,-max_speed,max_speed);
+        if (new_speed>0)
+        {
+            m1_dir=1;
+        }
+        else
+        {
+            m1_dir=0;
+        }
+        m1_speed.write(fabs(new_speed));
+        enca=encoderA.getPosition();
+    }
+    while(enca!=MINENCO)
+    {
+        new_speed=pid(MINENCO,encoderA.getPosition());
+        clamp(&new_speed,-0.5,0.5);
+        if (new_speed>0)
+        {
+            m1_dir=1;    
+        }
+        else
+        {
+            m1_dir=0;
+        }
+        m1_speed.write(fabs(new_speed));
+        enca=encoderA.getPosition();
+    }
 }
 
 void clamp(float * in, float min, float max)