Diff: main.cpp

Revision:

112:744fef64b8ba

Parent:

111:dc3d8ba48f83

--- a/main.cpp	Thu Jun 13 02:55:34 2019 +0000
+++ b/main.cpp	Sat Jun 29 14:31:52 2019 +0000
@@ -174,10 +174,10 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    motor.kp = 10.0;
-    motor.kd = 0.0;
-//    motor.ki = 5000;
-    motor.max = 125;
+    motor.kp = 12.9;
+    motor.kd = 2.0;
+    motor.ki = 6000;
+    motor.max = 180;
  
     DataComm = 0;
  
@@ -288,7 +288,9 @@
 }
  
 void tr_ready(void){
- 
+    unsigned int right_max = 0.8;
+    unsigned int left_max = 0.805;
+//    unsigned int sub = 0.5;
     while(1){
         // read current IR 1 ~ IR 5
         read_ir();
@@ -298,11 +300,17 @@
     
         // get current position
         get_pos();
-        pc.printf("pos: %d\r\n", pos);
-        pc.printf("on_line: %d\r\n\n", on_line);
-
-        tracing_pid();
-    
+        if(pos<1800){ //turn r
+            motor.speed_r(right_max);
+            motor.speed_l(left_max + 0.13);
+        }
+        else if(pos>2250){ //turn l
+            motor.speed_r(right_max + 0.18);
+            motor.speed_l(left_max - 0.1);
+        }
+        else{
+            tracing_pid();
+        }
     }
 }
  
@@ -392,9 +400,9 @@
  
     last_proportional  = proportional;
  
-    int power_difference = proportional / motor.kp+ derivative * motor.kd;
+    int power_difference = proportional / motor.kp+ integral/motor.ki + derivative * motor.kd;
 
-    const float std = 970.0;
+    const float std = 500.0;
  
     if(power_difference > motor.max)
         power_difference = motor.max;
@@ -403,12 +411,12 @@
         power_difference = -motor.max;
     
     // bot position: right
-    if(power_difference > 15){ //0.1 + p_d/std
+    if(power_difference > 10){ //0.1 + p_d/std
         motor.speed_r((motor.max + power_difference)/std);
         motor.speed_l((motor.max - power_difference)/std + 0.005);
     }
     // bot position: left
-    else if(power_difference < -15){
+    else if(power_difference < -10){
         motor.speed_r((motor.max + power_difference)/std);
         motor.speed_l((motor.max - power_difference)/std + 0.005);
     }