|
@@ -2554,8 +2554,58 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
|
publish_rack_speed();
|
|
|
return;
|
|
|
}
|
|
|
+ double set_speed_over = 0;
|
|
|
+ double set_speed_over_front = 0;
|
|
|
+ double set_speed_over_back = 0;
|
|
|
+ double delta_over_value_max = 0;
|
|
|
+ double delta_over_value_min = 0;
|
|
|
+ if(standard_name == "CruiseControl") //定速巡航模式
|
|
|
+ {
|
|
|
+ delta_over_value_min = 0;
|
|
|
+ delta_over_value_max = 0;
|
|
|
+ }
|
|
|
+ else //规范文件模式
|
|
|
+ {
|
|
|
+ if(nedc_speed_count > 0)
|
|
|
+ {
|
|
|
+ set_speed_over = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ if(nedc_speed_count == nedc_speed_number)
|
|
|
+ {
|
|
|
+ set_speed_over_back = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ set_speed_over_back = nedc_speeds[nedc_speed_count];
|
|
|
+ }
|
|
|
+ if(nedc_speed_count -1 > 0)
|
|
|
+ {
|
|
|
+ set_speed_over_front = nedc_speeds[nedc_speed_count-2];
|
|
|
+ }
|
|
|
+ delta_over_value_max =
|
|
|
+ set_speed_over >= set_speed_over_front ? set_speed_over : set_speed_over_front;
|
|
|
+ delta_over_value_max =
|
|
|
+ delta_over_value_max >= set_speed_over_back ? delta_over_value_max : set_speed_over_back;
|
|
|
+
|
|
|
+ delta_over_value_min =
|
|
|
+ set_speed_over <= set_speed_over_front ? set_speed_over : set_speed_over_front;
|
|
|
+ delta_over_value_min =
|
|
|
+ delta_over_value_min <= set_speed_over_back ? delta_over_value_min : set_speed_over_back;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ double over_max = 0;
|
|
|
+ double over_min = 0;
|
|
|
+
|
|
|
+ over_max = over_threshold_value + delta_over_value_max;
|
|
|
+ over_min = delta_over_value_min - over_threshold_value;
|
|
|
+ if(over_min < 0)
|
|
|
+ {
|
|
|
+ over_min = 0;
|
|
|
+ }
|
|
|
|
|
|
- if(rack_set_speed - rack_actual_speed > over_threshold_value || rack_set_speed - rack_actual_speed < -over_threshold_value)
|
|
|
+ if(rack_set_speed - rack_actual_speed > over_max
|
|
|
+ || rack_set_speed - rack_actual_speed < over_min)
|
|
|
{
|
|
|
if(!over_speed_flag)
|
|
|
{
|