-
Notifications
You must be signed in to change notification settings - Fork 15
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
add DriveToPoseSimple and MinTimeDriveController
- Loading branch information
Showing
8 changed files
with
365 additions
and
29 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
63 changes: 63 additions & 0 deletions
63
lib/src/main/java/org/team100/lib/commands/drivetrain/DriveToPoseSimple.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,63 @@ | ||
| package org.team100.lib.commands.drivetrain; | ||
|
|
||
| import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController; | ||
| import org.team100.lib.dashboard.Glassy; | ||
| import org.team100.lib.logging.Level; | ||
| import org.team100.lib.logging.LoggerFactory; | ||
| import org.team100.lib.logging.LoggerFactory.FieldRelativeVelocityLogger; | ||
| import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; | ||
| import org.team100.lib.motion.drivetrain.SwerveState; | ||
| import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| /** | ||
| * Given a pose, drive there using only the holonomic controller, | ||
| * no profile, no trajectory. This doesn't seem to work well to follow | ||
| * a trajectory or profile, don't use it for that, just give it a point | ||
| * you want to go to, and it will go there. It makes no attempt to | ||
| * impose feasibility constraints or coordinate the axes. | ||
| */ | ||
| public class DriveToPoseSimple extends Command implements Glassy { | ||
| private final FieldRelativeVelocityLogger m_log_output; | ||
| private final SwerveState m_goal; | ||
| private final HolonomicFieldRelativeController m_controller; | ||
| private final SwerveDriveSubsystem m_swerve; | ||
|
|
||
| public DriveToPoseSimple( | ||
| LoggerFactory parent, | ||
| Pose2d goal, | ||
| HolonomicFieldRelativeController controller, | ||
| SwerveDriveSubsystem swerve) { | ||
| LoggerFactory child = parent.child(this); | ||
| m_log_output = child.fieldRelativeVelocityLogger(Level.TRACE, "output"); | ||
| // goal is motionless at the specified pose. | ||
| m_goal = new SwerveState(goal); | ||
| m_controller = controller; | ||
| m_swerve = swerve; | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| // ? | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| SwerveState measurement = m_swerve.getState(); | ||
| FieldRelativeVelocity output = m_controller.calculate(measurement, m_goal); | ||
| m_log_output.log(() -> output); | ||
| m_swerve.driveInFieldCoordsVerbatim(output); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return m_controller.atReference(); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| m_swerve.stop(); | ||
| } | ||
| } |
106 changes: 106 additions & 0 deletions
106
lib/src/main/java/org/team100/lib/controller/drivetrain/MinTimeDriveController.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,106 @@ | ||
| package org.team100.lib.controller.drivetrain; | ||
|
|
||
| import org.team100.lib.controller.simple.MinTimeController; | ||
| import org.team100.lib.framework.TimedRobot100; | ||
| import org.team100.lib.logging.LoggerFactory; | ||
| import org.team100.lib.motion.drivetrain.SwerveState; | ||
| import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; | ||
| import org.team100.lib.state.State100; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
|
|
||
| /** Min-time controllers on all axes. */ | ||
| public class MinTimeDriveController implements HolonomicFieldRelativeController { | ||
| /** Should be stronger than switching. */ | ||
| private static final int INITIAL_CURVE_ACCEL = 12; | ||
| /** Should be weaker than switching. */ | ||
| private static final int GOAL_CURVE_ACCEL = 7; | ||
| private static final int SWITCHING_CURVE_ACCEL = 9; | ||
| private static final double[] FULL_STATE_K = new double[] { 2.0, 0.2 }; | ||
| /** Switch to full-state proportional when this close to the goal. */ | ||
| private static final double EASE = 0.1; | ||
| private static final double TOLERANCE = 0.01; | ||
| private static final int MAX_OMEGA_RAD_S = 5; | ||
| private static final int MAX_VELOCITY_M_S = 5; | ||
| private final MinTimeController m_xController; | ||
| private final MinTimeController m_yController; | ||
| private final MinTimeController m_thetaController; | ||
| private final Log m_log; | ||
|
|
||
| public MinTimeDriveController(LoggerFactory parent, Log log) { | ||
| LoggerFactory child = parent.child(this); | ||
| m_xController = new MinTimeController( | ||
| child, | ||
| x -> x, | ||
| MAX_VELOCITY_M_S, | ||
| SWITCHING_CURVE_ACCEL, | ||
| GOAL_CURVE_ACCEL, | ||
| INITIAL_CURVE_ACCEL, | ||
| TOLERANCE, | ||
| EASE, | ||
| FULL_STATE_K); | ||
| m_yController = new MinTimeController( | ||
| child, | ||
| x -> x, | ||
| MAX_VELOCITY_M_S, | ||
| SWITCHING_CURVE_ACCEL, | ||
| GOAL_CURVE_ACCEL, | ||
| INITIAL_CURVE_ACCEL, | ||
| TOLERANCE, | ||
| EASE, | ||
| FULL_STATE_K); | ||
| m_thetaController = new MinTimeController( | ||
| child, | ||
| MathUtil::angleModulus, | ||
| MAX_OMEGA_RAD_S, | ||
| SWITCHING_CURVE_ACCEL, | ||
| GOAL_CURVE_ACCEL, | ||
| INITIAL_CURVE_ACCEL, | ||
| TOLERANCE, | ||
| EASE, | ||
| FULL_STATE_K); | ||
| m_log = log; | ||
| } | ||
|
|
||
| @Override | ||
| public boolean atReference() { | ||
| return m_xController.atSetpoint() && | ||
| m_yController.atSetpoint() && | ||
| m_thetaController.atSetpoint(); | ||
| } | ||
|
|
||
| /** | ||
| * Makes no attempt to coordinate the axes or provide feasible output. | ||
| */ | ||
| @Override | ||
| public FieldRelativeVelocity calculate(SwerveState measurement, SwerveState reference) { | ||
| m_log.measurement.log(() -> measurement); | ||
| m_log.reference.log(() -> reference); | ||
| m_log.error.log(() -> reference.minus(measurement)); | ||
|
|
||
| FieldRelativeVelocity u_FF = reference.velocity(); | ||
|
|
||
| State100 xFB = m_xController.calculate( | ||
| TimedRobot100.LOOP_PERIOD_S, | ||
| measurement.x(), | ||
| reference.x()); | ||
| State100 yFB = m_yController.calculate( | ||
| TimedRobot100.LOOP_PERIOD_S, | ||
| measurement.y(), | ||
| reference.y()); | ||
| State100 thetaFB = m_thetaController.calculate( | ||
| TimedRobot100.LOOP_PERIOD_S, | ||
| measurement.theta(), | ||
| reference.theta()); | ||
|
|
||
| FieldRelativeVelocity u_FB = new FieldRelativeVelocity( | ||
| xFB.v(), yFB.v(), thetaFB.v()); | ||
| m_log.u_FB.log(() -> u_FB); | ||
| return u_FF.plus(u_FB); | ||
| } | ||
|
|
||
| @Override | ||
| public void reset() { | ||
| // nothing to do | ||
| } | ||
| } |
74 changes: 74 additions & 0 deletions
74
lib/src/main/java/org/team100/lib/controller/simple/FullStateController.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,74 @@ | ||
| package org.team100.lib.controller.simple; | ||
|
|
||
| import java.util.function.DoubleUnaryOperator; | ||
|
|
||
| import org.team100.lib.logging.Level; | ||
| import org.team100.lib.logging.LoggerFactory; | ||
| import org.team100.lib.logging.LoggerFactory.DoubleLogger; | ||
| import org.team100.lib.logging.LoggerFactory.State100Logger; | ||
| import org.team100.lib.state.State100; | ||
|
|
||
| /** | ||
| * Patterned after FullStateDriveController. | ||
| */ | ||
| public class FullStateController { | ||
|
|
||
| private final State100Logger m_log_measurement; | ||
| private final State100Logger m_log_reference; // ref v is FF | ||
| private final State100Logger m_log_error; | ||
| private final DoubleLogger m_log_u_FB; | ||
| private final double m_K1; // position | ||
| private final double m_K2; // velocity | ||
| private final DoubleUnaryOperator m_modulus; | ||
| private final double m_tol1; | ||
| private final double m_tol2; | ||
|
|
||
| private boolean m_atSetpoint = false; | ||
|
|
||
| public FullStateController( | ||
| LoggerFactory parent, | ||
| double k1, | ||
| double k2, | ||
| DoubleUnaryOperator modulus, | ||
| double xtol, | ||
| double vtol) { | ||
| LoggerFactory child = parent.child("FullStateController"); | ||
| m_log_reference = child.state100Logger(Level.DEBUG, "reference"); | ||
| m_log_measurement = child.state100Logger(Level.DEBUG, "measurement"); | ||
| m_log_error = child.state100Logger(Level.DEBUG, "error"); | ||
| m_log_u_FB = child.doubleLogger(Level.DEBUG, "u_FB"); | ||
| m_K1 = k1; | ||
| m_K2 = k2; | ||
| m_modulus = modulus; | ||
| m_tol1 = xtol; | ||
| m_tol2 = vtol; | ||
| } | ||
|
|
||
| public double calculate(State100 measurement, State100 reference) { | ||
| m_log_measurement.log(() -> measurement); | ||
| m_log_reference.log(() -> reference); | ||
| m_log_error.log(() -> reference.minus(measurement)); | ||
| double u_FF = reference.v(); | ||
| m_atSetpoint = true; | ||
| double u_FB = calculateFB(measurement, reference); | ||
| m_log_u_FB.log(() -> u_FB); | ||
| return u_FF + u_FB; | ||
| } | ||
|
|
||
| private double calculateFB(State100 measurement, State100 setpoint) { | ||
| double xError = m_modulus.applyAsDouble(setpoint.x() - measurement.x()); | ||
| double xDotError = setpoint.v() - measurement.v(); | ||
| m_atSetpoint &= Math.abs(xError) < m_tol1 && Math.abs(xDotError) < m_tol2; | ||
| return m_K1 * xError + m_K2 * xDotError; | ||
| } | ||
|
|
||
| /** True if the most recent call to calculate() was at the setpoint. */ | ||
| public boolean atSetpoint() { | ||
| return m_atSetpoint; | ||
| } | ||
|
|
||
| public void reset() { | ||
| m_atSetpoint = false; | ||
| } | ||
|
|
||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.