diff --git a/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 3c581a07..d4696bec 100644 --- a/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -24,19 +24,18 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param canCoder + * The CANcoder associated with the TalonFX + * @param gearRatio + * The gear ratio from the TalonFX to the mechanism + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { this._falcon = falcon; this._canCoder = canCoder; this._gearRatio = gearRatio; - this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, .001); + this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, rotorInertia); } /** diff --git a/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java b/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java index c812d3f4..84d7afaa 100644 --- a/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java +++ b/java/CommandBasedDrive/src/main/java/frc/robot/RobotContainer.java @@ -5,12 +5,11 @@ package frc.robot; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; import frc.robot.commands.DriveStraightCommand; import frc.robot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -25,7 +24,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final XboxController m_joystick = new XboxController(0); + private final CommandXboxController m_joystick = new CommandXboxController(0); /* invert the joystick Y because forward Y is negative */ private final Command m_teleopDrive = new RunCommand(() -> { @@ -55,7 +54,7 @@ public RobotContainer() { */ private void configureButtonBindings() { /* If the upper left shoulder button is pressed, drive straight */ - new Trigger(m_joystick::getLeftBumper).whileTrue(m_driveStraight); + m_joystick.leftBumper().whileTrue(m_driveStraight); } /** diff --git a/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 030df799..695b4716 100644 --- a/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -24,13 +24,12 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param canCoder + * The CANcoder associated with the TalonFX + * @param gearRatio + * The gear ratio from the TalonFX to the mechanism + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) { this._falcon = falcon; diff --git a/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 9899bc31..1c103d09 100644 --- a/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -21,13 +21,8 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { this._falcon = falcon; diff --git a/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 36f7c05f..e5208999 100644 --- a/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -23,13 +23,10 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param pigeon + * The Pigeon 2 associated with the TalonFX + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final double rotorInertia) { this._falcon = falcon; diff --git a/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java index 87ab6b72..033e2cf9 100644 --- a/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -22,13 +22,8 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java index d259d7cd..47245994 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java @@ -2,6 +2,7 @@ import java.util.function.Supplier; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -13,6 +14,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -22,15 +24,25 @@ * so it can be used in command-based projects easily. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { super(driveTrainConstants, OdometryUpdateFrequency, modules); configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } } public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { super(driveTrainConstants, modules); configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } } private void configurePathPlanner() { @@ -56,13 +68,22 @@ public Command getAutoPath(String pathName) { return new PathPlannerAuto(pathName); } - @Override - public void simulationPeriodic() { - /* Assume 20ms update rate, get battery voltage from WPILib */ - updateSimState(0.02, RobotController.getBatteryVoltage()); - } - public ChassisSpeeds getCurrentRobotChassisSpeeds() { return m_kinematics.toChassisSpeeds(getState().ModuleStates); } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } } diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java index 1d1e3837..48ffe3df 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java @@ -4,9 +4,6 @@ package frc.robot; - -import com.ctre.phoenix6.SignalLogger; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -25,8 +22,6 @@ public void robotInit() { m_robotContainer = new RobotContainer(); m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99); - - SignalLogger.start(); } @Override public void robotPeriodic() { diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Telemetry.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Telemetry.java index 3fb3ded5..acade591 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Telemetry.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Telemetry.java @@ -1,5 +1,6 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; @@ -10,6 +11,8 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -20,8 +23,8 @@ public class Telemetry { private final double MaxSpeed; - private int logEntry; - private int odomEntry; + private final DoubleArrayLogEntry logEntry; + private final DoubleLogEntry odomEntry; /** * Construct a telemetry object, with the specified max speed of the robot @@ -30,53 +33,54 @@ public class Telemetry { */ public Telemetry(double maxSpeed) { MaxSpeed = maxSpeed; - logEntry = DataLogManager.getLog().start("odometry", "double[]"); - odomEntry = DataLogManager.getLog().start("odom period", "double"); + SignalLogger.start(); + logEntry = new DoubleArrayLogEntry(DataLogManager.getLog(), "odometry"); + odomEntry = new DoubleLogEntry(DataLogManager.getLog(), "odom period"); } /* What to publish over networktables for telemetry */ - NetworkTableInstance inst = NetworkTableInstance.getDefault(); + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); /* Robot pose for field positioning */ - NetworkTable table = inst.getTable("Pose"); - DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); /* Robot speeds for general checking */ - NetworkTable driveStats = inst.getTable("Drive"); - DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); - DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); - DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); - DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); /* Keep a reference of the last pose to calculate the speeds */ - Pose2d m_lastPose = new Pose2d(); - double lastTime = Utils.getCurrentTimeSeconds(); + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), }; /* A direction and length changing ligament for speed representation */ - MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), }; /* A direction changing and length constant ligament for module direction */ - MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), }; /* Accept the swerve drive state and telemeterize it to smartdashboard */ @@ -85,9 +89,9 @@ public void telemeterize(SwerveDriveState state) { Pose2d pose = state.Pose; fieldTypePub.set("Field2d"); fieldPub.set(new double[] { - pose.getX(), - pose.getY(), - pose.getRotation().getDegrees() + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() }); /* Telemeterize the robot's general speeds */ @@ -113,7 +117,8 @@ public void telemeterize(SwerveDriveState state) { SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } - DataLogManager.getLog().appendDoubleArray(logEntry, new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, (long)(Timer.getFPGATimestamp() * 1000000)); - DataLogManager.getLog().appendDouble(odomEntry, state.OdometryPeriod, (long)(Timer.getFPGATimestamp() * 1000000)); + long timestamp = (long)(Timer.getFPGATimestamp() * 1000000); + logEntry.append(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, timestamp); + odomEntry.append(state.OdometryPeriod, timestamp); } } diff --git a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java index ef3ffbef..af5bdaa7 100644 --- a/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ b/java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java @@ -21,13 +21,8 @@ class TalonFXSimProfile extends SimProfile { * * @param falcon * The TalonFX device - * @param accelToFullTime - * The time the motor takes to accelerate from 0 to full, - * in seconds - * @param fullVel - * The maximum motor velocity, in rotations per second - * @param sensorPhase - * The phase of the TalonFX sensors + * @param rotorInertia + * Rotational Inertia of the mechanism at the rotor */ public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) { this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia);