Skip to content

Commit

Permalink
✨Motion chaining #109 (#115)
Browse files Browse the repository at this point in the history
* Motion chaining and quick exits are functional #109

* Added reverse constants, okapi units, finalized function names and comments
  • Loading branch information
ssejrog authored Jun 2, 2024
1 parent 13838bb commit 0566999
Show file tree
Hide file tree
Showing 7 changed files with 300 additions and 7 deletions.
175 changes: 175 additions & 0 deletions include/EZ-Template/drive/drive.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -890,6 +890,19 @@ class Drive {
*/
void pid_wait_until(double target);

/**
* Lock the code in a while loop until the robot has settled.
* Wrapper for pid_wait_until(target), target is your previously input target
*/
void pid_wait_quick();

/**
* Lock the code in a while loop until the robot has settled.
* This also adds distance to target, and then exits with pid_wait_quick
* This will exit the motion while carrying momentum into the next motion.
*/
void pid_wait_quick_chain();

/**
* Autonomous interference detection. Returns true when interfered, and false when nothing happened.
*/
Expand Down Expand Up @@ -936,6 +949,29 @@ class Drive {
*/
PID::Constants pid_turn_constants_get();

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets turning constants.
*
* \param input
* okapi angle unit
*/
void pid_turn_chain_constant_set(okapi::QAngle input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets turning constants.
*
* \param input
* angle in degrees
*/
void pid_turn_chain_constant_set(double input);

/**
* Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for turning.
*/
double pid_turn_chain_constant_get();

/**
* @brief Set the swing pid constants object
*
Expand Down Expand Up @@ -996,6 +1032,70 @@ class Drive {
*/
PID::Constants pid_swing_constants_backward_get();

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets forward and backwards swing constants.
*
* \param input
* okapi angle unit
*/
void pid_swing_chain_constant_set(okapi::QAngle input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets forward swing constants.
*
* \param input
* okapi angle unit
*/
void pid_swing_chain_forward_constant_set(okapi::QAngle input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets backward swing constants.
*
* \param input
* okapi angle unit
*/
void pid_swing_chain_backward_constant_set(okapi::QAngle input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets forward and backwards swing constants.
*
* \param input
* angle in degrees
*/
void pid_swing_chain_constant_set(double input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets forward constants.
*
* \param input
* angle in degrees
*/
void pid_swing_chain_forward_constant_set(double input);

/**
* Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for swinging forward.
*/
double pid_swing_chain_forward_constant_get();

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets backwards swing constants.
*
* \param input
* angle in degrees
*/
void pid_swing_chain_backward_constant_set(double input);

/**
* Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for swinging backward.
*/
double pid_swing_chain_backward_constant_get();

/**
* @brief Set the heading pid constants object
*
Expand Down Expand Up @@ -1076,6 +1176,70 @@ class Drive {
*/
PID::Constants pid_drive_constants_backward_get();

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets forward and backwards driving constants.
*
* \param input
* okapi length unit
*/
void pid_drive_chain_constant_set(okapi::QLength input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets forward driving constants.
*
* \param input
* okapi length unit
*/
void pid_drive_chain_forward_constant_set(okapi::QLength input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets backward driving constants.
*
* \param input
* okapi length unit
*/
void pid_drive_chain_backward_constant_set(okapi::QLength input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This sets forward and backwards driving constants.
*
* \param input
* distance in inches
*/
void pid_drive_chain_constant_set(double input);

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets forward driving constants.
*
* \param input
* distance in inches
*/
void pid_drive_chain_forward_constant_set(double input);

/**
* Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for driving forward.
*/
double pid_drive_chain_forward_constant_get();

/**
* Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
* This only sets backward driving constants.
*
* \param input
* distance in inches
*/
void pid_drive_chain_backward_constant_set(double input);

/**
* Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for driving backward.
*/
double pid_drive_chain_backward_constant_get();

/**
* Sets minimum power for swings when kI and startI are enabled.
*
Expand Down Expand Up @@ -1324,7 +1488,18 @@ class Drive {
double pid_tuner_increment_start_i_get();

private: // !Auton
double chain_target_start = 0.0;
double chain_sensor_start = 0.0;
double drive_forward_motion_chain_scale = 0.0;
double drive_backward_motion_chain_scale = 0.0;
double swing_forward_motion_chain_scale = 0.0;
double swing_backward_motion_chain_scale = 0.0;
double turn_motion_chain_scale = 0.0;
double used_motion_chain_scale = 0.0;
bool motion_chain_backward = false;

double IMU_SCALER = 1.0;

bool drive_toggle = true;
bool print_toggle = true;
int swing_min = 0;
Expand Down
1 change: 1 addition & 0 deletions include/autons.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ void turn_example();
void drive_and_turn();
void wait_until_change_speed();
void swing_example();
void motion_chaining();
void combining_movements();
void interfered_example();

Expand Down
4 changes: 4 additions & 0 deletions src/EZ-Template/drive/drive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,10 @@ void Drive::drive_defaults_set() {
pid_swing_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);
pid_drive_exit_condition_set(300_ms, 1_in, 500_ms, 3_in, 750_ms, 750_ms);

pid_turn_chain_constant_set(3_deg);
pid_swing_chain_constant_set(5_deg);
pid_drive_chain_constant_set(3_in);

// Modify joystick curve on controller (defaults to disabled)
opcontrol_curve_buttons_toggle(true);

Expand Down
74 changes: 73 additions & 1 deletion src/EZ-Template/drive/exit_conditions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -270,4 +270,76 @@ void Drive::pid_wait_until(double target) {
} else {
printf("Not in a valid drive mode!\n");
}
}
}

// Pid wait, but quickly :)
void Drive::pid_wait_quick() {
if (!(mode == DRIVE || mode == TURN || mode == SWING)) {
printf("Not in a valid drive mode!\n");
return;
}

// This is the target the user set, not the modified chained target
pid_wait_until(chain_target_start);
}

// Set drive motion chain constants
void Drive::pid_drive_chain_constant_set(double input) {
pid_drive_chain_forward_constant_set(input);
pid_drive_chain_backward_constant_set(input);
}
void Drive::pid_drive_chain_forward_constant_set(double input) { drive_forward_motion_chain_scale = fabs(input); }
void Drive::pid_drive_chain_backward_constant_set(double input) { drive_backward_motion_chain_scale = fabs(input); }
void Drive::pid_drive_chain_constant_set(okapi::QLength input) { pid_drive_chain_constant_set(input.convert(okapi::inch)); }
void Drive::pid_drive_chain_forward_constant_set(okapi::QLength input) { pid_drive_chain_forward_constant_set(input.convert(okapi::inch)); }
void Drive::pid_drive_chain_backward_constant_set(okapi::QLength input) { pid_drive_chain_backward_constant_set(input.convert(okapi::inch)); }

// Set turn motion chain constants
void Drive::pid_turn_chain_constant_set(double input) { turn_motion_chain_scale = fabs(input); }
void Drive::pid_turn_chain_constant_set(okapi::QAngle input) { pid_turn_chain_constant_set(input.convert(okapi::degree)); }

// Set swing motion chain constants
void Drive::pid_swing_chain_constant_set(double input) {
pid_swing_chain_forward_constant_set(input);
pid_swing_chain_backward_constant_set(input);
}
void Drive::pid_swing_chain_forward_constant_set(double input) { swing_forward_motion_chain_scale = fabs(input); }
void Drive::pid_swing_chain_backward_constant_set(double input) { swing_backward_motion_chain_scale = fabs(input); }
void Drive::pid_swing_chain_constant_set(okapi::QAngle input) { pid_swing_chain_constant_set(input.convert(okapi::degree)); }
void Drive::pid_swing_chain_forward_constant_set(okapi::QAngle input) { pid_swing_chain_forward_constant_set(input.convert(okapi::degree)); }
void Drive::pid_swing_chain_backward_constant_set(okapi::QAngle input) { pid_swing_chain_backward_constant_set(input.convert(okapi::degree)); }

// Get motion chain constants
double Drive::pid_drive_chain_forward_constant_get() { return drive_forward_motion_chain_scale; }
double Drive::pid_drive_chain_backward_constant_get() { return drive_backward_motion_chain_scale; }
double Drive::pid_turn_chain_constant_get() { return turn_motion_chain_scale; }
double Drive::pid_swing_chain_forward_constant_get() { return swing_forward_motion_chain_scale; }
double Drive::pid_swing_chain_backward_constant_get() { return swing_backward_motion_chain_scale; }

// Pid wait that hold momentum into the next motion
void Drive::pid_wait_quick_chain() {
// If driving, add drive_motion_chain_scale to target
if (mode == DRIVE) {
double chain_scale = motion_chain_backward ? drive_backward_motion_chain_scale : drive_forward_motion_chain_scale;
used_motion_chain_scale = chain_scale * util::sgn(chain_target_start);
leftPID.target_set(leftPID.target_get() + used_motion_chain_scale);
rightPID.target_set(leftPID.target_get() + used_motion_chain_scale);
}
// If turning, add turn_motion_chain_scale to target
else if (mode == TURN) {
used_motion_chain_scale = turn_motion_chain_scale * util::sgn(chain_target_start - chain_sensor_start);
turnPID.target_set(turnPID.target_get() + used_motion_chain_scale);
}
// If swinging, add swing_motion_chain_scale to target
else if (mode == SWING) {
double chain_scale = motion_chain_backward ? swing_backward_motion_chain_scale : swing_forward_motion_chain_scale;
used_motion_chain_scale = chain_scale * util::sgn(chain_target_start - chain_sensor_start);
swingPID.target_set(swingPID.target_get() + used_motion_chain_scale);
} else {
printf("Not in a valid drive mode!\n");
return;
}

// Exit at the real target
pid_wait_quick();
}
20 changes: 16 additions & 4 deletions src/EZ-Template/drive/set_pid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,9 @@ void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_he
if (print_toggle) printf("Drive Started... Target Value: %f", target);
if (slew_on && print_toggle) printf(" with slew");
if (print_toggle) printf("\n");
chain_target_start = target;
chain_sensor_start = drive_sensor_left();
used_motion_chain_scale = 0.0;

// Global setup
pid_speed_max_set(speed);
Expand All @@ -153,10 +156,11 @@ void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_he
if (l_target_encoder < l_start && r_target_encoder < r_start) {
pid_consts = backward_drivePID.constants_get();
slew_consts = slew_backward.constants_get();

motion_chain_backward = true;
} else {
pid_consts = forward_drivePID.constants_get();
slew_consts = slew_forward.constants_get();
motion_chain_backward=false;
}
leftPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i);
rightPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i);
Expand Down Expand Up @@ -185,14 +189,17 @@ void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool
void Drive::pid_turn_set(double target, int speed, bool slew_on) {
// Print targets
if (print_toggle) printf("Turn Started... Target Value: %f\n", target);
chain_sensor_start = drive_imu_get();
chain_target_start = target;
used_motion_chain_scale = 0.0;

// Set PID targets
turnPID.target_set(target);
headingPID.target_set(target); // Update heading target for next drive motion
pid_speed_max_set(speed);

// Initialize slew
slew_turn.initialize(slew_on, max_speed, target, drive_imu_get());
slew_turn.initialize(slew_on, max_speed, target, chain_sensor_start);

// Run task
drive_mode_set(TURN);
Expand Down Expand Up @@ -225,10 +232,13 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
// Print targets
if (print_toggle) printf("Swing Started... Target Value: %f\n", target);
current_swing = type;
chain_sensor_start = drive_imu_get();
chain_target_start = target;
used_motion_chain_scale = 0.0;

// Figure out if going forward or backward
int side = type == ez::LEFT_SWING ? 1 : -1;
int direction = util::sgn((target - drive_imu_get()) * side);
int direction = util::sgn((target - chain_sensor_start) * side);

// Set constants according to the robots direction
PID::Constants pid_consts;
Expand All @@ -240,12 +250,14 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
pid_swing_consts = backward_swingPID.constants_get();
slew_consts = slew_swing_backward.constants_get();
slew_swing_using_angle = slew_swing_rev_using_angle;
motion_chain_backward = true;

} else {
pid_consts = forward_drivePID.constants_get();
pid_swing_consts = forward_swingPID.constants_get();
slew_consts = slew_swing_forward.constants_get();
slew_swing_using_angle = slew_swing_fwd_using_angle;
motion_chain_backward = false;
}

// Set targets for the side that isn't moving
Expand All @@ -264,7 +276,7 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s

// Initialize slew
double slew_tar = slew_swing_using_angle ? target : direction * 100;
double current = slew_swing_using_angle ? drive_imu_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
double current = slew_swing_using_angle ? chain_sensor_start : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
slew_swing.initialize(slew_on, max_speed, slew_tar, current);

// Run task
Expand Down
Loading

0 comments on commit 0566999

Please sign in to comment.