diff --git a/VOSS@0.1.3.zip b/VOSS@0.1.3.zip index 2197c08..d80dd27 100644 Binary files a/VOSS@0.1.3.zip and b/VOSS@0.1.3.zip differ diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index 7b12d6c..9b6e402 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -28,16 +28,21 @@ BoomerangController::get_command(bool reverse, bool thru, double dy = target.y - current_pos.y; double distance_error = sqrt(dx * dx + dy * dy); - double at = voss::to_radians(target.theta.value()); + double at = target.theta.value(); this->carrotPoint = {this->target.x - distance_error * cos(at) * lead_pct, this->target.y - distance_error * sin(at) * lead_pct, target.theta}; - double current_angle = - this->l->get_orientation_rad() + (reverse ? M_PI : 0); + dx = carrotPoint.x - current_pos.x; + dy = carrotPoint.y - current_pos.y; + double current_angle = this->l->get_orientation_rad(); double angle_error; - angle_error = atan2(dy, dx) - current_angle; + if (!reverse) { + angle_error = atan2(dy, dx) - current_angle; + } else { + angle_error = atan2(-dy, -dx) - current_angle; + } angle_error = voss::norm_delta(angle_error);