diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 555b92db..cd622497 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/PID.hpp" #include "EZ-Template/util.hpp" +#include "okapi/api/units/QLength.hpp" #include "pros/motors.h" using namespace ez; @@ -414,7 +415,12 @@ class Drive { /** * The position of the right motor. */ - int right_sensor(); + double right_sensor(); + + /** + * The position of the right motor. + */ + int raw_right_sensor(); /** * The velocity of the right motor. @@ -434,7 +440,12 @@ class Drive { /** * The position of the left motor. */ - int left_sensor(); + double left_sensor(); + + /** + * The position of the left motor. + */ + int raw_left_sensor(); /** * The velocity of the left motor. @@ -507,7 +518,7 @@ class Drive { * \param toggle_heading * toggle for heading correction */ - void set_drive_pid(double target, int speed, bool slew_on = false, bool toggle_heading = true); + void set_drive_pid(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true); /** * Sets the robot to turn using PID. @@ -554,6 +565,14 @@ class Drive { */ void wait_until(double target); + /** + * Lock the code in a while loop until this position has passed for driving with okapi units. + * + * \param target + * for driving, using okapi units + */ + void wait_until(okapi::QLength target); + /** * Autonomous interference detection. Returns true when interfered, and false when nothing happened. */ @@ -673,12 +692,12 @@ class Drive { /** * Sets minimum slew distance constants. * - * \param fw - * minimum distance for forward drive pd - * \param bw - * minimum distance for backwards drive pd + * \param fwd + * minimum distance for forward drive pd, okapi unit + * \param rev + * minimum distance for backwards drive pd, okapi unit */ - void set_slew_distance(int fwd, int rev); + void set_slew_distance(okapi::QLength fwd, okapi::QLength rev); /** * Set's constants for drive exit conditions. @@ -686,15 +705,15 @@ class Drive { * \param p_small_exit_time * Sets small_exit_time. Timer for to exit within smalL_error. * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * Sets smalL_error. Timer will start when error is within this. Okapi unit. * \param p_big_exit_time * Sets big_exit_time. Timer for to exit within big_error. * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * Sets big_error. Timer will start when error is within this. Okapi unit. * \param p_velocity_exit_time * Sets velocity_exit_time. Timer will start when velocity is 0. */ - void set_drive_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout); + void set_drive_exit_condition(int p_small_exit_time, okapi::QLength p_small_error, int p_big_exit_time, okapi::QLength p_big_error, int p_velocity_exit_time, int p_mA_timeout); /** * Set's constants for turn exit conditions. @@ -783,14 +802,18 @@ class Drive { */ double slew_calculate(slew_ &input, double current); - bool using_inches = false; - private: // !Auton bool drive_toggle = true; bool print_toggle = true; int swing_min = 0; int turn_min = 0; bool practice_mode_is_on = false; + + /** + * Private wait until for drive + */ + void wait_until_drive(double target); + /** * Sets the chassis to voltage. * diff --git a/include/main.h b/include/main.h index 4b5716b3..a57785d1 100644 --- a/include/main.h +++ b/include/main.h @@ -58,6 +58,7 @@ // using namespace pros::literals; // using namespace okapi; // using namespace ez; +using namespace okapi::literals; /** * Prototypes for the competition control tasks are redefined here to ensure diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index 3d5eabce..84ed6b33 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -141,24 +141,21 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por void Drive::set_defaults() { // PID Constants - headingPID = {11, 0, 20, 0}; - forward_drivePID = {0.45, 0, 5, 0}; - backward_drivePID = {0.45, 0, 5, 0}; - turnPID = {5, 0.003, 35, 15}; - swingPID = {7, 0, 45, 0}; - leftPID = {0.45, 0, 5, 0}; - rightPID = {0.45, 0, 5, 0}; + set_heading_pid_constants(3, 0, 20, 0); + set_drive_pid_constants(15, 0, 150); + set_turn_pid_constants(3, 0, 20, 0); + set_swing_pid_constants(5, 0, 30, 0); set_turn_min(30); set_swing_min(30); // Slew constants set_slew_min_power(80, 80); - set_slew_distance(7, 7); + set_slew_distance(7_in, 7_in); // Exit condition constants - set_turn_exit_condition(100, 3, 500, 7, 500, 500); - set_swing_exit_condition(100, 3, 500, 7, 500, 500); - set_drive_exit_condition(80, 50, 300, 150, 500, 500); + set_turn_exit_condition(200, 3, 500, 7, 750, 750); + set_swing_exit_condition(200, 3, 500, 7, 750, 750); + set_drive_exit_condition(200, 1_in, 500, 3_in, 750, 750); // Modify joystick curve on controller (defaults to disabled) toggle_modify_curve_with_controller(true); @@ -189,31 +186,6 @@ double Drive::get_tick_per_inch() { void Drive::set_ratio(double ratio) { RATIO = ratio; } -void Drive::set_drive_pid_constants(double p, double i, double d, double p_start_i) { - set_drive_forward_pid_constants(p, i, d, p_start_i); - set_drive_backwards_pid_constants(p, i, d, p_start_i); -} - -void Drive::set_drive_forward_pid_constants(double p, double i, double d, double p_start_i) { - forward_drivePID.set_constants(p, i, d, p_start_i); -} - -void Drive::set_drive_backwards_pid_constants(double p, double i, double d, double p_start_i) { - backward_drivePID.set_constants(p, i, d, p_start_i); -} - -void Drive::set_turn_pid_constants(double p, double i, double d, double p_start_i) { - turnPID.set_constants(p, i, d, p_start_i); -} - -void Drive::set_swing_pid_constants(double p, double i, double d, double p_start_i) { - swingPID.set_constants(p, i, d, p_start_i); -} - -void Drive::set_heading_pid_constants(double p, double i, double d, double p_start_i) { - headingPID.set_constants(p, i, d, p_start_i); -} - void Drive::private_set_tank(int left, int right) { if (pros::millis() < 1500) return; @@ -258,24 +230,26 @@ void Drive::reset_drive_sensor() { } } -int Drive::right_sensor() { +int Drive::raw_right_sensor() { if (is_tracker == DRIVE_ADI_ENCODER) return right_tracker.get_value(); else if (is_tracker == DRIVE_ROTATION) return right_rotation.get_position(); return right_motors.front().get_position(); } +double Drive::right_sensor() { return raw_right_sensor() / get_tick_per_inch(); } int Drive::right_velocity() { return right_motors.front().get_actual_velocity(); } double Drive::right_mA() { return right_motors.front().get_current_draw(); } bool Drive::right_over_current() { return right_motors.front().is_over_current(); } -int Drive::left_sensor() { +int Drive::raw_left_sensor() { if (is_tracker == DRIVE_ADI_ENCODER) return left_tracker.get_value(); else if (is_tracker == DRIVE_ROTATION) return left_rotation.get_position(); return left_motors.front().get_position(); } +double Drive::left_sensor() { return raw_left_sensor() / get_tick_per_inch(); } int Drive::left_velocity() { return left_motors.front().get_actual_velocity(); } double Drive::left_mA() { return left_motors.front().get_current_draw(); } bool Drive::left_over_current() { return left_motors.front().is_over_current(); } diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index dca62cb8..1a22d24d 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -9,20 +9,18 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. using namespace ez; - -void Drive::set_drive_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout){ - leftPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); - rightPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); +void Drive::set_drive_exit_condition(int p_small_exit_time, okapi::QLength p_small_error, int p_big_exit_time, okapi::QLength p_big_error, int p_velocity_exit_time, int p_mA_timeout) { + leftPID.set_exit_condition(p_small_exit_time, p_small_error.convert(okapi::inch), p_big_exit_time, p_big_error.convert(okapi::inch), p_velocity_exit_time, p_mA_timeout); + rightPID.set_exit_condition(p_small_exit_time, p_small_error.convert(okapi::inch), p_big_exit_time, p_big_error.convert(okapi::inch), p_velocity_exit_time, p_mA_timeout); } -void Drive::set_turn_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout){ +void Drive::set_turn_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) { turnPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } -void Drive::set_swing_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout){ +void Drive::set_swing_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) { swingPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } - // User wrapper for exit condition void Drive::wait_drive() { // Let the PID run at least 1 iteration @@ -36,7 +34,7 @@ void Drive::wait_drive() { right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]); pros::delay(util::DELAY_TIME); } - if (print_toggle) std::cout << " Left: " << exit_to_string(left_exit) << " Exit. Right: " << exit_to_string(right_exit) << " Exit.\n"; + if (print_toggle) std::cout << " Left: " << exit_to_string(left_exit) << " Exit, error: " << leftPID.error << ". Right: " << exit_to_string(right_exit) << " Exit, error: " << rightPID.error << ".\n"; if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) { interfered = true; @@ -50,7 +48,7 @@ void Drive::wait_drive() { turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]}); pros::delay(util::DELAY_TIME); } - if (print_toggle) std::cout << " Turn: " << exit_to_string(turn_exit) << " Exit.\n"; + if (print_toggle) std::cout << " Turn: " << exit_to_string(turn_exit) << " Exit, error:" << turnPID.error << ".\n"; if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) { interfered = true; @@ -65,7 +63,7 @@ void Drive::wait_drive() { swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor); pros::delay(util::DELAY_TIME); } - if (print_toggle) std::cout << " Swing: " << exit_to_string(swing_exit) << " Exit.\n"; + if (print_toggle) std::cout << " Swing: " << exit_to_string(swing_exit) << " Exit, error:" << swingPID.error << ".\n"; if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) { interfered = true; @@ -73,13 +71,12 @@ void Drive::wait_drive() { } } -// Function to wait until a certain position is reached. Wrapper for exit condition. -void Drive::wait_until(double target) { +void Drive::wait_until_drive(double target) { // If robot is driving... if (mode == DRIVE) { // Calculate error between current and target (target needs to be an in between position) - int l_tar = l_start + (target * TICK_PER_INCH); - int r_tar = r_start + (target * TICK_PER_INCH); + int l_tar = l_start + target; + int r_tar = r_start + target; int l_error = l_tar - left_sensor(); int r_error = r_tar - right_sensor(); int l_sgn = util::sgn(l_error); @@ -116,6 +113,23 @@ void Drive::wait_until(double target) { pros::delay(util::DELAY_TIME); } } +} + +void Drive::wait_until(okapi::QLength target) { + // If robot is driving... + if (mode == DRIVE) { + wait_until_drive(target.convert(okapi::inch)); + } else { + printf("QLength not supported for turn or swing!\n"); + } +} + +// Function to wait until a certain position is reached. Wrapper for exit condition. +void Drive::wait_until(double target) { + // If robot is driving... + if (mode == DRIVE) { + wait_until_drive(target); + } // If robot is turning or swinging... else if (mode == TURN || mode == SWING) { diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index 55df31af..59d90b50 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -35,13 +35,9 @@ void Drive::ez_auto_task() { // Drive PID task void Drive::drive_pid_task() { // Compute PID - if (!using_inches) { - leftPID.compute(left_sensor()); - rightPID.compute(right_sensor()); - } else { - leftPID.compute(left_sensor() / TICK_PER_INCH); - rightPID.compute(right_sensor() / TICK_PER_INCH); - } + leftPID.compute(left_sensor()); + rightPID.compute(right_sensor()); + headingPID.compute(get_gyro()); // Compute slew diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index b036a663..22cf1822 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/src/EZ-Template/drive/set_pid.cpp @@ -6,6 +6,32 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "main.h" +// Set PID constants +void Drive::set_drive_pid_constants(double p, double i, double d, double p_start_i) { + set_drive_forward_pid_constants(p, i, d, p_start_i); + set_drive_backwards_pid_constants(p, i, d, p_start_i); +} + +void Drive::set_drive_forward_pid_constants(double p, double i, double d, double p_start_i) { + forward_drivePID.set_constants(p, i, d, p_start_i); +} + +void Drive::set_drive_backwards_pid_constants(double p, double i, double d, double p_start_i) { + backward_drivePID.set_constants(p, i, d, p_start_i); +} + +void Drive::set_turn_pid_constants(double p, double i, double d, double p_start_i) { + turnPID.set_constants(p, i, d, p_start_i); +} + +void Drive::set_swing_pid_constants(double p, double i, double d, double p_start_i) { + swingPID.set_constants(p, i, d, p_start_i); +} + +void Drive::set_heading_pid_constants(double p, double i, double d, double p_start_i) { + headingPID.set_constants(p, i, d, p_start_i); +} + // Updates max speed void Drive::set_max_speed(int speed) { max_speed = util::clip_num(abs(speed), 127, -127); @@ -25,9 +51,7 @@ void Drive::set_angle(double angle) { reset_gyro(angle); } -void Drive::set_mode(e_mode p_mode) { - mode = p_mode; -} +void Drive::set_mode(e_mode p_mode) { mode = p_mode; } void Drive::set_turn_min(int min) { turn_min = abs(min); } int Drive::get_turn_min() { return turn_min; } @@ -38,11 +62,11 @@ int Drive::get_swing_min() { return swing_min; } e_mode Drive::get_mode() { return mode; } // Set drive PID -void Drive::set_drive_pid(double target, int speed, bool slew_on, bool toggle_heading) { - TICK_PER_INCH = get_tick_per_inch(); +void Drive::set_drive_pid(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading) { + double target = p_target.convert(okapi::inch); // Print targets - if (print_toggle) printf("Drive Started... Target Value: %f (%f ticks)", target, target * TICK_PER_INCH); + if (print_toggle) printf("Drive Started... Target Value: %f in", target); if (slew_on && print_toggle) printf(" with slew"); if (print_toggle) printf("\n"); @@ -56,15 +80,8 @@ void Drive::set_drive_pid(double target, int speed, bool slew_on, bool toggle_he double l_target_encoder, r_target_encoder; // Figure actual target value - if (!using_inches) { - l_target_encoder = l_start + (target * TICK_PER_INCH); - r_target_encoder = r_start + (target * TICK_PER_INCH); - } else { - l_target_encoder = (l_start / TICK_PER_INCH) + target; - r_target_encoder = (r_start / TICK_PER_INCH) + target; - } - - printf("\nltar %f rtar %f", l_target_encoder, r_target_encoder); + l_target_encoder = l_start + target; + r_target_encoder = r_start + target; // Figure out if going forward or backward if (l_target_encoder < l_start && r_target_encoder < r_start) { diff --git a/src/EZ-Template/drive/slew.cpp b/src/EZ-Template/drive/slew.cpp index 3c77577a..9159af08 100644 --- a/src/EZ-Template/drive/slew.cpp +++ b/src/EZ-Template/drive/slew.cpp @@ -4,7 +4,7 @@ License, v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include "main.h" +#include "EZ-Template/api.hpp" using namespace ez; @@ -15,9 +15,9 @@ void Drive::set_slew_min_power(int fwd, int rev) { } // Set distance to slew for -void Drive::set_slew_distance(int fwd, int rev) { - SLEW_DISTANCE[0] = abs(fwd); - SLEW_DISTANCE[1] = abs(rev); +void Drive::set_slew_distance(okapi::QLength fwd, okapi::QLength rev) { + SLEW_DISTANCE[0] = fabs(fwd.convert(okapi::inch)); + SLEW_DISTANCE[1] = fabs(rev.convert(okapi::inch)); } // Initialize slew @@ -26,7 +26,7 @@ void Drive::slew_initialize(slew_ &input, bool slew_on, double max_speed, double input.max_speed = max_speed; input.sign = util::sgn(target - current); - input.x_intercept = start + ((SLEW_DISTANCE[backwards] * input.sign) * TICK_PER_INCH); + input.x_intercept = start + ((SLEW_DISTANCE[backwards] * input.sign)); input.y_intercept = max_speed * input.sign; input.slope = ((input.sign * SLEW_MIN_POWER[backwards]) - input.y_intercept) / (input.x_intercept - 0 - start); // y2-y1 / x2-x1 } diff --git a/src/autons.cpp b/src/autons.cpp index ec7a178a..d708489e 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -24,59 +24,47 @@ const int SWING_SPEED = 90; // If the objects are light or the cog doesn't change much, then there isn't a concern here. void default_constants() { - chassis.set_turn_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_swing_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_drive_exit_condition(80, 50, 300, 150, 500, 500); + chassis.set_turn_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_swing_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_drive_exit_condition(200, 1_in, 500, 3_in, 750, 750); chassis.set_slew_min_power(80, 80); - chassis.set_slew_distance(7, 7); - chassis.set_heading_pid_constants(11, 0, 20, 0); - chassis.set_drive_pid_constants(0.45, 0, 5, 0); - chassis.set_turn_pid_constants(5, 0.003, 35, 15); - chassis.set_swing_pid_constants(7, 0, 45, 0); -} - -void using_inches_constants() { - chassis.set_turn_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_swing_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_drive_exit_condition(80, 1, 300, 3, 500, 500); + chassis.set_slew_distance(7_in, 7_in); + chassis.set_heading_pid_constants(3, 0, 20, 0); + chassis.set_drive_pid_constants(15, 0, 150); + chassis.set_turn_pid_constants(3, 0, 20, 0); + chassis.set_swing_pid_constants(5, 0, 30, 0); - chassis.set_slew_min_power(80, 80); - chassis.set_slew_distance(7, 7); - chassis.set_heading_pid_constants(11, 0, 20, 0); - chassis.set_drive_pid_constants(22.5, 0, 250, 0); - chassis.set_turn_pid_constants(5, 0.003, 35, 15); - chassis.set_swing_pid_constants(7, 0, 45, 0); } void one_mogo_constants() { - chassis.set_slew_min_power(80, 80); - chassis.set_slew_distance(7, 7); - chassis.set_heading_pid_constants(11, 0, 20, 0); - chassis.set_drive_pid_constants(0.45, 0, 5, 0); - chassis.set_turn_pid_constants(5, 0.003, 35, 15); - chassis.set_swing_pid_constants(7, 0, 45, 0); -} + chassis.set_turn_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_swing_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_drive_exit_condition(200, 1_in, 500, 3_in, 750, 750); -void two_mogo_constants() { chassis.set_slew_min_power(80, 80); - chassis.set_slew_distance(7, 7); - chassis.set_heading_pid_constants(11, 0, 20, 0); - chassis.set_drive_pid_constants(0.45, 0, 5, 0); - chassis.set_turn_pid_constants(5, 0.003, 35, 15); - chassis.set_swing_pid_constants(7, 0, 45, 0); + chassis.set_slew_distance(7_in, 7_in); + chassis.set_heading_pid_constants(3, 0, 20, 0); + chassis.set_drive_pid_constants(15, 0, 150); + chassis.set_turn_pid_constants(3, 0, 20, 0); + chassis.set_swing_pid_constants(5, 0, 30, 0); } +void two_mogo_constants() { + chassis.set_turn_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_swing_exit_condition(200, 3, 500, 7, 750, 750); + chassis.set_drive_exit_condition(200, 1_in, 500, 3_in, 750, 750); -void modified_exit_condition() { - chassis.set_turn_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_swing_exit_condition(100, 3, 500, 7, 500, 500); - chassis.set_drive_exit_condition(80, 1, 300, 3, 500, 500); + chassis.set_slew_min_power(80, 80); + chassis.set_slew_distance(7_in, 7_in); + chassis.set_heading_pid_constants(3, 0, 20, 0); + chassis.set_drive_pid_constants(15, 0, 150); + chassis.set_turn_pid_constants(3, 0, 20, 0); + chassis.set_swing_pid_constants(5, 0, 30, 0); } - /// // Drive Example /// @@ -87,13 +75,13 @@ void drive_example() { // for slew, only enable it when the drive distance is greater then the slew distance + a few inches - chassis.set_drive_pid(24, DRIVE_SPEED, true); + chassis.set_drive_pid(24_in, DRIVE_SPEED, true); chassis.wait_drive(); - chassis.set_drive_pid(-12, DRIVE_SPEED); + chassis.set_drive_pid(-12_in, DRIVE_SPEED); chassis.wait_drive(); - chassis.set_drive_pid(-12, DRIVE_SPEED); + chassis.set_drive_pid(-12_in, DRIVE_SPEED); chassis.wait_drive(); } @@ -123,7 +111,7 @@ void turn_example() { // Combining Turn + Drive /// void drive_and_turn() { - chassis.set_drive_pid(24, DRIVE_SPEED, true); + chassis.set_drive_pid(24_in, DRIVE_SPEED, true); chassis.wait_drive(); chassis.set_turn_pid(45, TURN_SPEED); @@ -135,7 +123,7 @@ void drive_and_turn() { chassis.set_turn_pid(0, TURN_SPEED); chassis.wait_drive(); - chassis.set_drive_pid(-24, DRIVE_SPEED, true); + chassis.set_drive_pid(-24_in, DRIVE_SPEED, true); chassis.wait_drive(); } @@ -149,8 +137,8 @@ void wait_until_change_speed() { // When the robot gets to 6 inches, the robot will travel the remaining distance at a max speed of 40 - chassis.set_drive_pid(24, DRIVE_SPEED, true); - chassis.wait_until(6); + chassis.set_drive_pid(24_in, DRIVE_SPEED, true); + chassis.wait_until(6_in); chassis.set_max_speed(40); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed chassis.wait_drive(); @@ -164,8 +152,8 @@ void wait_until_change_speed() { chassis.wait_drive(); // When the robot gets to -6 inches, the robot will travel the remaining distance at a max speed of 40 - chassis.set_drive_pid(-24, DRIVE_SPEED, true); - chassis.wait_until(-6); + chassis.set_drive_pid(-24_in, DRIVE_SPEED, true); + chassis.wait_until(-6_in); chassis.set_max_speed(40); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed chassis.wait_drive(); } @@ -184,7 +172,7 @@ void swing_example() { chassis.set_swing_pid(ez::LEFT_SWING, 45, SWING_SPEED); chassis.wait_drive(); - chassis.set_drive_pid(24, DRIVE_SPEED, true); + chassis.set_drive_pid(24_in, DRIVE_SPEED, true); chassis.wait_until(12); chassis.set_swing_pid(ez::RIGHT_SWING, 0, SWING_SPEED); @@ -197,7 +185,7 @@ void swing_example() { // Auto that tests everything /// void combining_movements() { - chassis.set_drive_pid(24, DRIVE_SPEED, true); + chassis.set_drive_pid(24_in, DRIVE_SPEED, true); chassis.wait_drive(); chassis.set_turn_pid(45, TURN_SPEED); @@ -209,7 +197,7 @@ void combining_movements() { chassis.set_turn_pid(0, TURN_SPEED); chassis.wait_drive(); - chassis.set_drive_pid(-24, DRIVE_SPEED, true); + chassis.set_drive_pid(-24_in, DRIVE_SPEED, true); chassis.wait_drive(); } @@ -222,13 +210,13 @@ void tug (int attempts) { for (int i=0; i