changed HEV accel params

AliMehrpour · Nov 17, 2015 · df113d3 · df113d3
1 parent eed7ee9
commit df113d3
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 39 deletions.
diff --git a/vehicle/zmp/autoware_socket/drv.cpp b/vehicle/zmp/autoware_socket/drv.cpp
@@ -31,6 +31,7 @@
 #include "mainwindow.h"
 #include "autoware_socket.h"
 #include <queue>
+#include "shm_data.h"
 
 double estimate_accel = 0.0;
 int target_accel_level = 0;
@@ -141,6 +142,8 @@ double _accel_stroke_pid_control(double current_velocity, double cmd_velocity)
     e_d = e - e_prev;
 
     accel_diff_sum += e;
+
+#if 0 // shouldn't we limit the cycles for I control?
     accel_diff_buffer.push(e);
     if (accel_diff_buffer.size() > _K_ACCEL_I_CYCLES) {
       double e_old = accel_diff_buffer.front();
@@ -150,6 +153,7 @@ double _accel_stroke_pid_control(double current_velocity, double cmd_velocity)
       }
       accel_diff_buffer.pop();
     }
+#endif
 
     if (accel_diff_sum > _ACCEL_MAX_I) {
       e_i = _ACCEL_MAX_I;
@@ -158,8 +162,11 @@ double _accel_stroke_pid_control(double current_velocity, double cmd_velocity)
       e_i = accel_diff_sum;
     }
 
-    target_accel_stroke = _K_ACCEL_P * e + _K_ACCEL_I * e_i + _K_ACCEL_D * e_d;
-    if (target_accel_stroke > _ACCEL_PEDAL_MAX) {
+    //  target_accel_stroke = _K_ACCEL_P * e + _K_ACCEL_I * e_i + _K_ACCEL_D * e_d;
+    printf("accel_p = %lf, accel_i = %lf, accel_d = %lf\n", shm_ptr->accel.P, shm_ptr->accel.I, shm_ptr->accel.D);
+    target_accel_stroke = shm_ptr->accel.P * e + shm_ptr->accel.I * e_i + shm_ptr->accel.D * e_d;
+
+ if (target_accel_stroke > _ACCEL_PEDAL_MAX) {
       target_accel_stroke = _ACCEL_PEDAL_MAX;
     }
     else if (target_accel_stroke < 0) {

diff --git a/vehicle/zmp/autoware_socket/str.cpp b/vehicle/zmp/autoware_socket/str.cpp
@@ -92,17 +92,17 @@ double _str_torque_pid_control(double current_steering_angle, double cmd_steerin
 
   e_d = (e - e_prev) / (STEERING_INTERNAL_PERIOD/1000.0);
 
-  double k_p = _K_STEERING_P;
-  double k_i = _K_STEERING_I;
-  double k_d = _K_STEERING_D;
+  double k_p = _K_STEERING_P_SLOW;
+  double k_i = _K_STEERING_I_SLOW;
+  double k_d = _K_STEERING_D_SLOW;
 
   double steering_max_torque = _STEERING_MAX_TORQUE;
 
   // change PID params depending on the driving speed.
-  if (vstate.velocity < 10) {
-    k_p = _K_STEERING_P_10;
-    k_i = _K_STEERING_I_10;
-    k_d = _K_STEERING_D_10;
+  if (vstate.velocity < 30) {
+    k_p = _K_STEERING_P_SLOW;
+    k_i = _K_STEERING_I_SLOW;
+    k_d = _K_STEERING_D_SLOW;
 
     // if angular velocity is slow, ignore D to smoothen the steer.
     if (fabs(e_d) < _STEERING_ANGVEL_BOUNDARY) {

diff --git a/vehicle/zmp/hv/patch/autoware_config.h b/vehicle/zmp/hv/patch/autoware_config.h
@@ -98,11 +98,11 @@
 #define STEERING_INTERNAL_PERIOD 20 // ms (10ms is too fast for HEV)
 
 // accel/brake parameters
-#define _K_ACCEL_P 30.0
-#define _K_ACCEL_I 5.0
+#define _K_ACCEL_P 40.0 // 30.0
+#define _K_ACCEL_I 0.1 // 5.0
 #define _K_ACCEL_D 2.0
-#define _K_ACCEL_I_CYCLES 100
-#define _ACCEL_MAX_I 600
+#define _K_ACCEL_I_CYCLES 1000
+#define _ACCEL_MAX_I 3000
 #define _ACCEL_STROKE_DELTA_MAX 1000
 #define _ACCEL_RELEASE_STEP 400
 #define _ACCEL_PEDAL_MAX 1700
@@ -121,35 +121,20 @@
 
 // steering parameters
 #define _STEERING_MAX_ANGVELSUM 1000
-#define _K_STEERING_TORQUE 10
-#define _K_STEERING_TORQUE_I 0.5
 #define _STEERING_MAX_TORQUE 2000
-#define _STEERING_MAX_SUM 100 //deg*0.1s for I control
+#define _STEERING_MAX_SUM 100
+#define _STEERING_ANGVEL_BOUNDARY 500.0 // deg/s
+#define _STEERING_IGNORE_ERROR 1.0 // deg
 
-// default params
+// fast PID factors
 #define _K_STEERING_P 60
-#define _K_STEERING_I 12
-#define _K_STEERING_D 10
-
-// when slower than 40km/h
-#define _K_STEERING_P_40 40
-#define _K_STEERING_I_40 10
-#define _K_STEERING_D_40 10
-
-// When slower than 30km/h
-#define _K_STEERING_P_30 30
-#define _K_STEERING_I_30 10 
-#define _K_STEERING_D_30 8
-
-// when slower than 20km/h
-#define _K_STEERING_P_20 25
-#define _K_STEERING_I_20 10
-#define _K_STEERING_D_20 5
-
-// when slower than 10km/h
-#define _K_STEERING_P_10 20
-#define _K_STEERING_I_10 10
-#define _K_STEERING_D_10 5
+#define _K_STEERING_I 5.0
+#define _K_STEERING_D 5.0
+
+// slow PID factors
+#define _K_STEERING_P_SLOW 6.0 // 6.0 // 6.0
+#define _K_STEERING_I_SLOW 6.8 // 7.0 // 5.0
+#define _K_STEERING_D_SLOW 6.1 // 6.0 // 5.0
 
 #define _STEERING_ANGLE_ERROR 0 // deg