Diff: main.cpp

Revision:

8:05ac8875cb1f

Parent:

7:f7d503690ea5

--- a/main.cpp	Wed Apr 13 09:32:58 2016 +0000
+++ b/main.cpp	Wed Apr 13 10:26:17 2016 +0000
@@ -4,8 +4,8 @@
 //The number will be compiled as type "double" in default
 //Add a "f" after the number can make it compiled as type "float"
 #define Ts 0.01f    //period of timer1 (s)
-#define Kp 0.05f
-#define Ki 0.001f
+#define Kp 0.005f
+#define Ki 0.0001f
 
 Serial bluetooth(D10,D2); //宣告藍牙腳位
 Serial pc(D1, D0);
@@ -43,6 +43,9 @@
 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
 float v2 = 0.0, v2_ref = 0.0;
 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
+char speedCommand;
+int timer_count = 0;
+char str[100];
 
 int main() {
     
@@ -68,33 +71,33 @@
             if (speedCommand == 'a')
             { 
                 v1_ref = v1_ref + 10;
-                v2_ref = v2_ref + 10;
+                v2_ref = v2_ref - 10;
             }
             else if (speedCommand == 's')
             {
                 v1_ref = v1_ref - 10;
-                v2_ref = v2_ref - 10;
+                v2_ref = v2_ref + 10;
             }
             else if (speedCommand == 'q')
             {
-                v1_ref = v1_ref + 20;
-                v2_ref = v2_ref + 20;
+                v1_ref = v1_ref + 30;
+                v2_ref = v2_ref - 30;
             }
             else if (speedCommand == 'w')
             {
-                v1_ref = v1_ref - 20;
-                v2_ref = v2_ref - 20;
+                v1_ref = v1_ref - 30;
+                v2_ref = v2_ref + 30;
             }
             
-            if (v1_ref < -100)
-                v1_ref = -100;
-            else if (v1_ref > 100)
-                v1_ref = 100;
+            if (v1_ref < -300)
+                v1_ref = -300;
+            else if (v1_ref > 300)
+                v1_ref = 300;
                 
-            if (v2_ref < -100)
-                v2_ref = -100;
-            else if (v2_ref > 100)
-                v2_ref = 100;
+            if (v2_ref < -300)
+                v2_ref = -300;
+            else if (v2_ref > 300)
+                v2_ref = 300;
         }
     }
 }
@@ -134,6 +137,14 @@
     TIM1->CCER |= 0x40;
     
     PIout_2_old = PIout_2;
+    
+    /*timer_count++;
+    //send speed
+    if(timer_count == 50)
+    {
+        bluetooth.printf(str, "motor1: %f, motor2: %f\n", v1, v2);
+        timer_count = 0;
+    }*/
 }
 
 void CN_interrupt(void)
@@ -190,9 +201,9 @@
     //Forward: v2Count +1
     //Inverse: v2Count -1
     if ( (state_2 == (state_2_old + 1)) || (state_2 == (state_2_old - 3)) )
-        v1Count++;
+        v2Count++;
     else if ( (state_2 == (state_2_old - 1)) || (state_2  == (state_2_old + 3)))
-        v1Count--;
+        v2Count--;
 }
 
 void init_TIMER(void)