speed null fehler gelost

lalten · Jan 28, 2016 · ada7b46 · ada7b46
1 parent 2172268
commit ada7b46
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/tas_autonomous_control/src/control/lqr.cpp b/tas_autonomous_control/src/control/lqr.cpp
@@ -112,8 +112,8 @@ double lqr::control()
     if(cmd_thrust < -1)
         cmd_thrust=-1;
 
-    //publish_car();
-    //publish_sim();
+    publish_car();
+    publish_sim();
 }
 
 void lqr::getclosestpoint()
@@ -650,11 +650,14 @@ void lqr::publish_car()
     double backward_treshold = 1488;
 
     geometry_msgs::Vector3 control_servo;
-    if (cmd_thrust >= 0 )
+    if (cmd_thrust > 0 )
         control_servo.x = forward_treshold + addition*cmd_thrust;
     if (cmd_thrust < 0 )
         control_servo.x = backward_treshold + addition*cmd_thrust;
 
+    if(fabs(cmd_thrust)==0)
+        control_servo.x = 1500;
+
     control_servo.y = cmd_steeringAngle;
 
     ROS_INFO_STREAM("servo x " << control_servo.x << "servo y " << control_servo.y);