diff --git a/cfg/FTCPlanner.cfg b/cfg/FTCPlanner.cfg
index 6c9528e..ba917c6 100755
--- a/cfg/FTCPlanner.cfg
+++ b/cfg/FTCPlanner.cfg
@@ -25,12 +25,18 @@ grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
 grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
 grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)
 grp_pid.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0)
+grp_pid.add("kp_ang_rotate", double_t, 0, "KP for angle in pre/post rotate", 1.0, 0.0, 50.0)
+grp_pid.add("ki_ang_rotate", double_t, 0, "KI for angle in pre/post rotate", 0.0, 0.0, 5.0)
+grp_pid.add("ki_ang_max_rotate", double_t, 0, "KI windup for angle in pre/post rotate", 10.0, 0.0, 500.0)
+grp_pid.add("kd_ang_rotate", double_t, 0, "KD for angle in pre/post rotate", 0.0, 0.0, 5.0)
 
 grp_bot = gen.add_group("Robot", type="tab") 
 grp_bot.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0)
 grp_bot.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0)
-grp_bot.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
-grp_bot.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
+grp_bot.add("max_goal_distance_error", double_t, 0, "If robot does not get closer than max_goal_distance_error to the goal by goal_timeout assume crashed", 1.0, 0.0, 3.0)
+grp_bot.add("target_goal_distance_error", double_t, 0, "Wait for robot to get closer than target_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
+grp_bot.add("max_goal_angle_error", double_t, 0, "If robot does not get a better angle than max_goal_angle_error by goal_timeout assume crashed", 10.0, 0.0, 360.0)
+grp_bot.add("target_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than target_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
 grp_bot.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0)
 grp_bot.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0)
 
diff --git a/src/ftc_planner.cpp b/src/ftc_planner.cpp
index 29bcc13..b36cc89 100644
--- a/src/ftc_planner.cpp
+++ b/src/ftc_planner.cpp
@@ -269,12 +269,12 @@ namespace ftc_local_planner
         break;
         case POST_ROTATE:
         {
-            if (time_in_current_state() > config.goal_timeout)
+            if (time_in_current_state() > config.goal_timeout && abs(angle_error) * (180.0 / M_PI) > config.max_goal_angle_error)
             {
                 ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal rotation. config.goal_timeout (" << config.goal_timeout << ") reached");
                 return FINISHED;
             }
-            if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error)
+            if (abs(angle_error) * (180.0 / M_PI) <= config.target_goal_angle_error)
             {
                 ROS_INFO_STREAM("FTCLocalPlannerROS: POST_ROTATE finished.");
                 return FINISHED;
@@ -451,13 +451,25 @@ namespace ftc_local_planner
         {
             i_lat_error = -config.ki_lat_max;
         }
-        if (i_angle_error > config.ki_ang_max)
-        {
-            i_angle_error = config.ki_ang_max;
+        if (current_state == FOLLOWING) {
+            if (i_angle_error > config.ki_ang_max)
+            {
+                i_angle_error = config.ki_ang_max;
+            }
+            else if (i_angle_error < -config.ki_ang_max)
+            {
+                i_angle_error = -config.ki_ang_max;
+            }
         }
-        else if (i_angle_error < -config.ki_ang_max)
-        {
-            i_angle_error = -config.ki_ang_max;
+        else {
+            if (i_angle_error > config.ki_ang_max_rotate)
+            {
+                i_angle_error = config.ki_ang_max_rotate;
+            }
+            else if (i_angle_error < -config.ki_ang_max_rotate)
+            {
+                i_angle_error = -config.ki_ang_max_rotate;
+            }
         }
 
         double d_lat = (lat_error - last_lat_error) / dt;
@@ -519,7 +531,7 @@ namespace ftc_local_planner
         }
         else
         {
-            double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang;
+            double ang_speed = angle_error * config.kp_ang_rotate + i_angle_error * config.ki_ang_rotate + d_angle * config.kd_ang_rotate;
             if (ang_speed > config.max_cmd_vel_ang)
             {
                 ang_speed = config.max_cmd_vel_ang;