diff --git a/src/behaviors/followbwf.cpp b/src/behaviors/followbwf.cpp index 3aecf3d..2db7a5d 100644 --- a/src/behaviors/followbwf.cpp +++ b/src/behaviors/followbwf.cpp @@ -30,6 +30,7 @@ void FollowBWF::start() { obsticleCount = 0; lastObsticle = 0; lastOutside = millis(); + lastInside = millis(); } @@ -89,7 +90,9 @@ int FollowBWF::loop() { } controller->RunAsync(-FULL_SPEED, -FULL_SPEED, NORMAL_ACCELERATION_TIME); } - controller->TurnAngle(DOCKING_TURN_ANGLE_AFTER_BACK_UP); + // Turn toward the wire: CW = right turn, CCW = left turn + int turnAngle = BWF_CLOCKWISE ? DOCKING_TURN_ANGLE_AFTER_BACK_UP : -DOCKING_TURN_ANGLE_AFTER_BACK_UP; + controller->TurnAngle(turnAngle); return id(); }