diff --git a/include/VOSS/chassis/ChassisCommand.hpp b/include/VOSS/chassis/ChassisCommand.hpp index e4666b7e..598b3ab0 100644 --- a/include/VOSS/chassis/ChassisCommand.hpp +++ b/include/VOSS/chassis/ChassisCommand.hpp @@ -9,8 +9,12 @@ struct Voltages { double left; double right; }; +struct Chained { + double left; + double right; +}; -using ChassisCommand = std::variant; +using ChassisCommand = std::variant; template struct overload : Ts... { using Ts::operator()...; diff --git a/src/VOSS/chassis/DiffChassis.cpp b/src/VOSS/chassis/DiffChassis.cpp index c9527783..7e4ac67d 100644 --- a/src/VOSS/chassis/DiffChassis.cpp +++ b/src/VOSS/chassis/DiffChassis.cpp @@ -71,7 +71,25 @@ bool DiffChassis::execute(ChassisCommand cmd, double max) { this->prev_voltages = v; return false; - }}, + }, + [this, max](Chained& v){ + if (fabs(v.left) > max) { + v.left = max * ((v.left < 0) ? -1 : 1); + } + if (fabs(v.right) > max) { + v.right = max * ((v.right < 0) ? -1 : 1); + } + + v.left = slew(v.left, true); + v.right = slew(v.right, false); + + this->left_motors->move_voltage(120 * v.left); + this->right_motors->move_voltage(120 * v.right); + + this->prev_voltages = {v.left, v.right}; + + return true; + }}, cmd); } diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp index 9b8aa958..9eec68fe 100644 --- a/src/VOSS/controller/PIDController.cpp +++ b/src/VOSS/controller/PIDController.cpp @@ -13,7 +13,7 @@ PIDController::PIDController(std::shared_ptr l) chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) { Point current_pos = this->l->get_position(); - + bool chainedExecutable = false; bool noPose = this->target.theta == 361; double dx = target.x - current_pos.x; @@ -29,23 +29,26 @@ chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) { angle_error = voss::norm_delta(angle_error); - if (distance_error <= exit_error || (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) { - total_lin_err = 0; - close += 10; - } else { - close = 0; - } + double lin_speed; + + + + if (distance_error <= exit_error || (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) { + if(thru) { + chainedExecutable = true; + } else { + total_lin_err = 0; + close += 10; + } + } else { + close = 0; + } if (close > settle_time) { return chassis::ChassisCommand{chassis::Stop{}}; } - double lin_speed; - if (thru) { - lin_speed = 100; - } else { - lin_speed = linear_pid(distance_error) * (reverse ? -1 : 1); - } + lin_speed = thru ? 100.0 : (linear_pid(distance_error) * (reverse ? -1 : 1)); double ang_speed; if (distance_error < min_error) { @@ -74,9 +77,13 @@ chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) { ang_speed = angular_pid(angle_error); } + if(chainedExecutable){ + return chassis::ChassisCommand{ + chassis::Chained{100.0 - ang_speed, 100.0 + ang_speed}}; + } - return chassis::ChassisCommand{ - chassis::Voltages{lin_speed - ang_speed, lin_speed + ang_speed}}; + return chassis::ChassisCommand{ + chassis::Voltages{lin_speed - ang_speed, lin_speed + ang_speed}}; } chassis::ChassisCommand PIDController::get_angular_command(bool reverse,