From 88de3aaf6499a3f99910deb4538aea8f1111a646 Mon Sep 17 00:00:00 2001 From: m10653 Date: Wed, 17 Jan 2024 14:36:48 -0500 Subject: [PATCH] Fixed Sim latency issue (#42) * Fixed Sim latency issue * Fix Spotless check --- .../java/org/jmhsrobotics/frc2024/Robot.java | 182 ++++++----- .../subsystems/drive/DriveSubsystem.java | 306 +++++++++--------- .../subsystems/vision/VisionSubsystem.java | 4 +- 3 files changed, 242 insertions(+), 250 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/Robot.java b/src/main/java/org/jmhsrobotics/frc2024/Robot.java index 2eb78fe1..8b26358a 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/Robot.java +++ b/src/main/java/org/jmhsrobotics/frc2024/Robot.java @@ -16,97 +16,103 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; public class Robot extends TimedRobot { - private Command autonomousCommand; - - private RobotContainer m_robotContainer; - - @Override - public void robotInit() { - m_robotContainer = new RobotContainer(); - SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5)); - - } - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() { - } - - @Override - public void disabledPeriodic() { - } - - @Override - public void disabledExit() { - } - - @Override - public void autonomousInit() { - autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (autonomousCommand != null) { - autonomousCommand.schedule(); - } - - List bezierPoints = PathPlannerPath.bezierFromPoses( - new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), - new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)), - new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(0))); - - // Create the path using the bezier points created above - PathPlannerPath path = new PathPlannerPath( - bezierPoints, - new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a - // differential drivetrain, the angular constraints - // have no effect. - new GoalEndState(0.0, Rotation2d.fromDegrees(0)) // Goal end state. You can set a holonomic rotation here. If - // using a differential drivetrain, the rotation will have no - // effect. - ); - - } - - @Override - public void autonomousPeriodic() { - } - - @Override - public void autonomousExit() { - } - - @Override - public void teleopInit() { - if (autonomousCommand != null) { - autonomousCommand.cancel(); - } - } - - @Override - public void teleopPeriodic() { - } - - @Override - public void teleopExit() { - } - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() { - } - - @Override - public void testExit() { - } + private Command autonomousCommand; + + private RobotContainer m_robotContainer; + + @Override + public void robotInit() { + m_robotContainer = new RobotContainer(); + SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5)); + + } + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void disabledExit() { + } + + @Override + public void autonomousInit() { + autonomousCommand = m_robotContainer.getAutonomousCommand(); + + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } + + List bezierPoints = PathPlannerPath.bezierFromPoses( + new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)), + new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(0))); + + // Create the path using the bezier points created above + PathPlannerPath path = new PathPlannerPath(bezierPoints, + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a + // differential drivetrain, the angular + // constraints + // have no effect. + new GoalEndState(0.0, Rotation2d.fromDegrees(0)) // Goal end state. You can set a holonomic rotation + // here. If + // using a differential drivetrain, the rotation + // will have no + // effect. + ); + + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void autonomousExit() { + } + + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void teleopExit() { + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() { + } + + @Override + public void testExit() { + } + @Override + public void simulationInit() { + DriverStationSim.setEnabled(true); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java index 932b5a91..adfb94b9 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java @@ -23,163 +23,151 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DriveSubsystem extends SubsystemBase { - // Create MAXSwerveModules - private ISwerveModule m_frontLeft; - - private ISwerveModule m_frontRight; - - private ISwerveModule m_rearLeft; - - private ISwerveModule m_rearRight; - - // Create gyro - private final Pigeon2 m_gyro = new Pigeon2(SwerveConstants.kGyroCanId); - - // Create RevSwerveDrive - private final RevSwerveDrive swerveDrive; - - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { - if (Robot.isSimulation()) { - m_frontLeft = new SimSwerveModule(); - m_frontRight = new SimSwerveModule(); - m_rearLeft = new SimSwerveModule(); - m_rearRight = new SimSwerveModule(); - } else { - m_frontLeft = new MAXSwerveModule( - SwerveConstants.kFrontLeftDrivingCanId, - SwerveConstants.kFrontLeftTurningCanId, - SwerveConstants.kFrontLeftChassisAngularOffset); - - m_frontRight = new MAXSwerveModule( - SwerveConstants.kFrontRightDrivingCanId, - SwerveConstants.kFrontRightTurningCanId, - SwerveConstants.kFrontRightChassisAngularOffset); - - m_rearLeft = new MAXSwerveModule( - SwerveConstants.kRearLeftDrivingCanId, - SwerveConstants.kRearLeftTurningCanId, - SwerveConstants.kBackLeftChassisAngularOffset); - - m_rearRight = new MAXSwerveModule( - SwerveConstants.kRearRightDrivingCanId, - SwerveConstants.kRearRightTurningCanId, - SwerveConstants.kBackRightChassisAngularOffset); - } - - swerveDrive = new RevSwerveDrive(m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, - m_gyro); - } - - @Override - public void periodic() { - // Update the odometry in the periodic block - swerveDrive.updateOdometry(); - } - - public void drive(ChassisSpeeds chassisSpeeds) { - double xSpeed = chassisSpeeds.vxMetersPerSecond / SwerveConstants.kMaxSpeedMetersPerSecond; - double ySpeed = chassisSpeeds.vyMetersPerSecond / SwerveConstants.kMaxSpeedMetersPerSecond; - double rotation = chassisSpeeds.omegaRadiansPerSecond / SwerveConstants.kMaxAngularSpeed; - this.drive(xSpeed, ySpeed, rotation, false, false); - } - - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed Speed of the robot in the x direction (forward). - * @param ySpeed Speed of the robot in the y direction (sideways). - * @param rot Angular rate of the robot. - * @param fieldRelative Whether the provided x and y speeds are relative to the - * field. - * @param rateLimit Whether to enable rate limiting for smoother control. - */ - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { - swerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative, rateLimit); - } - - public void brake() { - swerveDrive.setX(); - } - - /** - * Sets the swerve ModuleStates. - * - * @param desiredStates The desired SwerveModule states. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, SwerveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(desiredStates[0]); - m_frontRight.setDesiredState(desiredStates[1]); - m_rearLeft.setDesiredState(desiredStates[2]); - m_rearRight.setDesiredState(desiredStates[3]); - } - - /** Resets the drive encoders to currently read a position of 0. */ - public void resetEncoders() { - m_frontLeft.resetEncoders(); - m_frontRight.resetEncoders(); - m_rearLeft.resetEncoders(); - m_rearRight.resetEncoders(); - } - - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - // if sideways became for/backward somehow, change it to 0 degree - m_gyro.setYaw(0); - } - - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - // public double getTurnRate() { - // double[] vels = new double[3]; - // m_gyro.getRawGyro(vels); - // return vels[2] * (SwerveConstants.kGyroReversed ? -1.0 : 1.0); - // } - // yee haw brother - - // stop driving(for future commands and driveCommand) - public void stopDrive() { - this.drive(0, 0, 0, SwerveConstants.kFieldRelative, SwerveConstants.kRateLimit); - } - - private final Pigeon2SimState imuSim = m_gyro.getSimState(); - - @Override - public void simulationPeriodic() { - var prevPos = new SwerveDriveWheelPositions(new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }).copy(); - m_frontLeft.update(0.2); - m_frontRight.update(0.2); - m_rearLeft.update(0.2); - m_rearRight.update(0.2); - var currpos = new SwerveDriveWheelPositions(new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }).copy(); - var twist = SwerveConstants.kDriveKinematics.toTwist2d(prevPos, currpos); - imuSim.addYaw(Math.toDegrees(twist.dtheta)); - } - - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - public void resetOdometry(Pose2d pose2d) { - swerveDrive.resetOdometry(pose2d); - } - - public ChassisSpeeds getChassisSpeeds() { - return swerveDrive.getChassisSpeeds(); - } + // Create MAXSwerveModules + private ISwerveModule m_frontLeft; + + private ISwerveModule m_frontRight; + + private ISwerveModule m_rearLeft; + + private ISwerveModule m_rearRight; + + // Create gyro + private final Pigeon2 m_gyro = new Pigeon2(SwerveConstants.kGyroCanId); + + // Create RevSwerveDrive + private final RevSwerveDrive swerveDrive; + + /** Creates a new DriveSubsystem. */ + public DriveSubsystem() { + if (Robot.isSimulation()) { + m_frontLeft = new SimSwerveModule(); + m_frontRight = new SimSwerveModule(); + m_rearLeft = new SimSwerveModule(); + m_rearRight = new SimSwerveModule(); + } else { + m_frontLeft = new MAXSwerveModule(SwerveConstants.kFrontLeftDrivingCanId, + SwerveConstants.kFrontLeftTurningCanId, SwerveConstants.kFrontLeftChassisAngularOffset); + + m_frontRight = new MAXSwerveModule(SwerveConstants.kFrontRightDrivingCanId, + SwerveConstants.kFrontRightTurningCanId, SwerveConstants.kFrontRightChassisAngularOffset); + + m_rearLeft = new MAXSwerveModule(SwerveConstants.kRearLeftDrivingCanId, + SwerveConstants.kRearLeftTurningCanId, SwerveConstants.kBackLeftChassisAngularOffset); + + m_rearRight = new MAXSwerveModule(SwerveConstants.kRearRightDrivingCanId, + SwerveConstants.kRearRightTurningCanId, SwerveConstants.kBackRightChassisAngularOffset); + } + + swerveDrive = new RevSwerveDrive(m_frontLeft, m_frontRight, m_rearLeft, m_rearRight, m_gyro); + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + swerveDrive.updateOdometry(); + } + + public void drive(ChassisSpeeds chassisSpeeds) { + double xSpeed = chassisSpeeds.vxMetersPerSecond / SwerveConstants.kMaxSpeedMetersPerSecond; + double ySpeed = chassisSpeeds.vyMetersPerSecond / SwerveConstants.kMaxSpeedMetersPerSecond; + double rotation = chassisSpeeds.omegaRadiansPerSecond / SwerveConstants.kMaxAngularSpeed; + this.drive(xSpeed, ySpeed, rotation, false, false); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed + * Speed of the robot in the x direction (forward). + * @param ySpeed + * Speed of the robot in the y direction (sideways). + * @param rot + * Angular rate of the robot. + * @param fieldRelative + * Whether the provided x and y speeds are relative to the field. + * @param rateLimit + * Whether to enable rate limiting for smoother control. + */ + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { + swerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative, rateLimit); + } + + public void brake() { + swerveDrive.setX(); + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates + * The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); + m_frontRight.setDesiredState(desiredStates[1]); + m_rearLeft.setDesiredState(desiredStates[2]); + m_rearRight.setDesiredState(desiredStates[3]); + } + + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + m_frontLeft.resetEncoders(); + m_frontRight.resetEncoders(); + m_rearLeft.resetEncoders(); + m_rearRight.resetEncoders(); + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + // if sideways became for/backward somehow, change it to 0 degree + m_gyro.setYaw(0); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + // public double getTurnRate() { + // double[] vels = new double[3]; + // m_gyro.getRawGyro(vels); + // return vels[2] * (SwerveConstants.kGyroReversed ? -1.0 : 1.0); + // } + // yee haw brother + + // stop driving(for future commands and driveCommand) + public void stopDrive() { + this.drive(0, 0, 0, SwerveConstants.kFieldRelative, SwerveConstants.kRateLimit); + } + + private final Pigeon2SimState imuSim = m_gyro.getSimState(); + public Pose2d simpos = new Pose2d(); + @Override + public void simulationPeriodic() { + var prevPos = new SwerveDriveWheelPositions(new SwerveModulePosition[]{m_frontLeft.getPosition(), + m_frontRight.getPosition(), m_rearLeft.getPosition(), m_rearRight.getPosition()}).copy(); + m_frontLeft.update(0.2); + m_frontRight.update(0.2); + m_rearLeft.update(0.2); + m_rearRight.update(0.2); + var currpos = new SwerveDriveWheelPositions(new SwerveModulePosition[]{m_frontLeft.getPosition(), + m_frontRight.getPosition(), m_rearLeft.getPosition(), m_rearRight.getPosition()}).copy(); + var twist = SwerveConstants.kDriveKinematics.toTwist2d(prevPos, currpos); + simpos = getPose().exp(twist); + imuSim.addYaw(Math.toDegrees(twist.dtheta)); + } + + public Pose2d getPose() { + return swerveDrive.getPose(); + } + + public void resetOdometry(Pose2d pose2d) { + swerveDrive.resetOdometry(pose2d); + } + + public ChassisSpeeds getChassisSpeeds() { + return swerveDrive.getChassisSpeeds(); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java index 1cab5d38..8bf6e382 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java @@ -88,8 +88,6 @@ public void periodic() { SmartDashboard.putNumberArray("Vision/flucialIDs", flucialIDs); NT4Util.putPose3d("Vision/poseList", posList); - - // Puting the estimated pose to the network table var estimatedPose = this.getEstimatedGlobalPose(this.drive.getPose()); if (estimatedPose.isPresent()) { @@ -134,7 +132,7 @@ private void simulationInit() { @Override public void simulationPeriodic() { - visionSim.update(drive.getPose()); + visionSim.update(drive.simpos); // Slight hack but fixes latency issue } }