Diff: main.cpp

Revision:

1:d0c9519c70eb

Parent:

0:74ea99c4db88

--- a/main.cpp	Thu Feb 23 12:35:53 2017 +0000
+++ b/main.cpp	Fri Mar 24 04:36:58 2017 +0000
@@ -29,7 +29,7 @@
 void init_PWM();
 
 // servo motor
-float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
+float servo_duty = 0.069;  // 0.069 +(0.088/180)*angle, -90<angle<90
 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
 int angle = 0;
 
@@ -59,6 +59,9 @@
 float err_2 = 0.0;
 float ierr_2 = 0.0;
 
+float Kp = 0.006;
+float Ki = 0.02;
+
 int main()
 {
     init_TIMER();
@@ -107,9 +110,36 @@
     HallB_1_state = HallB.read();
 
     ///code for state determination///
-
-
-
+    state_1 = 10*HallA_1_state + HallB_1_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
+   
+   if(state_1_old != state_1)
+   { 
+        if((state_1_old/10) == (state_1_old%10))
+        {
+            if((state_1%10) != (state_1_old%10))
+            {
+                speed_count_1++;
+            }
+            else if((state_1/10) != (state_1_old/10))
+            {
+                speed_count_1--;
+            }
+        }
+        else if((state_1_old/10) != (state_1_old%10))
+        {
+            if((state_1%10) != (state_1_old%10))
+            {
+                speed_count_1--;
+            }
+            else if((state_1/10) != (state_1_old/10))
+            {
+                speed_count_1++;
+            }    
+        }
+    
+        state_1_old = state_1;
+    }
+    
     //////////////////////////////////
 
     //forward : speed_count_1 + 1
@@ -121,9 +151,35 @@
     HallB_2_state = HallB_2.read();
 
     ///code for state determination///
-
-
-
+    state_2 = 10*HallA_2_state + HallB_2_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
+    
+    if(state_2_old != state_2)
+    {
+        if((state_2_old/10) == (state_2_old%10))
+        {
+            if((state_2%10) != (state_2_old%10))
+            {
+                speed_count_2++;
+            }
+            else if((state_2/10) != (state_2_old/10))
+            {
+                speed_count_2--;
+            }
+        }
+        else if((state_2_old/10) != (state_2_old%10))
+        {
+            if((state_2%10) != (state_2_old%10))
+            {
+                speed_count_2--;
+            }
+            else if((state_2/10) != (state_2_old/10))
+            {
+                speed_count_2++;
+            }    
+        }
+    
+        state_2_old = state_2;
+    }
     //////////////////////////////////
 
     //forward : speed_count_2 + 1
@@ -135,7 +191,7 @@
     // servo motor
     ///code for sevo motor///
     
-    
+    servo_duty += 11.0f/1500.0f;
     
     /////////////////////////
     
@@ -148,8 +204,10 @@
     speed_count_1 = 0;
 
     ///PI controller for motor1///
-    
-    
+    rotation_speed_ref_1 = 150;
+    err_1 = rotation_speed_ref_1 - rotation_speed_1;
+    ierr_1 = ierr_1 + err_1 * Ts;
+    PI_out_1 = Kp * err_1 + Ki * ierr_1;
     
     //////////////////////////////
     
@@ -164,7 +222,10 @@
     speed_count_2 = 0;
 
     ///PI controller for motor2///
-    
+    rotation_speed_ref_2 = 150;
+    err_2 = rotation_speed_ref_2 - rotation_speed_2;
+    ierr_2 = ierr_2 + err_2 * Ts;
+    PI_out_2 = Kp * err_2 + Ki * ierr_2;
     
     
     //////////////////////////////