diff --git a/VOSS@0.1.0.zip b/VOSS@0.1.0.zip index ec9d125..327aa45 100644 Binary files a/VOSS@0.1.0.zip and b/VOSS@0.1.0.zip differ diff --git a/include/VOSS/controller/ArcPIDControllerBuilder.hpp b/include/VOSS/controller/ArcPIDControllerBuilder.hpp index 9edbdec..3df2015 100644 --- a/include/VOSS/controller/ArcPIDControllerBuilder.hpp +++ b/include/VOSS/controller/ArcPIDControllerBuilder.hpp @@ -18,7 +18,8 @@ class ArcPIDControllerBuilder { ArcPIDControllerBuilder& with_linear_constants(double kP, double kI, double kD); - ArcPIDControllerBuilder& with_angular_constants(double kP, double kI, double kD); + ArcPIDControllerBuilder& with_angular_constants(double kP, double kI, + double kD); /* * The track width is measured from your robot. * Due to turning scrub, you want to use a track width a few inches larger diff --git a/src/VOSS/controller/ArcPIDController.cpp b/src/VOSS/controller/ArcPIDController.cpp index a53bb5a..0d082d9 100644 --- a/src/VOSS/controller/ArcPIDController.cpp +++ b/src/VOSS/controller/ArcPIDController.cpp @@ -41,11 +41,8 @@ ArcPIDController::get_command(bool reverse, bool thru, if (std::isnan(arc_radius)) { arc_radius = t; - arc_center = { - current_pos.x + t * a, - current_pos.y + t * d - }; - //printf("arc center: %f %f\n", arc_center.x, arc_center.y); + arc_center = {current_pos.x + t * a, current_pos.y + t * d}; + // printf("arc center: %f %f\n", arc_center.x, arc_center.y); } double distance_error = sqrt(b * b + e * e); @@ -81,15 +78,18 @@ ArcPIDController::get_command(bool reverse, bool thru, // right_speed = (t + track_width / 2) / t * lin_speed; left_speed = lin_speed * (2 - track_width / arc_radius) / 2; right_speed = lin_speed * (2 + track_width / arc_radius) / 2; - double tangent = atan2(current_pos.y - arc_center.y, current_pos.x - arc_center.x); + double tangent = + atan2(current_pos.y - arc_center.y, current_pos.x - arc_center.x); if ((bool)(arc_radius > 0) != reverse) { tangent += M_PI_2; } else { tangent -= M_PI_2; } - //printf("current %f target %f\n", voss::to_degrees(current_angle), voss::to_degrees(tangent)); + // printf("current %f target %f\n", voss::to_degrees(current_angle), + // voss::to_degrees(tangent)); printf("x %f y %f\n", current_pos.x, current_pos.y); - double ang_speed = angular_pid.update(voss::norm_delta(tangent - current_angle)); + double ang_speed = + angular_pid.update(voss::norm_delta(tangent - current_angle)); left_speed -= ang_speed; right_speed += ang_speed; } else { diff --git a/src/VOSS/controller/ArcPIDControllerBuilder.cpp b/src/VOSS/controller/ArcPIDControllerBuilder.cpp index 5749c80..e0dfb5d 100644 --- a/src/VOSS/controller/ArcPIDControllerBuilder.cpp +++ b/src/VOSS/controller/ArcPIDControllerBuilder.cpp @@ -32,7 +32,8 @@ ArcPIDControllerBuilder::with_linear_constants(double kP, double kI, } ArcPIDControllerBuilder& -ArcPIDControllerBuilder::with_angular_constants(double kP, double kI, double kD) { +ArcPIDControllerBuilder::with_angular_constants(double kP, double kI, + double kD) { this->ctrl.angular_pid.set_constants(kP, kI, kD); return *this; } diff --git a/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp index 2e06f5d..38ab9a9 100644 --- a/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp @@ -5,14 +5,15 @@ namespace voss::controller { -ToleranceAngularExitCondition::ToleranceAngularExitCondition(double tolerance, double tolerance_time) +ToleranceAngularExitCondition::ToleranceAngularExitCondition( + double tolerance, double tolerance_time) : tolerance(voss::to_radians(tolerance)), tolerance_time(tolerance_time) { } bool ToleranceAngularExitCondition::is_met(Pose current_pose, bool thru) { if (std::abs(voss::norm_delta(current_pose.theta.value() - - this->target_pose.theta.value())) < - this->tolerance) { + this->target_pose.theta.value())) < + this->tolerance) { if (thru) { return true; } diff --git a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp index f0b9e36..c04d826 100644 --- a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp @@ -33,13 +33,15 @@ void ToleranceExitCondition::reset() { } } -void ToleranceExitCondition::add_ang_exit(double angular_tolerance, double tolerance_time) { - this->ang_exit = - std::make_shared(angular_tolerance, tolerance_time); +void ToleranceExitCondition::add_ang_exit(double angular_tolerance, + double tolerance_time) { + this->ang_exit = std::make_shared( + angular_tolerance, tolerance_time); } -void ToleranceExitCondition::add_lin_exit(double linear_tolerance, double tolerance_time) { - this->lin_exit = - std::make_shared(linear_tolerance, tolerance_time); +void ToleranceExitCondition::add_lin_exit(double linear_tolerance, + double tolerance_time) { + this->lin_exit = std::make_shared( + linear_tolerance, tolerance_time); } } // namespace voss::controller diff --git a/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp index 44ba373..36749ef 100644 --- a/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp @@ -5,7 +5,8 @@ namespace voss::controller { -ToleranceLinearExitCondition::ToleranceLinearExitCondition(double tolerance, double tolerance_time) +ToleranceLinearExitCondition::ToleranceLinearExitCondition( + double tolerance, double tolerance_time) : tolerance(tolerance), tolerance_time(tolerance_time) { }