diff --git a/HalloRobot/libDIY/include/diy_gparser.h b/HalloRobot/libDIY/include/diy_gparser.h
new file mode 100644
index 0000000000000000000000000000000000000000..128e1a26407cff13ecdc5d4579169df66ca4fc17
--- /dev/null
+++ b/HalloRobot/libDIY/include/diy_gparser.h
@@ -0,0 +1,12 @@
+#ifndef DIY_GPARSER_H_INCLUDED
+#define DIY_GPARSER_H_INCLUDED
+
+
+typedef struct {
+    uint8_t code;
+    uint16_t feed_speed;
+    uint16_t x_pos;
+}g_code_t;
+
+uint8_t diy_parse_g_code(char* command_string, g_code_t* g_code);
+#endif // DIY_GPARSER_H_INCLUDED
diff --git a/HalloRobot/libDIY/src/diy_gparser.c b/HalloRobot/libDIY/src/diy_gparser.c
new file mode 100644
index 0000000000000000000000000000000000000000..04c4e50e5bc8807e0e6682cc49cf34d262872074
--- /dev/null
+++ b/HalloRobot/libDIY/src/diy_gparser.c
@@ -0,0 +1,6 @@
+#include "diy_parser.h"
+
+
+uint8_t diy_parse_g_code(char* command_string, g_code_t* g_code){
+}
+
diff --git a/HalloRobot/libProject/src/control.c b/HalloRobot/libProject/src/control.c
index 98c96eb47fbf32ea132c4ff8817042903d114290..fd2d1af3c68266d4332fb449045878556d96bf18 100644
--- a/HalloRobot/libProject/src/control.c
+++ b/HalloRobot/libProject/src/control.c
@@ -68,9 +68,13 @@ float pi_control(float e)
  */
 float pid_control(float e)
 {
-    const float K = 0.8f;
-    const float T_i = 0.5f;//0.18
+    float K = 0.8f;
+    const float T_i = 0.1f;//0.18
     const float T_d = 0.05;
+   
+    if(e > 2 || e < -2){
+        K = 5.0;
+    }
 
     static float e_old = 0;
 
diff --git a/HalloRobot/main.c b/HalloRobot/main.c
index abea84da66136edf05579e7cd7f04b967a57a39a..84c28dc90f1ba3fe36df88b6266216f662e9e6ed 100644
--- a/HalloRobot/main.c
+++ b/HalloRobot/main.c
@@ -181,7 +181,8 @@ void control_thread(cyg_addrword_t arg)
     while(1){
         error = null_offset_pos - phi_cor;
         motor_signal = control_func_ptr(error);
-        null_of/\\\////zs_printf("%f,%f,%f\n",null_offset_pos,phi_cor,motor_signal);
+        null_offset_pos = offset_control(motor_signal);
+        ezs_printf("%f,%f,%f,%f\n",null_offset_pos,phi_cor,error,motor_signal);
         motor_pwm(motor_signal);
 #ifdef WCET
         uint32_t current_run = ezs_watch_stop(&time_sample);