diff --git a/.vscode/settings.json b/.vscode/settings.json index e360e69..fec83f7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,7 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m", + "java.checkstyle.configuration": "${workspaceFolder}\\buildSrc\\checkstyle.xml", + "java.checkstyle.version": "10.8.0" } diff --git a/Logs/CougarRobot_20230321_152530.log.lck b/Logs/CougarRobot_20230321_152530.log.lck new file mode 100644 index 0000000..e69de29 diff --git a/scripteditor/src/main/java/gui/MainWindow.java b/scripteditor/src/main/java/gui/MainWindow.java index 24fee6f..e5defce 100644 --- a/scripteditor/src/main/java/gui/MainWindow.java +++ b/scripteditor/src/main/java/gui/MainWindow.java @@ -164,8 +164,8 @@ public void onClick() { try { logoImage = ImageIO.read(new File("scripteditor/src/resources/logo.png")); frame.setIconImage(new ImageIcon("scripteditor/src/resources/Icon.png").getImage()); - setFieldImage(ImageIO.read(new File("scripteditor/src/resources/47.350364343 FieldImage.png")), - "47.350364343 FieldImage"); + setFieldImage(ImageIO.read(new File("scripteditor/src/resources/21.1215992958 FieldImage.png")), + "21.1215992958 FieldImage.png"); } catch (IOException e) { e.printStackTrace(); } diff --git a/scripteditor/src/resources/21.1215992958 FieldImage.png b/scripteditor/src/resources/21.1215992958 FieldImage.png new file mode 100644 index 0000000..daa0a11 Binary files /dev/null and b/scripteditor/src/resources/21.1215992958 FieldImage.png differ diff --git a/src/main/java/team1403/lib/core/CougarRobot.java b/src/main/java/team1403/lib/core/CougarRobot.java index 64bb603..e99ba16 100644 --- a/src/main/java/team1403/lib/core/CougarRobot.java +++ b/src/main/java/team1403/lib/core/CougarRobot.java @@ -97,6 +97,13 @@ public final void changeMode(Mode mode) { startMode(mode); } + /** + * Adds functationality to the robot init method found in WpiLibRobotAdapter. + */ + public void robotInit() { + + } + /** * Adds functionality to the teleop init method found in WpiLibRobotAdapter */ diff --git a/src/main/java/team1403/lib/core/WpiLibRobotAdapter.java b/src/main/java/team1403/lib/core/WpiLibRobotAdapter.java index 115de66..d0a1584 100644 --- a/src/main/java/team1403/lib/core/WpiLibRobotAdapter.java +++ b/src/main/java/team1403/lib/core/WpiLibRobotAdapter.java @@ -105,6 +105,8 @@ public void robotInit() { m_cougarRobot = m_cougarRobotFactory.apply(parameters); m_schedulerTimer = new BaseTimer("SchedulerTimer", clock); m_periodTimer = new BaseTimer("LoopPeriodTimer", clock); + + m_cougarRobot.robotInit(); } diff --git a/src/main/java/team1403/lib/device/DeviceFactory.java b/src/main/java/team1403/lib/device/DeviceFactory.java index 970f1c7..0a0164c 100644 --- a/src/main/java/team1403/lib/device/DeviceFactory.java +++ b/src/main/java/team1403/lib/device/DeviceFactory.java @@ -8,6 +8,7 @@ * *

