diff --git a/src/VOSS/chassis/DiffChassis.cpp b/src/VOSS/chassis/DiffChassis.cpp index e9f7c6f4..7c424957 100644 --- a/src/VOSS/chassis/DiffChassis.cpp +++ b/src/VOSS/chassis/DiffChassis.cpp @@ -63,80 +63,81 @@ void DiffChassis::set_brake_mode(pros::motor_brake_mode_e mode) { // constructor, returns true if movement is complete bool DiffChassis::execute(DiffChassisCommand cmd, double max) { return std::visit( - overload{ - [this](Stop&) -> bool { - this->set_brake_mode(this->brakeMode); - this->left_motors->brake(); - this->right_motors->brake(); - - return true; - }, - [this, max](diff_commands::Voltages& v) -> bool { - double v_max = std::max(fabs(v.left), fabs(v.right)); - if (v_max > max) { - v.left = v.left * max / v_max; - v.right = v.right * max / v_max; - } - - 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; - - return false; - }, - // Logic allowing for individual movements within a chain of - // movements to be registered at completed even though robot may - // still be moving - [this, max](diff_commands::Chained& v) -> bool { - double v_max = std::max(fabs(v.left), fabs(v.right)); - if (v_max > max) { - v.left = v.left * max / v_max; - v.right = v.right * max / v_max; - } - - 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; - }, - // Logic to brake one side of the drive alloing for a turn around - // the side of the robot and returning true when the turn is - // finished - [this, max](diff_commands::Swing& v) { - double v_max = std::max(fabs(v.left), fabs(v.right)); - if (v.right == 0) { - this->right_motors->set_brake_mode_all(pros::MotorBrake::hold); - this->right_motors->brake(); - if (v_max > max) { - v.left = v.left * max / v_max; - } - v.left = slew(v.left, true); - this->left_motors->move_voltage(120 * v.left); - this->prev_voltages = {v.left, 0.0}; - - } else if (v.left == 0) { - this->left_motors->set_brake_mode_all(pros::MotorBrake::hold); - this->left_motors->brake(); - if (v_max > max) { - v.right = v.right * max / v_max; - } - v.right = slew(v.right, false); - - this->right_motors->move_voltage(120 * v.right); - this->prev_voltages = {0.0, v.right}; - } - - return false; - }}, + overload{[this](Stop&) -> bool { + this->set_brake_mode(this->brakeMode); + this->left_motors->brake(); + this->right_motors->brake(); + + return true; + }, + [this, max](diff_commands::Voltages& v) -> bool { + double v_max = std::max(fabs(v.left), fabs(v.right)); + if (v_max > max) { + v.left = v.left * max / v_max; + v.right = v.right * max / v_max; + } + + 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; + + return false; + }, + // Logic allowing for individual movements within a chain of + // movements to be registered at completed even though robot + // may still be moving + [this, max](diff_commands::Chained& v) -> bool { + double v_max = std::max(fabs(v.left), fabs(v.right)); + if (v_max > max) { + v.left = v.left * max / v_max; + v.right = v.right * max / v_max; + } + + 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; + }, + // Logic to brake one side of the drive alloing for a turn + // around the side of the robot and returning true when the + // turn is finished + [this, max](diff_commands::Swing& v) { + double v_max = std::max(fabs(v.left), fabs(v.right)); + if (v.right == 0) { + this->right_motors->set_brake_mode_all( + pros::MotorBrake::hold); + this->right_motors->brake(); + if (v_max > max) { + v.left = v.left * max / v_max; + } + v.left = slew(v.left, true); + this->left_motors->move_voltage(120 * v.left); + this->prev_voltages = {v.left, 0.0}; + + } else if (v.left == 0) { + this->left_motors->set_brake_mode_all( + pros::MotorBrake::hold); + this->left_motors->brake(); + if (v_max > max) { + v.right = v.right * max / v_max; + } + v.right = slew(v.right, false); + + this->right_motors->move_voltage(120 * v.right); + this->prev_voltages = {0.0, v.right}; + } + + return false; + }}, cmd); }