diff --git a/comp/src/main/java/org/team100/frc2025/robot/Machinery.java b/comp/src/main/java/org/team100/frc2025/robot/Machinery.java index 5cd2a42f..0a075222 100644 --- a/comp/src/main/java/org/team100/frc2025/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2025/robot/Machinery.java @@ -29,6 +29,7 @@ import org.team100.lib.targeting.SimulatedTargetWriter; import org.team100.lib.targeting.Targets; import org.team100.lib.util.CanId; +import org.team100.lib.visualization.RobotPoseVisualization; import org.team100.lib.visualization.TrajectoryVisualization; import edu.wpi.first.math.geometry.Pose2d; @@ -52,6 +53,7 @@ public class Machinery { private static final LoggerFactory logger = Logging.instance().rootLogger; private static final LoggerFactory fieldLogger = Logging.instance().fieldLogger; + private final Runnable m_robotViz; private final Runnable m_combinedViz; private final Runnable m_climberViz; private final SwerveModuleCollection m_modules; @@ -143,7 +145,6 @@ public Machinery() { // DRIVETRAIN // m_drive = SwerveDriveFactory.get( - fieldLogger, driveLog, m_swerveKinodynamics, m_localizer, @@ -151,6 +152,8 @@ public Machinery() { history, m_modules); m_drive.resetPose(new Pose2d(m_drive.getPose().getTranslation(), new Rotation2d(Math.PI))); + m_robotViz = new RobotPoseVisualization( + fieldLogger, () -> m_drive.getState().pose(), "robot"); //////////////////////////////////////////////////////////// // @@ -171,6 +174,7 @@ public void periodic() { // show the closest target on field2d m_targets.periodic(); m_leds.periodic(); + m_robotViz.run(); m_combinedViz.run(); m_climberViz.run(); } diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java index 637945c0..798a42f1 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3.java @@ -7,7 +7,7 @@ /** * Feedback and feedforward control. */ -public interface ControllerR3 { +public interface ControllerR3 { /** * Feedback should compare the current-instant measurement to the @@ -20,8 +20,12 @@ public interface ControllerR3 { * @param measurement Current measurement state in field coordinates * @param currentReference Current reference state i.e. setpoint * @param nextReference Reference for dt in the future, used for feedforward. - * @return Control output, should be given to - * SwerveDriveSubsystem.driveInFieldCoords() or something similar. + * @return Control output for the period during dt, so it's also what the next + * measurement should be. If there's no current error (i.e. feedback is + * zero), then this output is usually just the feedforward, i.e. the + * next reference velocity. + * Give this to SwerveDriveSubsystem.driveInFieldCoords() or something + * similar. */ GlobalVelocityR3 calculate( ModelR3 measurement, diff --git a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java index 8c1d2803..250991e4 100644 --- a/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/controller/r3/ControllerR3Base.java @@ -87,6 +87,12 @@ public GlobalVelocityR3 calculate( return calculate100(m_positionError, m_velocityError, nextReference); } + /** + * @param positionError current reference minus current measurement + * @param velocityError current reference minus current measurement + * @param nextReference velocity for dt from now + * @returns control output for the period during dt + */ public abstract GlobalVelocityR3 calculate100( GlobalDeltaR3 positionError, GlobalVelocityR3 velocityError, diff --git a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java index e9e887d7..18cab790 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java +++ b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDrive100.java @@ -86,18 +86,22 @@ public ModelR3 getState() { return new ModelR3(m_pose, m_input); } - /** Use inverse kinematics to set wheel speeds. */ + /** + * Use inverse kinematics to set wheel speeds. + * + * @param nextV for the next timestep + */ @Override - public void setVelocity(GlobalVelocityR3 input) { + public void setVelocity(GlobalVelocityR3 nextV) { Rotation2d yaw = getYaw(); ChassisSpeeds speed = SwerveKinodynamics.toInstantaneousChassisSpeeds( - input, yaw); + nextV, yaw); MecanumDriveWheelSpeeds mSpeed = m_kinematics.toWheelSpeeds(speed); m_frontLeft.setVelocity(mSpeed.frontLeftMetersPerSecond); m_frontRight.setVelocity(mSpeed.frontRightMetersPerSecond); m_rearLeft.setVelocity(mSpeed.rearLeftMetersPerSecond); m_rearRight.setVelocity(mSpeed.rearRightMetersPerSecond); - m_log_input.log(() -> input); + m_log_input.log(() -> nextV); } private Rotation2d getYaw() { diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java index 64f2b51d..aa33464f 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/VelocitySubsystemR3.java @@ -10,6 +10,10 @@ */ public interface VelocitySubsystemR3 extends SubsystemR3 { - /** No scaling or filtering. */ - void setVelocity(GlobalVelocityR3 setpoint); + /** + * No scaling or filtering. + * + * @param nextV for the next timestep. + */ + void setVelocity(GlobalVelocityR3 nextV); } \ No newline at end of file diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java index a6637dd1..b4a68034 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/PositionReferenceControllerR3.java @@ -31,6 +31,10 @@ public PositionReferenceControllerR3( m_subsystem = subsystem; } + /** + * @param next set the subsystem position to this + * @param u ignored, since this uses outboard feedback only. + */ @Override void execute100(ControlR3 next, GlobalVelocityR3 u) { m_subsystem.set(next); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java index 0b16050e..237894fc 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/ReferenceControllerR3Base.java @@ -50,7 +50,7 @@ public abstract class ReferenceControllerR3Base { * Actuate the subsystem here. * * @param next The next control setpoint. - * @param u The controller output. + * @param u The controller output for the next dt */ abstract void execute100(ControlR3 next, GlobalVelocityR3 u); @@ -63,6 +63,8 @@ public void execute() { ModelR3 current = m_reference.current(); ControlR3 next = m_reference.next(); ModelR3 error = current.minus(measurement); + // u represents the time from now until now+dt, so it's also + // what the mechanism should be doing at the next time step GlobalVelocityR3 u = m_controller.calculate(measurement, current, next); execute100(next, u); m_log_measurement.log(() -> measurement); diff --git a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java index 22002067..68b62f19 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java +++ b/lib/src/main/java/org/team100/lib/subsystems/r3/commands/helper/VelocityReferenceControllerR3.java @@ -28,6 +28,12 @@ public VelocityReferenceControllerR3( m_subsystem = subsystem; } + /** + * @param next ignored + * @param u represents the control for the next dt, so it's also what the + * subsystem should be doing at the next timestep. set the subsystem + * velocity to this + */ @Override void execute100(ControlR3 next, GlobalVelocityR3 u) { m_subsystem.setVelocity(u); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveFactory.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveFactory.java index ac560370..860c671b 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveFactory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveFactory.java @@ -18,7 +18,6 @@ public class SwerveDriveFactory { public static SwerveDriveSubsystem get( - LoggerFactory fieldLogger, LoggerFactory driveLog, SwerveKinodynamics swerveKinodynamics, AprilTagRobotLocalizer localizer, @@ -39,7 +38,6 @@ public static SwerveDriveSubsystem get( swerveKinodynamics, RobotController::getBatteryVoltage); return new SwerveDriveSubsystem( - fieldLogger, driveLog, odometryUpdater, estimate, diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java index 5c939d84..fb680089 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveDriveSubsystem.java @@ -6,6 +6,7 @@ import org.team100.lib.coherence.ObjectCache; import org.team100.lib.coherence.Takt; import org.team100.lib.config.DriverSkill; +import org.team100.lib.framework.TimedRobot100; import org.team100.lib.geometry.GlobalVelocityR3; import org.team100.lib.localization.FreshSwerveEstimate; import org.team100.lib.localization.OdometryUpdater; @@ -47,15 +48,12 @@ public class SwerveDriveSubsystem extends SubsystemBase implements VelocitySubsy private final ModelR3Logger m_log_state; private final DoubleLogger m_log_turning; private final DoubleArrayLogger m_log_pose_array; - // TODO: pull the field logger out into a separate observer. - private final DoubleArrayLogger m_log_field_robot; private final EnumLogger m_log_skill; private final GlobalVelocityR3Logger m_log_input; private final List m_players; public SwerveDriveSubsystem( - LoggerFactory fieldLogger, LoggerFactory parent, OdometryUpdater odometryUpdater, FreshSwerveEstimate estimate, @@ -71,7 +69,6 @@ public SwerveDriveSubsystem( m_log_state = log.modelR3Logger(Level.COMP, "state"); m_log_turning = log.doubleLogger(Level.TRACE, "Tur Deg"); m_log_pose_array = log.doubleArrayLogger(Level.COMP, "pose array"); - m_log_field_robot = fieldLogger.doubleArrayLogger(Level.COMP, "robot"); m_log_skill = log.enumLogger(Level.TRACE, "skill level"); m_log_input = log.globalVelocityR3Logger(Level.TRACE, "drive input"); m_players = m_swerveLocal.players(); @@ -82,17 +79,28 @@ public SwerveDriveSubsystem( // ACTUATORS // - /** Skip all scaling, limits generator, etc. */ + /** + * Skip all scaling, limits generator, etc. + * + * @param nextV for the next timestep + */ @Override - public void setVelocity(GlobalVelocityR3 input) { + public void setVelocity(GlobalVelocityR3 nextV) { // keep the limiter up to date on what we're doing - m_limiter.updateSetpoint(input); + m_limiter.updateSetpoint(nextV); + + // Actuation is constant for the whole control period, which means + // that to calculate robot-relative speed from field-relative speed, + // we need to use the robot rotation *at the future time*. + ModelR3 currentState = getState(); + // Note this may add a bit of noise. + ModelR3 nextState = currentState.evolve(TimedRobot100.LOOP_PERIOD_S); + Rotation2d nextTheta = nextState.rotation(); - Rotation2d theta = getPose().getRotation(); - ChassisSpeeds targetChassisSpeeds = SwerveKinodynamics.toInstantaneousChassisSpeeds( - input, theta); - m_swerveLocal.setChassisSpeeds(targetChassisSpeeds); - m_log_input.log(() -> input); + ChassisSpeeds nextSpeed = SwerveKinodynamics.toInstantaneousChassisSpeeds( + nextV, nextTheta); + m_swerveLocal.setChassisSpeeds(nextSpeed); + m_log_input.log(() -> nextV); } /** @@ -174,10 +182,6 @@ public void periodic() { m_log_turning.log(() -> getPose().getRotation().getDegrees()); m_log_pose_array.log(this::poseArray); - // Update the Field2d widget - // the name "field" is used by Field2d. - // the name "robot" can be anything. - m_log_field_robot.log(this::poseArray); m_log_skill.log(() -> DriverSkill.level()); m_swerveLocal.periodic(); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveLocal.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveLocal.java index 88c0d82a..d03765a2 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveLocal.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/SwerveLocal.java @@ -62,11 +62,13 @@ public List players() { /** * Discretizes the speeds, calculates the inverse kinematic module states, and * sets the module states. + * + * @param nextSpeed for the next timestep. */ - void setChassisSpeeds(ChassisSpeeds speeds) { - SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(speeds); + void setChassisSpeeds(ChassisSpeeds nextSpeed) { + SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(nextSpeed); setModuleStates(states); - m_log_chassis_speed.log(() -> speeds); + m_log_chassis_speed.log(() -> nextSpeed); } void stop() { @@ -118,7 +120,10 @@ void periodic() { ///////////////////////////////////////////////////////// - private void setModuleStates(SwerveModuleStates states) { - m_modules.setDesiredStates(states); + /** + * @param nextStates for now+dt + */ + private void setModuleStates(SwerveModuleStates nextStates) { + m_modules.setDesiredStates(nextStates); } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java index 0a411218..9ebb6ef2 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java @@ -203,9 +203,11 @@ public double getMaxCapsizeAccelM_S2() { * translational acceleration. * * States may include empty angles for motionless wheels. + * + * @param nextSpeed represents the desired speed for now+dt. */ - public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in) { - return toSwerveModuleStates(in, TimedRobot100.LOOP_PERIOD_S); + public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds nextSpeed) { + return toSwerveModuleStates(nextSpeed, TimedRobot100.LOOP_PERIOD_S); } /** @@ -213,16 +215,18 @@ public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in) { * * States may include empty angles for motionless wheels. * Otherwise angle is always within [-pi, pi]. + * + * @param nextSpeed represents the desired speed for now+dt. */ - SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in, double dt) { + SwerveModuleStates toSwerveModuleStates(ChassisSpeeds nextSpeed, double dt) { // This is the extra correction angle ... - Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(in.omegaRadiansPerSecond)); + Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(nextSpeed.omegaRadiansPerSecond)); // ... which is subtracted here; this isn't really a field-relative // transformation it's just a rotation. ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - in.vxMetersPerSecond, - in.vyMetersPerSecond, - in.omegaRadiansPerSecond, + nextSpeed.vxMetersPerSecond, + nextSpeed.vyMetersPerSecond, + nextSpeed.omegaRadiansPerSecond, angle); // discretization does not affect omega DiscreteSpeed descretized = discretize(chassisSpeeds, dt); diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModule100.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModule100.java index 232c31e4..5f730a77 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModule100.java @@ -50,6 +50,8 @@ public abstract class SwerveModule100 implements Player { /** * The previous desired angle, used if the current desired angle is empty (i.e. * the module is motionless) and to calculate steering velocity. + * This is set to the "next" value and read, one step later, as the "current" + * value */ private Rotation2d m_previousDesiredWrappedAngle; private double m_previousTime; @@ -89,6 +91,8 @@ public List players() { * * Note: this uses "wrapped" states that come from inverse kinematics, since the * kinematics doesn't care about the total turns of the modules; + * + * @param desiredWrapped for now+dt */ void setDesiredState(SwerveModuleState100 desiredWrapped) { desiredWrapped = usePreviousAngleIfEmpty(desiredWrapped); @@ -179,60 +183,76 @@ static double reduceCrossTrackError( ///////////////////////////////////////////////////////////////// /** - * Turning servo commands always specify zero-velocity goal. + * Turning servo commands compute the velocity based on the previous desired + * angle. + * + * @param nextWrapped for now+dt, i.e. "next" */ - private void actuate(SwerveModuleState100 desiredWrapped) { - double speed = desiredWrapped.speedMetersPerSecond(); - if (desiredWrapped.angle().isEmpty()) + private void actuate(SwerveModuleState100 nextWrapped) { + double nextSpeed = nextWrapped.speedMetersPerSecond(); + if (nextWrapped.angle().isEmpty()) throw new IllegalArgumentException("actuation needs a real angle"); - Rotation2d desiredWrappedAngle = desiredWrapped.angle().get(); + Rotation2d nextWrappedAngle = nextWrapped.angle().get(); + double dt = dt(); + double nextOmega = omega(nextWrappedAngle, dt); if (Experiments.instance.enabled(Experiment.CorrectSpeedForSteering)) { // help drive motors overcome steering. - speed = correctSpeedForSteering(speed, desiredWrappedAngle); + nextSpeed = correctSpeedForSteering(nextSpeed, nextOmega, dt); } if (Experiments.instance.enabled(Experiment.ReduceCrossTrackError)) { double measuredAngleRad = m_turningServo.getWrappedPositionRad(); - speed = reduceCrossTrackError(measuredAngleRad, speed, desiredWrappedAngle); + nextSpeed = reduceCrossTrackError(measuredAngleRad, nextSpeed, nextWrappedAngle); } - m_driveServo.setVelocity(speed); + m_driveServo.setVelocity(nextSpeed); if (Experiments.instance.enabled(Experiment.UnprofiledSteering)) { - // no profile, just low-level position - m_turningServo.setPositionDirect(desiredWrappedAngle.getRadians(), 0, 0); + // no profile, just low-level position. Note the omega here may show up as noise. + m_turningServo.setPositionDirect(nextWrappedAngle.getRadians(), nextOmega, 0); } else { // use the profile - m_turningServo.setPositionProfiled(desiredWrappedAngle.getRadians(), 0); + m_turningServo.setPositionProfiled(nextWrappedAngle.getRadians(), 0); } - m_previousDesiredWrappedAngle = desiredWrappedAngle; + m_previousDesiredWrappedAngle = nextWrappedAngle; } /** * Correct the desired speed for steering coupling. - * - * Angle can be the "wrapped" version since the difference from the previous - * iteration is surely small. */ - private double correctSpeedForSteering(double desiredSpeed, Rotation2d desiredWrappedAngle) { - double dt = dt(); + private double correctSpeedForSteering(double speed, double omega, double dt) { + double correction = speedCorrection(speed, omega, dt); + if (DEBUG) { + System.out.printf("correction %6.3f\n", correction); + } + return speed + correction; + } + + private double speedCorrection(double desiredSpeed, double omega, double dt) { if (dt > 0.04) { // clock is unreliable, don't do anything. - return desiredSpeed; + return 0; } if (dt < 0.01) { // avoid short intervals - return desiredSpeed; + return 0; } + return m_wheelRadiusM * omega / m_finalDriveRatio; + } + + /** + * use the previous desired angle to compute steering angular velocity in + * radians per sec + * + * @param desiredWrappedAngle angle for the next timestep + * @param dt time until then (s) + * @returns rad/s + */ + private double omega(Rotation2d desiredWrappedAngle, double dt) { // dtheta is definitely a lot less than 2pi so wrapped is fine. Rotation2d dthetaWrapped = desiredWrappedAngle.minus(m_previousDesiredWrappedAngle); - double omega = dthetaWrapped.getRadians() / dt; - double correction = m_wheelRadiusM * omega / m_finalDriveRatio; - if (DEBUG) { - System.out.printf("correction %6.3f\n", correction); - } - return desiredSpeed + correction; + return dthetaWrapped.getRadians() / dt; } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModuleCollection.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModuleCollection.java index f4c01f2f..ee66d46e 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModuleCollection.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SwerveModuleCollection.java @@ -171,16 +171,16 @@ public static SwerveModuleCollection forTest(LoggerFactory log, SwerveKinodynami * * Works fine with empty angles. * - * @param swerveModuleStates + * @param nextStates for now+dt */ - public void setDesiredStates(SwerveModuleStates swerveModuleStates) { + public void setDesiredStates(SwerveModuleStates nextStates) { if (DEBUG) { - System.out.printf("setDesiredStates() %s\n", swerveModuleStates); + System.out.printf("setDesiredStates() %s\n", nextStates); } - m_frontLeft.setDesiredState(swerveModuleStates.frontLeft()); - m_frontRight.setDesiredState(swerveModuleStates.frontRight()); - m_rearLeft.setDesiredState(swerveModuleStates.rearLeft()); - m_rearRight.setDesiredState(swerveModuleStates.rearRight()); + m_frontLeft.setDesiredState(nextStates.frontLeft()); + m_frontRight.setDesiredState(nextStates.frontRight()); + m_rearLeft.setDesiredState(nextStates.rearLeft()); + m_rearRight.setDesiredState(nextStates.rearRight()); } /** diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java index 26863a9a..3700dbd6 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/OffsetDrivetrain.java @@ -55,23 +55,23 @@ public ModelR3 getState() { * Set delegate velocity from toolpoint velocity and offset. * r is from toolpoint to delegate, so invert offset. * - * @param setpoint toolpoint velocity + * @param nextV toolpoint velocity for the next timestep */ @Override - public void setVelocity(GlobalVelocityR3 setpoint) { + public void setVelocity(GlobalVelocityR3 nextV) { // the component of the cartesian part that tries to spin // the delegate // adding some of this will make the toolpoint move more rapidly // towards the cartesian goal, while injecting theta error. - GlobalVelocityR3 perpendicularOmega = omega(r(m_offset), velocity(setpoint)); + GlobalVelocityR3 perpendicularOmega = omega(r(m_offset), velocity(nextV)); // the component of the rotation part that tries to move the // delegate in x and y // respecting 100% of this velocity will keep the toolpoint // where it wants to go (if the delegate responds perfectly) - GlobalVelocityR3 tangentialVelocity = tangentialVelocity(omega(setpoint), r(m_offset.unaryMinus())); + GlobalVelocityR3 tangentialVelocity = tangentialVelocity(omega(nextV), r(m_offset.unaryMinus())); - m_delegate.setVelocity(setpoint + m_delegate.setVelocity(nextV .plus(tangentialVelocity) .plus(perpendicularOmega.times(OMEGA_MIXER))); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java index 5fae2836..bf7faf8c 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java +++ b/lib/src/main/java/org/team100/lib/subsystems/test/TrivialDrivetrain.java @@ -15,6 +15,7 @@ public class TrivialDrivetrain implements VelocitySubsystemR3 { private static final double DT = TimedRobot100.LOOP_PERIOD_S; private final ObjectCache m_stateCache; + /** setpoint for the next timestep. used only by update(). */ private GlobalVelocityR3 m_setpoint; private ModelR3 m_state; diff --git a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java index 77bcbf85..c8ded83c 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/r3/commands/test/DriveWithTrajectoryTest.java @@ -175,7 +175,6 @@ void testRealDrive() throws IOException { SwerveLimiter limiter = new SwerveLimiter(logger, swerveKinodynamics, () -> 12); SwerveDriveSubsystem drive = new SwerveDriveSubsystem( - fieldLogger, logger, odometryUpdater, estimate, diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java index d87a8c26..575c9647 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/Fixture.java @@ -74,7 +74,6 @@ public Fixture() throws IOException { SwerveLimiter limiter = new SwerveLimiter(logger, swerveKinodynamics, () -> 12); drive = new SwerveDriveSubsystem( - fieldLogger, logger, odometryUpdater, estimate, diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java index ea5b007e..55bb837a 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/RealisticFixture.java @@ -71,7 +71,6 @@ public RealisticFixture() throws IOException { SwerveLimiter limiter = new SwerveLimiter(logger, swerveKinodynamics, () -> 12); drive = new SwerveDriveSubsystem( - fieldLogger, logger, odometryUpdater, estimate, diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java index cb3036ef..8a2529a4 100644 --- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java +++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SimulatedDrivingTest.java @@ -77,7 +77,6 @@ public class SimulatedDrivingTest implements Timeless { limiter = new SwerveLimiter(logger, swerveKinodynamics, () -> 12); drive = new SwerveDriveSubsystem( - fieldLogger, logger, odometryUpdater, estimate,