Subsystems create their devices through this factory. */ + public interface DeviceFactory { /** * Creates a PowerDistributor for monitoring the power panel. @@ -76,12 +77,13 @@ public AdvancedMotorController makeTalonSrx( * * @param name The name of the new device instance. * @param deviceNumber The CAN bus channel the motor controller is on. - * @param mode The control mode for the TalonFX * @param logger The logger to use with the new instance. * * @return a new MotorController for a TalonFX. */ - public AdvancedMotorController makeCougarTalonFx(String name, int deviceNumber, CougarLogger logger); + + public AdvancedMotorController makeCougarTalonFx(String name, int deviceNumber, + CougarLogger logger); /** * Creates a VictorSpPwm MotorController. diff --git a/src/main/java/team1403/lib/device/GyroscopeDevice.java b/src/main/java/team1403/lib/device/GyroscopeDevice.java index 4b10591..b46f726 100644 --- a/src/main/java/team1403/lib/device/GyroscopeDevice.java +++ b/src/main/java/team1403/lib/device/GyroscopeDevice.java @@ -67,4 +67,6 @@ public default double getHeading() { public default Rotation2d getRotation2d() { return Rotation2d.fromDegrees(getHeading()); } + + } \ No newline at end of file diff --git a/src/main/java/team1403/lib/subsystems/BuiltinSubsystem.java b/src/main/java/team1403/lib/subsystems/BuiltinSubsystem.java index 51c6322..0adf69d 100644 --- a/src/main/java/team1403/lib/subsystems/BuiltinSubsystem.java +++ b/src/main/java/team1403/lib/subsystems/BuiltinSubsystem.java @@ -37,10 +37,10 @@ public BuiltinSubsystem(CougarLibInjectedParameters injectedParameters, @Override public void periodic() { - SmartDashboard.putNumber("BuiltinDevices.Pdp.Temp", m_pdp.getCelsius()); - SmartDashboard.putNumber("BuiltinDevices.Pdp.Amps", m_pdp.getAmps()); - SmartDashboard.putNumber("BuiltinDevices.Pdp.Volts", m_pdp.getVoltage()); - SmartDashboard.putNumber("BuiltinDevices.Pdp.Joules", m_pdp.getTotalJoules()); + // SmartDashboard.putNumber("BuiltinDevices.Pdp.Temp", m_pdp.getCelsius()); + // SmartDashboard.putNumber("BuiltinDevices.Pdp.Amps", m_pdp.getAmps()); + // SmartDashboard.putNumber("BuiltinDevices.Pdp.Volts", m_pdp.getVoltage()); + // SmartDashboard.putNumber("BuiltinDevices.Pdp.Joules", m_pdp.getTotalJoules()); // Let's show memory usage (in MB) for curiosity. // There doesnt seem to be a wawy to see in-use so we will show free. diff --git a/src/main/java/team1403/robot/chargedup/AutoManager.java b/src/main/java/team1403/robot/chargedup/AutoManager.java index c8a0a5c..151fe9e 100644 --- a/src/main/java/team1403/robot/chargedup/AutoManager.java +++ b/src/main/java/team1403/robot/chargedup/AutoManager.java @@ -2,21 +2,20 @@ import java.util.List; -import com.revrobotics.CANSparkMax.IdleMode; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import team1403.robot.chargedup.StateManager.GamePiece; import team1403.robot.chargedup.arm.ArmStateGroup; import team1403.robot.chargedup.arm.ArmSubsystem; import team1403.robot.chargedup.arm.RunIntake; @@ -28,120 +27,140 @@ public class AutoManager { static private AutoManager m_instance; - static private final TrajectoryConfig m_trajectoryConfig = new TrajectoryConfig(RobotConfig.Swerve.kMaxSpeed, - 3).setKinematics(RobotConfig.Swerve.kDriveKinematics); - private AutoManager() {} + private final TrajectoryConfig m_trajectoryConfig = new TrajectoryConfig(4, + 2).setKinematics(RobotConfig.Swerve.kDriveKinematics); + + private final PIDController xController = new PIDController( + RobotConfig.Swerve.kPTranslation, + RobotConfig.Swerve.kITranslation, + RobotConfig.Swerve.kDTranslation); + + private final PIDController yController = new PIDController( + RobotConfig.Swerve.kPTranslation, + RobotConfig.Swerve.kITranslation, + RobotConfig.Swerve.kDTranslation); + private final ProfiledPIDController thetaController = new ProfiledPIDController( + 4, + RobotConfig.Swerve.kIAutoTurning, + RobotConfig.Swerve.kDAutoTurning, + RobotConfig.Swerve.kThetaControllerConstraints); + + private SwerveControllerCommand grid3Trajectory1; + private SwerveControllerCommand grid3Trajectory2; + private SwerveControllerCommand grid3Trajectory3; + + private AutoManager() { + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } public static AutoManager getInstance() { - if(m_instance == null) { + if (m_instance == null) { m_instance = new AutoManager(); } return m_instance; } - /** - * Command for grids 1 and 3. - * @param swerve - * @param arm - * @return the command - */ - Command getSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) { - //Config navx - swerve.zeroGyroscope(); - swerve.setGyroscopeOffset(199); - - Rotation2d rotation = swerve.getGyroscopeRotation(); - - // 2. Generate trajectory - final Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, rotation), - List.of( - new Translation2d(0.2, 0), - new Translation2d(0.4, 0), - new Translation2d(0.6, 0), - new Translation2d(0.8, 0), - new Translation2d(0.9, 0)), - new Pose2d(1, 0, rotation), - m_trajectoryConfig); - - Trajectory trajectoryTwo = TrajectoryGenerator.generateTrajectory( - new Pose2d(1, 0, rotation), - List.of( - new Translation2d(1.5, 0), - new Translation2d(2, 0), - new Translation2d(2.5, 0), - new Translation2d(3, 0)), - new Pose2d(3.5, 0, rotation), - m_trajectoryConfig); - - // 3. Define PID controllers for tracking trajectory - PIDController xController = new PIDController( - RobotConfig.Swerve.kPTranslation, - RobotConfig.Swerve.kITranslation, - RobotConfig.Swerve.kDTranslation); - PIDController yController = new PIDController( - RobotConfig.Swerve.kPTranslation, - RobotConfig.Swerve.kITranslation, - RobotConfig.Swerve.kDTranslation); - ProfiledPIDController thetaController = new ProfiledPIDController( - 0, - RobotConfig.Swerve.kIAutoTurning, - RobotConfig.Swerve.kDAutoTurning, - RobotConfig.Swerve.kThetaControllerConstraints); + public void init(SwerveSubsystem swerve) { + grid3Trajectory1 = new SwerveControllerCommand( + TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + List.of( + new Translation2d(-1, -0.3), + new Translation2d(-2, -0.3)), + new Pose2d(-3, 0, Rotation2d.fromDegrees(180)), + m_trajectoryConfig), + swerve::getPose, + xController, + yController, + thetaController, + swerve); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - SwerveControllerCommand swerveControllerCommandOne = new SwerveControllerCommand( - trajectory, + grid3Trajectory2 = new SwerveControllerCommand( + TrajectoryGenerator.generateTrajectory( + new Pose2d(-3, 0, Rotation2d.fromDegrees(180)), + List.of( + new Translation2d(-4.8, 0.7), + new Translation2d(-4.9, -0.7), + new Translation2d(-4.7, -0.2)), + new Pose2d(-4, 0.05, Rotation2d.fromDegrees(225)), + m_trajectoryConfig), swerve::getPose, xController, yController, thetaController, swerve); - SwerveControllerCommand swerveControllerCommandTwo = new SwerveControllerCommand( - trajectoryTwo, - swerve::getPose, - xController, - yController, - thetaController, - swerve); - - return new SequentialCommandGroup( - // new SequentialMoveArmCommand(m_arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false), - new RunIntake(arm, 1), - swerveControllerCommandOne, - // new SetpointArmCommand(m_arm, ArmStateGroup.getTuck(), false), - swerveControllerCommandTwo - ); + grid3Trajectory3 = new SwerveControllerCommand( + TrajectoryGenerator.generateTrajectory( + new Pose2d(-4, -0.3, Rotation2d.fromDegrees(225)), + List.of( + new Translation2d(-2.5, -0.5), + new Translation2d(-0.5, -0.5)), + new Pose2d(0.1, -1.05, Rotation2d.fromDegrees(4)), + m_trajectoryConfig), + swerve::getPose, + xController, + yController, + thetaController, + swerve); } + /** + * Command for grids 3. + * + * @param swerve + * @param arm + * @return the command + */ + public Command getRightGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) { + swerve.setSpeedLimiter(1); + - public Command getAlternateSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) { + return new SequentialCommandGroup( + new SequentialMoveArmCommand(arm, + StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false), + new RunIntake(arm, 1), + new ParallelCommandGroup( + new SequentialCommandGroup( + new WaitCommand(0.1), + new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false), + new WaitCommand(1.8), + new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CUBE)), + new SetpointArmCommand(arm, () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), + true), + new RunIntake(arm, 3, 1.5), + new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), true), + new WaitCommand(1.2), + new SetpointArmCommand(arm, () -> StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false) + ), + new SequentialCommandGroup( + grid3Trajectory1, + grid3Trajectory2, + grid3Trajectory3)), + new RunIntake(arm, -1)); + } + + public Command getTimedSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) { swerve.setSpeedLimiter(1); return new SequentialCommandGroup( - new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false), - new RunIntake(arm, 1), - new TimedDrive(swerve, 1, new ChassisSpeeds(-2, 0, 0)), - new ParallelCommandGroup( - new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false), - new TimedDrive(swerve,6.5, new ChassisSpeeds(-2, 0, 0)) - ) - ); + new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false), + new RunIntake(arm, 1), + new TimedDrive(swerve, 1, new ChassisSpeeds(-2, 0, 0)), + new ParallelCommandGroup( + new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false), + new TimedDrive(swerve, 6.5, new ChassisSpeeds(-2, 0, 0)))); } public Command getMiddleGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) { - swerve.setSpeedLimiter(1); - return new SequentialCommandGroup( - new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false), - new RunIntake(arm, 1), - new TimedDrive(swerve, 1, new ChassisSpeeds(-2, -0.5, 0.2)), - new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false), - new TimedDrive(swerve, 2.5, new ChassisSpeeds(-4.5, 0.1, 0)), - new InstantCommand(() -> swerve.setXModeEnabled(true)) - ); + swerve.setSpeedLimiter(1); + return new SequentialCommandGroup( + new TimedDrive(swerve, 3.4, new ChassisSpeeds(-4, 0, 0)), + new TimedDrive(swerve, 1.6, new ChassisSpeeds(0, -4, 0)), + new TimedDrive(swerve, 1.8, new ChassisSpeeds(4, 0, 0)), + new InstantCommand(() -> swerve.setXModeEnabled(true))); } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java b/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java index 3a9fd85..cc3c6c6 100644 --- a/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java +++ b/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java @@ -2,41 +2,36 @@ import java.util.List; -import com.revrobotics.CANSparkMax.IdleMode; - -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PS4Controller; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; + import team1403.lib.core.CougarLibInjectedParameters; import team1403.lib.core.CougarRobot; import team1403.lib.util.CougarLogger; -import team1403.robot.chargedup.cse.CougarScriptObject; -import team1403.robot.chargedup.cse.CougarScriptReader; -import team1403.robot.chargedup.swerve.SwerveAutoBalanceYaw; -import team1403.robot.chargedup.swerve.SwerveCommand; -import team1403.robot.chargedup.swerve.SwerveDrivePath; -import team1403.robot.chargedup.swerve.SwerveSubsystem; import team1403.robot.chargedup.RobotConfig.Operator; import team1403.robot.chargedup.StateManager.GamePiece; -import team1403.robot.chargedup.StateManager.LED; -import team1403.robot.chargedup.arm.ArmState; import team1403.robot.chargedup.arm.ArmStateGroup; import team1403.robot.chargedup.arm.ArmSubsystem; import team1403.robot.chargedup.arm.ManualArmCommand; import team1403.robot.chargedup.arm.RunIntake; import team1403.robot.chargedup.arm.SequentialMoveArmCommand; import team1403.robot.chargedup.arm.SetpointArmCommand; -import team1403.robot.chargedup.arm.UpdateArmState; +import team1403.robot.chargedup.cse.CougarScriptObject; +import team1403.robot.chargedup.cse.CougarScriptReader; +import team1403.robot.chargedup.photonvision.PhotonVisionSubsystem; +import team1403.robot.chargedup.swerve.SwerveAutoBalanceYaw; +import team1403.robot.chargedup.swerve.SwerveCommand; +import team1403.robot.chargedup.swerve.SwerveDrivePath; +import team1403.robot.chargedup.swerve.SwerveSubsystem; /** * The heart of the robot. @@ -57,7 +52,6 @@ public class CougarRobotImpl extends CougarRobot { * Constructor. * * @param parameters Standard framework injected parameters. - * @param config Our robot's custom configuration values. */ public CougarRobotImpl(CougarLibInjectedParameters parameters) { super(parameters); @@ -67,23 +61,26 @@ public CougarRobotImpl(CougarLibInjectedParameters parameters) { // m_builtins = new BuiltinSubsystem(parameters, logger); m_arm = new ArmSubsystem(parameters); m_swerveSubsystem = new SwerveSubsystem(parameters); - CameraServer.startAutomaticCapture(); - // m_visionSubsystem = new PhotonVisionSubsystem(parameters); - // m_lightSubsystem = new LightSubsystem(parameters); - registerAutoCommands(); + m_visionSubsystem = new PhotonVisionSubsystem(); } + @Override + public void robotInit() { + AutoManager.getInstance().init(m_swerveSubsystem); + super.robotInit(); + + } @Override public Command getAutonomousCommand() { CommandScheduler.getInstance().removeDefaultCommand(m_swerveSubsystem); CommandScheduler.getInstance().removeDefaultCommand(m_arm); - return AutoManager.getInstance().getAlternateSideGridCommand(m_swerveSubsystem, m_arm); + return AutoManager.getInstance().getRightGridCommand(m_swerveSubsystem, m_arm); } @Override public void teleopInit() { - // m_swerveSubsystem.setGyroRollOffset(m_swerveSubsystem.getGyroRoll()); + m_swerveSubsystem.setSpeedLimiter(0.6); configureOperatorInterface(); configureDriverInterface(); } @@ -92,7 +89,7 @@ public void teleopInit() { * Configures the driver commands and their bindings. */ public void configureDriverInterface() { - XboxController xboxDriver = getJoystick("Driver", RobotConfig.Driver.pilotPort); + XboxController driveController = getXboxJoystick("Driver", RobotConfig.Driver.pilotPort); SwerveAutoBalanceYaw autoBalanceYaw = new SwerveAutoBalanceYaw(m_swerveSubsystem); // The controls are for field-oriented driving: @@ -102,45 +99,39 @@ public void configureDriverInterface() { // Setting default command of swerve subsystem m_swerveSubsystem.setDefaultCommand(new SwerveCommand( m_swerveSubsystem, - () -> -deadband(xboxDriver.getLeftX(), 0.05), - () -> -deadband(xboxDriver.getLeftY(), 0.05), - () -> -deadband(xboxDriver.getRightX(), 0.05), - () -> xboxDriver.getYButtonReleased())); - - new Trigger(() -> xboxDriver.getLeftBumper()).onTrue( - new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.2))); - - new Trigger(() -> xboxDriver.getLeftBumper()).onFalse( - new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.6))); + () -> -deadband(driveController.getLeftX(), 0), + () -> -deadband(driveController.getLeftY(), 0), + () -> -deadband(driveController.getRightX(), 0), + () -> driveController.getYButton())); - new Trigger(() -> xboxDriver.getRightBumper()).onTrue( + new Trigger(() -> driveController.getRightBumper()).onTrue( new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.8))); - - new Trigger(() -> xboxDriver.getRightBumper()).onFalse( + + new Trigger(() -> driveController.getRightBumper()).onFalse( new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.6))); - new Trigger(() -> xboxDriver.getLeftTriggerAxis() >= 0.08).onTrue( - new InstantCommand(() -> m_swerveSubsystem.setPivotAroundOneWheel(false))) - .onFalse(new InstantCommand(() -> m_swerveSubsystem.setMiddlePivot())); + new Trigger(() -> driveController.getLeftBumper()).onTrue( + new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.2))); - new Trigger(() -> xboxDriver.getRightTriggerAxis() >= 0.08).onTrue( - new InstantCommand(() -> m_swerveSubsystem.setPivotAroundOneWheel(true))) - .onFalse(new InstantCommand(() -> m_swerveSubsystem.setMiddlePivot())); + new Trigger(() -> driveController.getLeftBumper()).onFalse( + new InstantCommand(() -> m_swerveSubsystem.setSpeedLimiter(0.6))); - new Trigger(() -> xboxDriver.getBButton()).onFalse( + new Trigger(() -> driveController.getBButton()).onFalse( new InstantCommand(() -> m_swerveSubsystem.zeroGyroscope())); - // new Trigger(() -> xboxDriver.getAButton()).whileTrue(autoBalanceYaw); + new Trigger(() -> driveController.getAButton()).whileTrue(autoBalanceYaw); - new Trigger(() -> xboxDriver.getXButton()).onTrue (new InstantCommand(() -> m_swerveSubsystem.setXModeEnabled(true))); - new Trigger(() -> xboxDriver.getXButton()).onFalse(new InstantCommand(() -> m_swerveSubsystem.setXModeEnabled(false))); + new Trigger(() -> driveController.getXButton()) + .onTrue(new InstantCommand(() -> m_swerveSubsystem.setXModeEnabled(true))); + new Trigger(() -> driveController.getXButton()) + .onFalse(new InstantCommand(() -> m_swerveSubsystem.setXModeEnabled(false))); } /** * Configures the operator commands and their bindings. */ public void configureOperatorInterface() { - XboxController xboxOperator = getJoystick("Operator", Operator.pilotPort); + XboxController xboxOperator = getXboxJoystick("Operator", Operator.pilotPort); m_arm.setDefaultCommand(new ManualArmCommand(m_arm, () -> -deadband(xboxOperator.getLeftY(), 0.05), @@ -149,59 +140,66 @@ public void configureOperatorInterface() { () -> deadband(xboxOperator.getRightTriggerAxis(), 0.2), () -> xboxOperator.getRightBumper(), () -> xboxOperator.getLeftBumper())); - + // Intake tipped towards cone // new Trigger(() -> xboxOperator.getAButton()).onFalse( - // new SequentialCommandGroup( - // new UpdateArmState(GamePiece.CONE_TOWARDS), - // new InstantCommand(() -> System.out.println(StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState())), - // new SetpointArmCommand(m_arm, StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), - // true))); + // new SequentialCommandGroup( + // new UpdateArmState(GamePiece.CONE_TOWARDS), + // new InstantCommand(() -> + // System.out.println(StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState())), + // new SetpointArmCommand(m_arm, + // StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), + // true))); new Trigger(() -> xboxOperator.getAButton()).onFalse( - new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_TOWARDS)) + new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_TOWARDS)) .andThen( - new SetpointArmCommand(m_arm, - () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), - false))); + new SetpointArmCommand(m_arm, + () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), + false))); // Intake upright cone new Trigger(() -> xboxOperator.getXButton()).onFalse( - new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_UPRIGHT)) + new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_UPRIGHT)) .andThen( - new SetpointArmCommand(m_arm, - () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), - true))); + new SetpointArmCommand(m_arm, + () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), + true))); // Intake cube new Trigger(() -> xboxOperator.getYButton()).onFalse( - new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CUBE)) + new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CUBE)) .andThen( - new SetpointArmCommand(m_arm, - () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), - true))); + new SetpointArmCommand(m_arm, + () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(), + true))); - //Shelf Intake + // Shelf Intake new Trigger(() -> xboxOperator.getBButton()).onFalse( - new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_TOWARDS)).andThen( - new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup().getSingleShelfIntakeState(), - false) - )); + new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CONE_TOWARDS)) + .andThen(new SetpointArmCommand(m_arm, + () -> StateManager.getInstance().getCurrentArmGroup().getSingleShelfIntakeState(), + false))); new Trigger(() -> xboxOperator.getPOV() == 180).onFalse( new SetpointArmCommand(m_arm, () -> ArmStateGroup.getTuck(), false)); new Trigger(() -> xboxOperator.getPOV() == 0).onFalse( - new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false)); + new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup() + .getHighNodeState(), false)); new Trigger(() -> xboxOperator.getPOV() == 90).onFalse( - new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup().getMiddleNodeState(), false)); + new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup() + .getMiddleNodeState(), + false)); new Trigger(() -> xboxOperator.getPOV() == 270).onFalse( - new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup().getLowNodeState(), false)); - + new SetpointArmCommand(m_arm, () -> StateManager.getInstance().getCurrentArmGroup() + .getLowNodeState(), false)); // lights // new Trigger(() -> xboxOperator.getStartButton()).onTrue( - // new InstantCommand(() -> StateManager.getInstance().updateLEDState(LED.YELLOW))); + // new InstantCommand(() -> + // StateManager.getInstance().updateLEDState(LED.YELLOW))); // new Trigger(() -> xboxOperator.getBackButton()).onTrue( - // new InstantCommand(() -> StateManager.getInstance().updateLEDState(LED.PURPLE))); + // new InstantCommand(() -> + // StateManager.getInstance().updateLEDState(LED.PURPLE))); } /** @@ -292,7 +290,7 @@ private double deadband(double value, double deadband) { * * @return controller for port, though might not be temporarily disconnected. */ - private XboxController getJoystick(String role, int port) { + private XboxController getXboxJoystick(String role, int port) { if (!DriverStation.isJoystickConnected(port)) { DriverStation.silenceJoystickConnectionWarning(true); CougarLogger.getAlwaysOn().warningf("No controller found on port %d for '%s'", @@ -301,10 +299,25 @@ private XboxController getJoystick(String role, int port) { return new XboxController(port); } - // private final BuiltinSubsystem m_builtins; - // private final PhotonVisionSubsystem m_visionSubsystem; + /** + * Get controller and silence warnings if not found. + * + * @param role The role for the port for logging purposes. + * @param port The expected port for the controller. + * + * @return controller for port, though might not be temporarily disconnected. + */ + private PS4Controller getPS4Controller(String role, int port) { + if (!DriverStation.isJoystickConnected(port)) { + DriverStation.silenceJoystickConnectionWarning(true); + CougarLogger.getAlwaysOn().warningf("No controller found on port %d for '%s'", + port, role); + } + return new PS4Controller(port); + } + + private final PhotonVisionSubsystem m_visionSubsystem; private CougarScriptReader m_reader; private final ArmSubsystem m_arm; private final SwerveSubsystem m_swerveSubsystem; - // private final LightSubsystem m_lightSubsystem; } diff --git a/src/main/java/team1403/robot/chargedup/LightSubsystem.java b/src/main/java/team1403/robot/chargedup/LightSubsystem.java index e0d2a4f..1cd34e4 100644 --- a/src/main/java/team1403/robot/chargedup/LightSubsystem.java +++ b/src/main/java/team1403/robot/chargedup/LightSubsystem.java @@ -15,8 +15,7 @@ */ public class LightSubsystem extends CougarSubsystem { - private AddressableLED m_lightsLeft; - private AddressableLED m_lightsRight; + private AddressableLED m_lights; private AddressableLEDBuffer m_ledBuffer; private double m_rainbowFirstPixelHue; @@ -26,13 +25,10 @@ public class LightSubsystem extends CougarSubsystem { public LightSubsystem(CougarLibInjectedParameters injectedParameters) { super("lights", injectedParameters); - m_lightsLeft = new AddressableLED(0); - m_lightsRight = new AddressableLED(1); + m_lights = new AddressableLED(0); m_ledBuffer = new AddressableLEDBuffer(60); - m_lightsLeft.setLength(m_ledBuffer.getLength()); - m_lightsRight.setLength(m_ledBuffer.getLength()); - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); + m_lights.setLength(m_ledBuffer.getLength()); + m_lights.setData(m_ledBuffer); ledBufferClear(); @@ -46,10 +42,8 @@ private void ledBufferClear() { for (var i = 0; i < m_ledBuffer.getLength(); i++) { m_ledBuffer.setRGB(i, 0, 0, 0); } - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); - m_lightsLeft.start(); - m_lightsRight.start(); + m_lights.setData(m_ledBuffer); + m_lights.start(); } /** @@ -63,10 +57,8 @@ private void setPurple() { m_ledBuffer.setRGB(i, 107, 3, 252); } - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); - m_lightsLeft.start(); - m_lightsRight.start(); + m_lights.setData(m_ledBuffer); + m_lights.start(); } /** @@ -80,10 +72,8 @@ private void setYellow() { m_ledBuffer.setRGB(i, 255, 230, 0); } - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); - m_lightsLeft.start(); - m_lightsRight.start(); + m_lights.setData(m_ledBuffer); + m_lights.start(); } @@ -102,10 +92,8 @@ private void rainbow() { // Check bounds m_rainbowFirstPixelHue %= 180; - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); - m_lightsLeft.start(); - m_lightsRight.start(); + m_lights.setData(m_ledBuffer); + m_lights.start(); } private void monty() { @@ -115,10 +103,8 @@ private void monty() { m_ledBuffer.setHSV(i, hue[j], 255, 128); } } - m_lightsLeft.setData(m_ledBuffer); - m_lightsRight.setData(m_ledBuffer); - m_lightsLeft.start(); - m_lightsRight.start(); + m_lights.setData(m_ledBuffer); + m_lights.start(); //right yellow left not } @@ -150,6 +136,6 @@ private void setLEDColor(LED led) { @Override public void periodic() { - // setLEDColor(StateManager.getInstance().getLEDState()); + setLEDColor(StateManager.getInstance().getLEDState()); } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/chargedup/Main.java b/src/main/java/team1403/robot/chargedup/Main.java index 3af9223..be4b0a6 100644 --- a/src/main/java/team1403/robot/chargedup/Main.java +++ b/src/main/java/team1403/robot/chargedup/Main.java @@ -2,6 +2,7 @@ import java.util.function.Function; +import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException; import edu.wpi.first.wpilibj.RobotBase; import team1403.lib.core.CougarLibInjectedParameters; @@ -31,13 +32,17 @@ public static void main(String... args) { // This is going to create our CougarRobotImpl when called. Function cougarFactory = (CougarLibInjectedParameters params) -> { - RobotConfig config = new RobotConfig(); return new CougarRobotImpl(params); }; + try { RobotBase.startRobot( () -> { return new WpiLibRobotAdapter(cougarFactory); }); + } catch (MalformedSplineException e) { + System.err.println(e); + throw e; + } } } diff --git a/src/main/java/team1403/robot/chargedup/RobotConfig.java b/src/main/java/team1403/robot/chargedup/RobotConfig.java index 78b9ce0..27f9761 100644 --- a/src/main/java/team1403/robot/chargedup/RobotConfig.java +++ b/src/main/java/team1403/robot/chargedup/RobotConfig.java @@ -4,7 +4,9 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -14,26 +16,22 @@ import team1403.lib.util.Dimension; import team1403.robot.chargedup.arm.ArmState; - /** * This class holds attributes for the robot configuration. * - *

- * The RobotConfig is broken out into different areas, + *

The RobotConfig is broken out into different areas, * each of which is captured in a class for that area. Each * subsystem has its own independent config. * - *

- * The "electrical" configs are treated separate and independent + *

The "electrical" configs are treated separate and independent * to make it easier to see how the robot should be wired and see * any conflicts since these ports specify their config together. */ public class RobotConfig { - //Variables to used by all subsystems. + // Variables to used by all subsystems. public static final Dimension robotDimensions = new Dimension(0, 0, 0); - public static final Dimension wristDimensions = new Dimension(0, 0, 0); //TODO - + public static final Dimension wristDimensions = new Dimension(0, 0, 0); // TODO public static double kRobotHeight = 32; public static double kHeightFromGround = 33.465; @@ -65,16 +63,15 @@ public static class Swerve { public static final double kTrackWidth = Units.inchesToMeters(17.5); public static final double kWheelBase = Units.inchesToMeters(17.5); - public static final SwerveDriveKinematics kDriveKinematics = - new SwerveDriveKinematics( - // Front left - new Translation2d(kTrackWidth / 2.0, kWheelBase / 2.0), - // Front right - new Translation2d(kTrackWidth / 2.0, -kWheelBase / 2.0), - // Back left - new Translation2d(-kTrackWidth / 2.0, kWheelBase / 2.0), - // Back right - new Translation2d(-kTrackWidth / 2.0, -kWheelBase / 2.0)); + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + // Front left + new Translation2d(kTrackWidth / 2.0, kWheelBase / 2.0), + // Front right + new Translation2d(kTrackWidth / 2.0, -kWheelBase / 2.0), + // Back left + new Translation2d(-kTrackWidth / 2.0, kWheelBase / 2.0), + // Back right + new Translation2d(-kTrackWidth / 2.0, -kWheelBase / 2.0)); public static final double frontLeftEncoderOffset = -(4.669437518323892 - Math.PI); public static final double frontRightEncoderOffset = -(0.009203884727314 + Math.PI); @@ -95,7 +92,8 @@ public static class Swerve { public static final double kMaxSpeed = 14.5; public static final double kMaxAngularSpeed = 50; - // (kMaxSpeed / Math.hypot(kTrackWidth / 2.0, kWheelBase / 2.0)); // 39.795095397 + // (kMaxSpeed / Math.hypot(kTrackWidth / 2.0, kWheelBase / 2.0)); // + // 39.795095397 public static final double kVoltageSaturation = 12.0; public static final double kCurrentLimit = 40.0; @@ -103,54 +101,85 @@ public static class Swerve { public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 2; - public static final TrapezoidProfile.Constraints kThetaControllerConstraints - = new TrapezoidProfile.Constraints( + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints( kMaxAngularSpeed, kMaxAngularAccelerationRadiansPerSecondSquared); } - public static class Vision { + /** + * Constants that deal with vision. + */ + public static class VisionConfig { + public static final Pose2d[] reflectiveTapeLayout = new Pose2d[] { + new Pose2d(new Translation2d(Units.inchesToMeters(20), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(64), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(84), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(124), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(144), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(204), + Units.inchesToMeters(30)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(20), + Units.inchesToMeters(621)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(64), + Units.inchesToMeters(621)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(84), + Units.inchesToMeters(621)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(124), + Units.inchesToMeters(621)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(144), + Units.inchesToMeters(621)), new Rotation2d()), + new Pose2d(new Translation2d(Units.inchesToMeters(204), + Units.inchesToMeters(621)), new Rotation2d()) + }; + public static AprilTagFieldLayout fieldLayout = new AprilTagFieldLayout(Arrays.asList( - new AprilTag(1, (new Pose3d( - Units.inchesToMeters(610.77), - Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)))), - new AprilTag(2, new Pose3d( - Units.inchesToMeters(610.77), - Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI))), - new AprilTag(3,new Pose3d( - Units.inchesToMeters(610.77), - Units.inchesToMeters(174.19), // FIRST's diagram has a typo (it says 147.19) - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI))), - new AprilTag(4, new Pose3d( - Units.inchesToMeters(636.96), - Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), - new Rotation3d(0.0, 0.0, Math.PI))), - new AprilTag(5, new Pose3d( - Units.inchesToMeters(14.25), - Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), - new Rotation3d())), - new AprilTag(6, new Pose3d( - Units.inchesToMeters(610.77), - Units.inchesToMeters(174.19), // FIRST's diagram has a typo (it says 147.19) - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI))), - new AprilTag(7, new Pose3d( - Units.inchesToMeters(610.77), - Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI))), - new AprilTag(8, new Pose3d( - Units.inchesToMeters(40.45), - Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), - new Rotation3d()))), Units.inchesToMeters(651.25), Units.inchesToMeters(315.5)); + new AprilTag(1, (new Pose3d( + Units.inchesToMeters(610.77), + Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)))), + new AprilTag(2, new Pose3d( + Units.inchesToMeters(610.77), + Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag(3, new Pose3d( + Units.inchesToMeters(0), + Units.inchesToMeters(0), // FIRST's diagram has a typo (it says 147.19) + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag(4, new Pose3d( + Units.inchesToMeters(636.96), + Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag(5, new Pose3d( + Units.inchesToMeters(14.25), + Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), + new Rotation3d())), + new AprilTag(6, new Pose3d( + Units.inchesToMeters(8), + Units.inchesToMeters(0), // FIRST's diagram has a typo (it says 147.19) + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag(7, new Pose3d( + Units.inchesToMeters(610.77), + Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag(8, new Pose3d( + Units.inchesToMeters(40.45), + Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), + new Rotation3d()))), + Units.inchesToMeters(651.25), Units.inchesToMeters(315.5)); } /** @@ -160,7 +189,7 @@ public static class Vision { */ public static class CanBus { - //Arm Ports + // Arm Ports public static final int wheelIntakeMotor = 5; public static final int telescopicArmMotor = 4; public static final int leftPivotMotor = 2; @@ -190,15 +219,14 @@ public static class CanBus { */ public static class RioPorts { - - public static final int kWristAbsoluteEncoder = 1; //DIO + public static final int kWristAbsoluteEncoder = 1; // DIO - public static final int kArmAbsoluteEncoder = 3; ///Analog + public static final int kArmAbsoluteEncoder = 3; /// Analog - public static final int kArmLimitSwitch = 0; //DIO + public static final int kArmLimitSwitch = 0; // DIO - public static final int kExtensionMinMagneticSwitch = 2; //DIO - public static final int kExtensionMaxMagneticSwitch = 3; //DIO + public static final int kExtensionMinMagneticSwitch = 2; // DIO + public static final int kExtensionMaxMagneticSwitch = 3; // DIO } /** @@ -243,28 +271,29 @@ public static class Driver { */ public static class Arm { - //Pivot + // Pivot public static final int kPArmPivot = 1; public static final int kIArmPivot = 0; public static final int kDArmPivot = 0; - public static double kAbsolutePivotOffset = 180-27.2492019663337 + 17.66 + 7.5 + 10 - 25 +10; + public static double kAbsolutePivotOffset = 355.5375784610152-180+10; public static double kMaxPivotAngle = 256.84208720995503; - public static final double kFrameAngle = 242.71777151085703; + public static final double kFrameAngle = 250.24629; public static final double kFrameClearanceAngle = 234.5; //cone angle public static final double kMinPivotAngle = 132.211; public static final double kPivotAngleMaxAmperage = 40; public static final double kHorizonAngle = 210; + public static final double kPivotLimitSwitchOffset = -6; - //Wrist + // Wrist public static final double kPWristMotor = 0.95; public static final double kIWristMotor = 0; public static final double kDWristMotor = 90; public static final double kMaxWristAngle = 265; public static final double kMinWristAngle = 29.196293229907326; public static final double kWristConversionFactor = 90.0 / 100; - public static final double kAbsoluteWristOffset = 180-136.28614255715357; + public static final double kAbsoluteWristOffset = 180-138.81934247048358; - //Extension + // Extension public static final double kPArmExtension = 0.3; public static final double kIArmExtension = 0; public static final double kDArmExtension = 0; @@ -282,21 +311,23 @@ public static class Arm { public static final double kIIntake = 0; public static final double kDIntake = 0; - //Dimensions - public static final double kBaseArmLength = 31; //37 //28 inches - public static final double kPhysicalArmMaxExtension = kBaseArmLength + kMaxArmExtension; //inches + // Dimensions + public static final double kBaseArmLength = 31; // 37 //28 inches + public static final double kPhysicalArmMaxExtension = + kBaseArmLength + kMaxArmExtension; // inches - public static final double kArmWeight = 16; //Pounds + public static final double kArmWeight = 16; // Pounds } public static class ArmStates { - public static final ArmState coneTowardsFloorIntake = new ArmState(0, 141.346825025, 241.74272039269, 0); //0.039682067930698, 140.0363630009091, 240.55448872511047 - public static final ArmState coneTowardsHighConeNode = new ArmState(22.987735748, 249.49720658743016, 149.88693815473684, 0); - public static final ArmState coneTowardsMiddleNode = new ArmState(8.345230102539062, 264, 149.45086251051157, 0); + public static final ArmState coneTowardsFloorIntake = new ArmState(0, 144.5086341127158, 247.40214774876398, 0); //0.039682067930698, 140.0363630009091, 240.55448872511047 + public static final ArmState coneTowardsHighConeNode = new ArmState(22.987735748, 252.595120962628, 154.96542524635782, 0); + public static final ArmState coneTowardsMiddleNode = new ArmState(8.345230102539062, 264, 156.45086251051157, 0); public static final ArmState singleSubstationIntake = new ArmState(0, 51.3175107829, 241.777313195, 0); public static final ArmState coneTowardsLowNode = new ArmState(0, 80.18787350469682, 245.42271036546947, 0); - public static final ArmState cubeFloorIntake = new ArmState(0.11, 109.01336428783411, 237.26611599405615, 0); + + public static final ArmState cubeFloorIntake = new ArmState(0.099, 109.27611033940275, 244.565611599405615, 0); public static final ArmState cubeHighNode = new ArmState(19.6710987091, 182.465261949, 161.356773014, 0); public static final ArmState cubeMiddleNode = new ArmState(0.05158682167, 177.61028394, 181.482400676, 0); diff --git a/src/main/java/team1403/robot/chargedup/StateManager.java b/src/main/java/team1403/robot/chargedup/StateManager.java index ecc3786..81c163a 100644 --- a/src/main/java/team1403/robot/chargedup/StateManager.java +++ b/src/main/java/team1403/robot/chargedup/StateManager.java @@ -98,15 +98,19 @@ public void updateArmState(GamePiece newGamePiece) { if (newGamePiece == GamePiece.CONE_UPRIGHT) { m_currentArmGroup = m_coneUprightGroup; m_armGroupUsed = 0; + coneAwayCounter++; + updateLEDState(LED.YELLOW); } else if (newGamePiece == GamePiece.CUBE) { m_currentArmGroup = m_cubeGroup; m_armGroupUsed = 1; cubeCounter++; + updateLEDState(LED.PURPLE); } else if (newGamePiece == GamePiece.CONE_TOWARDS) { m_currentArmGroup = m_coneTowardsGroup; m_armGroupUsed = 2; coneTowardsCounter++; + updateLEDState(LED.YELLOW); } } diff --git a/src/main/java/team1403/robot/chargedup/arm/ArmSubsystem.java b/src/main/java/team1403/robot/chargedup/arm/ArmSubsystem.java index 2adef31..328391c 100644 --- a/src/main/java/team1403/robot/chargedup/arm/ArmSubsystem.java +++ b/src/main/java/team1403/robot/chargedup/arm/ArmSubsystem.java @@ -39,6 +39,8 @@ public class ArmSubsystem extends CougarSubsystem { private final PIDController m_pivotPid; private final WpiLimitSwitch m_maxArmLimitSwitch; + private boolean previousLimitSwitchTrigger = true; + // Intake private final CANSparkMax m_intakeMotor; @@ -87,10 +89,15 @@ public ArmSubsystem(CougarLibInjectedParameters injectedParameters) { m_minMagneticSwitch = new DigitalInput(RobotConfig.RioPorts.kExtensionMinMagneticSwitch); m_maxMagneticSwitch = new DigitalInput(RobotConfig.RioPorts.kExtensionMaxMagneticSwitch); + RobotConfig.Arm.kAbsolutePivotOffset = 0; + double difference = RobotConfig.Arm.kMaxPivotAngle - getAbsolutePivotAngle(); + RobotConfig.Arm.kAbsolutePivotOffset = difference; + this.m_pivotAngleSetpoint = getAbsolutePivotAngle(); this.m_wristAngleSetpoint = getAbsoluteWristAngle(); this.m_extensionLengthSetpoint = getExtensionLength(); + } // --------------------------- Setup methods --------------------------- @@ -492,6 +499,7 @@ public boolean isAtSetpoint() { @Override public void periodic() { + // Wrist if (getAbsolutePivotAngle() < RobotConfig.Arm.kFrameAngle) { if (isInWristBounds(m_wristMotor.getEncoder().getPosition()) @@ -508,9 +516,14 @@ public void periodic() { runIntake(m_intakeSpeedSetpoint); // Pivot - // if(isArmSwitchActive()) { - // rezeroPivot(); - // } + if(!isArmSwitchActive() && previousLimitSwitchTrigger) { + System.out.println("sdjklfane;voiajfasdlvnabuegoasd;jvansdinasdj;lcvnaweovnaewufa;sogha"); + RobotConfig.Arm.kAbsolutePivotOffset = 0; + double difference = RobotConfig.Arm.kMaxPivotAngle - getAbsolutePivotAngle() - 10; + RobotConfig.Arm.kAbsolutePivotOffset = difference; + } + + previousLimitSwitchTrigger = isArmSwitchActive(); if ((isInPivotBounds(getAbsolutePivotAngle()) && !isArmSwitchActive()) || isInPivotBounds(this.m_pivotAngleSetpoint)) { diff --git a/src/main/java/team1403/robot/chargedup/arm/RunIntake.java b/src/main/java/team1403/robot/chargedup/arm/RunIntake.java index d1ea102..3864dfb 100644 --- a/src/main/java/team1403/robot/chargedup/arm/RunIntake.java +++ b/src/main/java/team1403/robot/chargedup/arm/RunIntake.java @@ -17,9 +17,18 @@ public class RunIntake extends CommandBase{ private ArmSubsystem m_arm; + private double length; + + public RunIntake(ArmSubsystem arm, double intakeSpeed, double length) { + this.m_arm = arm; + this.m_intakeSpeed = intakeSpeed; + this.length = length; + } + public RunIntake(ArmSubsystem arm, double intakeSpeed) { this.m_arm = arm; this.m_intakeSpeed = intakeSpeed; + this.length = 0.5; } @Override @@ -35,8 +44,7 @@ public void initialize() { @Override public boolean isFinished() { - System.out.println(Timer.getFPGATimestamp() - startTime); - return (Timer.getFPGATimestamp() - startTime) >= 0.5; + return (Timer.getFPGATimestamp() - startTime) >= length; } @Override diff --git a/src/main/java/team1403/robot/chargedup/arm/SetpointArmCommand.java b/src/main/java/team1403/robot/chargedup/arm/SetpointArmCommand.java index 1b5afad..63ede13 100644 --- a/src/main/java/team1403/robot/chargedup/arm/SetpointArmCommand.java +++ b/src/main/java/team1403/robot/chargedup/arm/SetpointArmCommand.java @@ -55,6 +55,6 @@ public void execute() { @Override public boolean isFinished() { m_arm.ignoreExtensionLimit(false); - return m_pivotProfile.isFinished(Timer.getFPGATimestamp() - m_startTime); + return m_pivotProfile.isFinished(Timer.getFPGATimestamp() - m_startTime) && m_arm.isAtSetpoint(); } } diff --git a/src/main/java/team1403/robot/chargedup/photonvision/AutoAprilTagCommand.java b/src/main/java/team1403/robot/chargedup/photonvision/AutoAprilTagCommand.java new file mode 100644 index 0000000..db44d09 --- /dev/null +++ b/src/main/java/team1403/robot/chargedup/photonvision/AutoAprilTagCommand.java @@ -0,0 +1,77 @@ +package team1403.robot.chargedup.photonvision; + +import java.util.List; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import team1403.robot.chargedup.swerve.SwerveDrivePath; +import team1403.robot.chargedup.swerve.SwerveSubsystem; + +/** + * Automatically aligns the robot to the nearest April Tag in the limelight's field of view. + */ +@SuppressWarnings("checkstyle:LocalVariableNameCheck") +public class AutoAprilTagCommand extends CommandBase { + + private final SwerveSubsystem m_drivetrainSubsystem; + private final PhotonVisionSubsystem m_photonVisionSubsystem; + + private Pose2d m_lockedOnTarget; + private SwerveDrivePath m_drivePathCommand; + + /** + * Constructor for the April Tag command. + * + * @param drivetrainSubsystem The drivetrain subsystem used by the command. + * @param photonVisionSubsystem The photonVision subsystem used by the command. + */ + public AutoAprilTagCommand(SwerveSubsystem drivetrainSubsystem, + PhotonVisionSubsystem photonVisionSubsystem) { + + m_drivetrainSubsystem = drivetrainSubsystem; + m_photonVisionSubsystem = photonVisionSubsystem; + } + + @Override + public void initialize() { + double xPoseOfTarget = m_photonVisionSubsystem.getTarget().getX(); + double yPoseOfTarget = m_photonVisionSubsystem.getTarget().getY(); + + double swerveSubsystemRotation = m_drivetrainSubsystem.getGyroscopeRotation().getDegrees(); + double thetaOfTarget; + + //Find the rotation needed to align to the april tag + if ((-90 < swerveSubsystemRotation) && (swerveSubsystemRotation < 90)) { + thetaOfTarget = 1; + } else { + thetaOfTarget = 179; + } + + m_lockedOnTarget = new Pose2d(new Translation2d(xPoseOfTarget, yPoseOfTarget), + new Rotation2d(thetaOfTarget)); + + + m_drivePathCommand = new SwerveDrivePath( + m_drivetrainSubsystem, + m_drivetrainSubsystem.getGyroscopeRotation().getDegrees(), + m_lockedOnTarget.getRotation().getDegrees(), + List.of( + m_drivetrainSubsystem.getPose().getTranslation(), + m_lockedOnTarget.getTranslation())); + + m_drivePathCommand.schedule(); + } + + @Override + public boolean isFinished() { + return m_drivePathCommand.isFinished(); + } + + @Override + public void end(boolean interrupted) { + m_drivePathCommand.end(interrupted); + } +} \ No newline at end of file diff --git a/src/main/java/team1403/robot/chargedup/photonvision/AutoReflectiveTapeCommand.java b/src/main/java/team1403/robot/chargedup/photonvision/AutoReflectiveTapeCommand.java new file mode 100644 index 0000000..75674ee --- /dev/null +++ b/src/main/java/team1403/robot/chargedup/photonvision/AutoReflectiveTapeCommand.java @@ -0,0 +1,80 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package team1403.robot.chargedup.photonvision; + +import java.util.List; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import team1403.robot.chargedup.RobotConfig.VisionConfig; +import team1403.robot.chargedup.swerve.SwerveDrivePath; +import team1403.robot.chargedup.swerve.SwerveSubsystem; + +/** + * Automatically aligns the robot to the nearest refelctive tape grid + * scoring location using the drivetrain's odometry. + */ +public class AutoReflectiveTapeCommand extends CommandBase { + + private SwerveSubsystem m_drivetrain; + private double m_lowestX; + private double m_lowestY; + private SwerveDrivePath m_drivePathCommand; + private Translation2d m_target; + + /** Creates a new AutoReflectiveTapeCommand. */ + public AutoReflectiveTapeCommand(SwerveSubsystem drivetrain) { + m_drivetrain = drivetrain; + addRequirements(drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + //Find the closest reflective tape + m_lowestX = VisionConfig.reflectiveTapeLayout[0].getX(); + m_lowestY = VisionConfig.reflectiveTapeLayout[0].getY(); + for (int i = 1; i < 6; i++) { + if (m_lowestX > VisionConfig.reflectiveTapeLayout[i].getX() + && m_lowestY > VisionConfig.reflectiveTapeLayout[i].getY()) { + m_lowestX = VisionConfig.reflectiveTapeLayout[i].getX(); + m_lowestY = VisionConfig.reflectiveTapeLayout[i].getY(); + } + } + + //Find the rotation needed to align to the reflective tape + double thetaOfTarget; + double swerveSubsystemRotation = m_drivetrain.getGyroscopeRotation().getDegrees(); + if ((-90 < swerveSubsystemRotation) && (swerveSubsystemRotation < 90)) { + thetaOfTarget = 1; + } else { + thetaOfTarget = 179; + } + + //Call the command to align to the reflective tape + m_target = new Translation2d(m_lowestX, m_lowestY); + m_drivePathCommand = new SwerveDrivePath( + m_drivetrain, + m_drivetrain.getGyroscopeRotation().getDegrees(), + thetaOfTarget, + List.of( + m_drivetrain.getPose().getTranslation(), + m_target)); + + m_drivePathCommand.schedule(); + } + + + @Override + public void end(boolean interrupted) { + m_drivePathCommand.end(interrupted); + } + + @Override + public boolean isFinished() { + return m_drivePathCommand.isFinished(); + } +} diff --git a/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionDefault.java b/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionDefault.java new file mode 100644 index 0000000..4fa7ae8 --- /dev/null +++ b/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionDefault.java @@ -0,0 +1,67 @@ +package team1403.robot.chargedup.photonvision; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import team1403.lib.util.SwerveDrivePoseEstimator; +import team1403.robot.chargedup.swerve.SwerveSubsystem; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; + +/** + * The default command for photon vision. + */ +public class PhotonVisionDefault extends CommandBase { + private SwerveSubsystem m_drivetrainSubsystem; + private PhotonVisionSubsystem m_photonVisionSubsystem; + + private PhotonPoseEstimator m_photonPoseEstimator; + private SwerveDrivePoseEstimator m_swervePoseEstimator; + + /** + * Constructor for the default PhotonVision. + * + * @param drivetrainSubsystem The Drivetrain subsystem used by PhotonVision + * default. + * @param photonVisionSubsystem The PhotonVision subsystem used by PhtonVision + * default. + */ + public PhotonVisionDefault(SwerveSubsystem drivetrainSubsystem, + PhotonVisionSubsystem photonVisionSubsystem) { + m_drivetrainSubsystem = drivetrainSubsystem; + m_photonVisionSubsystem = photonVisionSubsystem; + + m_swervePoseEstimator = m_drivetrainSubsystem.getOdometer(); + + m_photonPoseEstimator = m_photonVisionSubsystem.getPhotonPoseEstimator(); + + addRequirements(m_photonVisionSubsystem); + } + + @Override + public void initialize() { + m_photonPoseEstimator.setReferencePose(m_drivetrainSubsystem.getPose()); + } + + @Override + public void execute() { + Optional result = m_photonVisionSubsystem.getPhotonPose(); + if (result.isPresent()) { + Pose2d photonPose = m_photonVisionSubsystem.getFieldLimelightBasedPose(); + double distanceFromCurrentMeasurment = photonPose.getTranslation() + .getDistance(m_drivetrainSubsystem.getPose().getTranslation()); + if (Math.abs(distanceFromCurrentMeasurment) <= 1) { + m_swervePoseEstimator.addVisionMeasurement( + photonPose, Timer.getFPGATimestamp()); + m_photonPoseEstimator.setReferencePose(m_drivetrainSubsystem.getPose()); + + SmartDashboard.putString("Vision Odometry", photonPose.toString()); + } + } + } +} diff --git a/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionSubsystem.java b/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionSubsystem.java index 524c660..1221a54 100644 --- a/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionSubsystem.java +++ b/src/main/java/team1403/robot/chargedup/photonvision/PhotonVisionSubsystem.java @@ -1,54 +1,183 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package team1403.robot.chargedup.photonvision; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import java.util.Optional; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import team1403.lib.core.CougarLibInjectedParameters; -import team1403.lib.core.CougarSubsystem; -import team1403.robot.chargedup.StateManager; -import team1403.robot.chargedup.RobotConfig.Vision; -import team1403.robot.chargedup.StateManager.GamePiece; - -public class PhotonVisionSubsystem extends CougarSubsystem { - private PhotonCamera limeLight; - private PhotonPoseEstimator photonPoseEstimator; - - private final IntegerSubscriber m_coneOrientationSubsriber; - - public PhotonVisionSubsystem(CougarLibInjectedParameters injectedParameters) { - super("Vision Subsystem", injectedParameters); - // Photonvision +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import team1403.robot.chargedup.RobotConfig; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import com.fasterxml.jackson.databind.util.ArrayBuilders.DoubleBuilder; + +/** + * Base subsystem for photon vision. + */ +public class PhotonVisionSubsystem extends SubsystemBase { + private PhotonCamera m_limeLight; + private PhotonPoseEstimator m_photonPoseEstimator; + private Optional m_photonPose; + private Transform3d m_target; + private double targetX; + private DoubleBuilder targetY; + + /** + * Takes in the parmeters needed for photon vision. + * + */ + public PhotonVisionSubsystem() { PortForwarder.add(5800, "photonvision.local", 5800); - limeLight = new PhotonCamera("OV5647"); + + m_limeLight = new PhotonCamera("OV5647"); + + m_limeLight.setPipelineIndex(0); // 0: April Tags // 1: Reflective Tape - limeLight.setPipelineIndex(0); - photonPoseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, PoseStrategy.LOWEST_AMBIGUITY, limeLight, + + m_photonPoseEstimator = new PhotonPoseEstimator(RobotConfig.VisionConfig.fieldLayout, + PoseStrategy.MULTI_TAG_PNP, m_limeLight, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))); + m_photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + if (DriverStation.getAlliance() == Alliance.Red) { + RobotConfig.VisionConfig.fieldLayout.setOrigin(OriginPosition.kRedAllianceWallRightSide); + } else { + RobotConfig.VisionConfig.fieldLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + } + } - //Cone detection - NetworkTableInstance instance = NetworkTableInstance.getDefault(); - NetworkTable coneTable = instance.getTable("conetable"); - m_coneOrientationSubsriber = coneTable.getIntegerTopic("rotcone").subscribe(-1); + /** + * Switches the pipeline of the Photonvision between april tag + * and retroflective tape. + */ + public void switchPipeLine() { + if (m_limeLight.getPipelineIndex() == 0) { + m_limeLight.setPipelineIndex(1); + } else { + m_limeLight.setPipelineIndex(0); + } } - @Override - public void periodic() { - // int coneOrientation = (int) m_coneOrientationSubsriber.get(); - // StateManager.getInstance().updateArmState(GamePiece.fromInt(coneOrientation)); - // super.periodic(); + public Optional getPhotonPose() { + return m_photonPoseEstimator.update(); + } + + public PhotonPoseEstimator getPhotonPoseEstimator() { + return m_photonPoseEstimator; + } + + public Transform3d getTarget() { + return m_target; + } + + /** + * Returns if photon vision sees target + * april tags or reflective tape depending on the set pipline. + * + * @return + * + */ + public boolean hasTarget() { + if (m_limeLight.getLatestResult().hasTargets()) { + return true; + } else { + return false; + } + } + + public Pose2d getLimelightBasedPose() { + if(hasTarget()){ + return new Pose2d(new Translation2d(m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getX(),m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getY()) + , m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getRotation().toRotation2d()); + } + else { + return new Pose2d(); + } + } + + public Pose2d getFieldLimelightBasedPose() { + int AprilTagID = + m_limeLight.getLatestResult().getBestTarget().getFiducialId(); + + double xDistancefromRobottoAprilTag = + m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getX(); + double yDistancefromRobottoAprilTag = + m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getY(); + + double xPositionofAprilTag = + RobotConfig.VisionConfig.fieldLayout.getTagPose( + m_limeLight.getLatestResult().getBestTarget().getFiducialId()).get().getX(); + double yPositionofAprilTag = + RobotConfig.VisionConfig.fieldLayout.getTagPose( + m_limeLight.getLatestResult().getBestTarget().getFiducialId()).get().getY(); + + Rotation2d rotationalComponent = + m_limeLight.getLatestResult().getBestTarget().getBestCameraToTarget().getRotation().toRotation2d(); + + + if(hasTarget()){ + if (DriverStation.getAlliance() == Alliance.Red) { + if (AprilTagID < 5) { + return new Pose2d( + new Translation2d( + xDistancefromRobottoAprilTag + + + xPositionofAprilTag + , + yDistancefromRobottoAprilTag + + + yPositionofAprilTag), + rotationalComponent); + } else { + return new Pose2d( + new Translation2d( + xPositionofAprilTag + - + xDistancefromRobottoAprilTag + , + yPositionofAprilTag + - + yDistancefromRobottoAprilTag), + rotationalComponent); + } + } else { + if (AprilTagID > 4) { + return new Pose2d( + new Translation2d( + xDistancefromRobottoAprilTag + + + xPositionofAprilTag + , + yDistancefromRobottoAprilTag + + + yPositionofAprilTag), + rotationalComponent); + } else { + return new Pose2d( + new Translation2d( + xPositionofAprilTag + - + xDistancefromRobottoAprilTag + , + yPositionofAprilTag + - + yDistancefromRobottoAprilTag), + rotationalComponent); + } + } + } + return new Pose2d(); } } diff --git a/src/main/java/team1403/robot/chargedup/swerve/SwerveAutoBalanceYaw.java b/src/main/java/team1403/robot/chargedup/swerve/SwerveAutoBalanceYaw.java index ef8c701..f53af48 100644 --- a/src/main/java/team1403/robot/chargedup/swerve/SwerveAutoBalanceYaw.java +++ b/src/main/java/team1403/robot/chargedup/swerve/SwerveAutoBalanceYaw.java @@ -23,7 +23,7 @@ public class SwerveAutoBalanceYaw extends CommandBase { public SwerveAutoBalanceYaw(SwerveSubsystem drivetrainSubsystem) { m_drivetrainSubsystem = drivetrainSubsystem; m_xPIDController = new ProfiledPIDController( - 0.225, + 0.1, RobotConfig.Swerve.kITranslation, 0, new TrapezoidProfile.Constraints(200, 200)); @@ -31,21 +31,18 @@ public SwerveAutoBalanceYaw(SwerveSubsystem drivetrainSubsystem) { previousRollValue = 0; rollValue = 0; rollSetpoint = 0; - } @Override public void execute() { - rollVelocity = (previousRollValue - m_drivetrainSubsystem.getGyroRoll()) * 20; rollValue = m_drivetrainSubsystem.getGyroRoll(); - rollSetpoint = rollVelocity + rollValue; - - SmartDashboard.putNumber("roll velocity", rollValue); - System.out.println(rollValue); + rollVelocity = (rollValue - previousRollValue) / 0.2; velocity = m_xPIDController.calculate(rollValue, 0); + SmartDashboard.putNumber("roll velocity", velocity); + m_drivetrainSubsystem.drive(new ChassisSpeeds(velocity, 0, 0), new Translation2d()); @@ -54,11 +51,11 @@ public void execute() { @Override public boolean isFinished() { - return Math.abs(rollValue) < 2; + return rollValue == 0 && Math.abs(rollVelocity) > 3; } @Override - public void end(boolean interuptted) { + public void end(boolean interupted) { m_drivetrainSubsystem.setXModeEnabled(true); } } diff --git a/src/main/java/team1403/robot/chargedup/swerve/SwerveModule.java b/src/main/java/team1403/robot/chargedup/swerve/SwerveModule.java index 9b4bd08..5ad469f 100644 --- a/src/main/java/team1403/robot/chargedup/swerve/SwerveModule.java +++ b/src/main/java/team1403/robot/chargedup/swerve/SwerveModule.java @@ -112,9 +112,9 @@ public void initEncoders() { } private void initSteerMotor() { - m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 100); - m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); - m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20); + m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); + m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); + m_steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); m_steerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); m_steerMotor.setInverted(false); m_steerMotor.enableVoltageCompensation(12); @@ -134,11 +134,11 @@ public void initDriveMotor() { m_driveMotor.setVoltageCompensation(RobotConfig.Swerve.kVoltageSaturation); m_driveMotor.setAmpLimit(RobotConfig.Swerve.kCurrentLimit); m_driveMotor.getCanSparkMaxApi().setPeriodicFramePeriod( - CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 100); + CANSparkMaxLowLevel.PeriodicFrame.kStatus0, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); m_driveMotor.getCanSparkMaxApi().setPeriodicFramePeriod( - CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); + CANSparkMaxLowLevel.PeriodicFrame.kStatus1, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); m_driveMotor.getCanSparkMaxApi().setPeriodicFramePeriod( - CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20); + CANSparkMaxLowLevel.PeriodicFrame.kStatus2, RobotConfig.Swerve.kStatusFrameGeneralPeriodMs); } /** diff --git a/src/main/java/team1403/robot/chargedup/swerve/SwerveSubsystem.java b/src/main/java/team1403/robot/chargedup/swerve/SwerveSubsystem.java index 495b804..eaaa614 100644 --- a/src/main/java/team1403/robot/chargedup/swerve/SwerveSubsystem.java +++ b/src/main/java/team1403/robot/chargedup/swerve/SwerveSubsystem.java @@ -3,6 +3,7 @@ import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -12,7 +13,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import team1403.lib.core.CougarLibInjectedParameters; @@ -53,16 +55,21 @@ public class SwerveSubsystem extends CougarSubsystem { -RobotConfig.Swerve.kTrackWidth / 2.0, RobotConfig.Swerve.kWheelBase / 2.0); - private final PIDController m_driftCorrectionPid = new PIDController(0.37, 0, 0); + private final PIDController m_driftCorrectionPid = new PIDController(0.75, 0, 0); private double m_desiredHeading = 0; private double m_speedLimiter = 0.6; private Translation2d m_offset; + private double m_yawOffset; + private double m_rollOffset; + private double m_calc = 0; private boolean m_isXModeEnabled = false; + private Field2d m_field2d; + /** * Creates a new {@link SwerveSubsystem}. * Instantiates the 4 {@link SwerveModule}s, @@ -92,10 +99,8 @@ public SwerveSubsystem(CougarLibInjectedParameters parameters) { CanBus.backRightEncoderId, Swerve.backRightEncoderOffset, logger), }; - m_odometer = new SwerveDrivePoseEstimator(Swerve.kDriveKinematics, getGyroscopeRotation(), - getModulePositions(), new Pose2d(0, 0, new Rotation2d(0))); - addDevice(m_navx2.getName(), m_navx2); + new Thread(() -> { while (m_navx2.isCalibrating()) { try { @@ -105,15 +110,24 @@ public SwerveSubsystem(CougarLibInjectedParameters parameters) { } } zeroGyroscope(); - }).start(); + }).start(); + m_odometer = new SwerveDrivePoseEstimator(RobotConfig.Swerve.kDriveKinematics, + new Rotation2d(), getModulePositions(), new Pose2d(0, 0, new Rotation2d(0)), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + m_desiredHeading = getGyroscopeRotation().getDegrees(); setRobotRampRate(0.0); setRobotIdleMode(IdleMode.kBrake); m_offset = new Translation2d(); + m_rollOffset = -m_navx2.getRoll(); + m_yawOffset = 0; + m_field2d = new Field2d(); + SmartDashboard.putData("Field", m_field2d); } /** @@ -186,17 +200,20 @@ public void setRobotIdleMode(IdleMode mode) { public void zeroGyroscope() { // tracef("zeroGyroscope %f", getGyroscopeRotation()); m_navx2.reset(); - m_navx2.setAngleOffset(0); m_desiredHeading = 0; } + public SwerveDrivePoseEstimator getPoseEstimator() { + return m_odometer; + } + /** * Set the offset of the gyroscope. * * @param offset offset to set on the gyroscope */ - public void setGyroscopeOffset(double offset) { - m_navx2.setAngleOffset(offset); + public void setYawGyroscopeOffset(double offset) { + m_yawOffset = offset; } /** @@ -221,12 +238,6 @@ public SwerveDrivePoseEstimator getOdometer() { return m_odometer; } - public void updateOdometerWithVision(Pose2d pose) { - if (pose.getTranslation().getDistance(getPose().getTranslation()) < 1) { - m_odometer.addVisionMeasurement(pose, Timer.getFPGATimestamp()); - } - } - /** * Reset the position of the drivetrain odometry. */ @@ -241,7 +252,7 @@ public void resetOdometry() { * @return a Rotation2d object that contains the gyroscope's heading */ public Rotation2d getGyroscopeRotation() { - return m_navx2.getRotation2d(); + return m_navx2.getRotation2d().minus(Rotation2d.fromDegrees(m_yawOffset)); } /** @@ -250,7 +261,7 @@ public Rotation2d getGyroscopeRotation() { * @return a double representing the roll of robot in degrees */ public double getGyroRoll() { - return m_navx2.getRoll(); + return m_navx2.getRoll() + m_rollOffset; } /** @@ -274,6 +285,8 @@ public void drive(ChassisSpeeds chassisSpeeds, Translation2d offset) { SmartDashboard.putString("Chassis Speeds", m_chassisSpeeds.toString()); } + + /** * Stops the drivetrain. */ @@ -282,15 +295,12 @@ public void stop() { m_chassisSpeeds = new ChassisSpeeds(); } - public Pose2d getOdometryValue() { - return m_odometer.getEstimatedPosition(); - } - /** * Sets the module speed and heading for all 4 modules. * * @param states an array of states for each module. */ + public void setModuleStates(SwerveModuleState[] states) { SwerveDriveKinematics.desaturateWheelSpeeds( states, Swerve.kMaxSpeed); @@ -430,7 +440,7 @@ public void setXModeEnabled(boolean enabled) { /** * Adds rotational velocity to the chassis speed to compensate for * unwanted changes in gyroscope heading. - * + * * @param chassisSpeeds the given chassisspeeds * @return the corrected chassisspeeds */ @@ -458,7 +468,7 @@ private ChassisSpeeds translationalDriftCorrection(ChassisSpeeds chassisSpeeds) *

* Looks forward one control loop to figure out where the robot * should be given the chassisspeed and backs out a twist command from that. - * + * * @param chassisSpeeds the given chassisspeeds * @return the corrected chassisspeeds */ @@ -480,9 +490,13 @@ private ChassisSpeeds rotationalDriftCorrection(ChassisSpeeds chassisSpeeds) { @Override public void periodic() { SmartDashboard.putNumber("Gyro Reading", getGyroscopeRotation().getDegrees()); - m_odometer.updateWithTime(Timer.getFPGATimestamp(), getGyroscopeRotation(), getModulePositions()); + + m_odometer.update(getGyroscopeRotation(), getModulePositions()); + SmartDashboard.putString("Odometry", m_odometer.getEstimatedPosition().toString()); + SmartDashboard.putNumber("Speed", m_speedLimiter); + SmartDashboard.putNumber("Roll Value", getGyroRoll()); if (this.m_isXModeEnabled) { xMode(); @@ -498,6 +512,5 @@ public void periodic() { SmartDashboard.putNumber("Front Right Absolute Encoder", m_modules[1].getAbsoluteAngle()); SmartDashboard.putNumber("Back Left Absolute Encoder", m_modules[2].getAbsoluteAngle()); SmartDashboard.putNumber("Back Right Absolute Encoder", m_modules[3].getAbsoluteAngle()); - } } diff --git a/src/main/java/team1403/robot/chargedup/swerve/TimedDrive.java b/src/main/java/team1403/robot/chargedup/swerve/TimedDrive.java index b8b42ea..fb01537 100644 --- a/src/main/java/team1403/robot/chargedup/swerve/TimedDrive.java +++ b/src/main/java/team1403/robot/chargedup/swerve/TimedDrive.java @@ -23,7 +23,6 @@ public TimedDrive(SwerveSubsystem swerve, double duration, ChassisSpeeds chassis @Override public void initialize() { this.m_startTime = Timer.getFPGATimestamp(); - System.out.println("Starting: ********************************************"); } @@ -34,7 +33,6 @@ public void execute() { @Override public void end(boolean interrupted) { - System.out.println("HVBfiuasfbhoun--------------------------------------------------------------------"); m_swerve.stop(); super.end(interrupted); } @@ -42,7 +40,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - System.out.println(Timer.getFPGATimestamp() - m_startTime); return Timer.getFPGATimestamp() - m_startTime >= m_duration; } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index a5ffafa..dad3105 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.3.0", + "version": "v2023.4.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-cpp", - "version": "v2023.3", + "version": "v2023.4.2", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -30,12 +30,12 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-java", - "version": "v2023.2.1" + "version": "v2023.4.2" }, { "groupId": "org.photonvision", "artifactId": "PhotonTargeting-java", - "version": "v2023.2.1" + "version": "v2023.4.2" } ] } \ No newline at end of file