diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 178ebb9..c4a17d9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,7 +19,9 @@ import frc.robot.SubsystemChecker.SubsystemType; import frc.robot.commands.BrakeModeDisabled; import frc.robot.commands.CheckArmSetpoints; +import frc.robot.commands.SetBlingCommand; import frc.robot.commands.SyncArmEncoders; +import frc.robot.commands.SyncArmEncodersV2; import frc.robot.commands.SyncSteerEncoders; import frc.robot.config.Config; import frc.robot.robotcontainers.ArmBotContainer; @@ -30,6 +32,7 @@ import frc.robot.robotcontainers.MiniNeoDiffContainer; import frc.robot.robotcontainers.MiniSwerveContainer; import frc.robot.robotcontainers.RobotContainer; +import frc.robot.subsystems.BlingSubsystem; import frc.robot.subsystems.DiffNeoSubsystem; import frc.robot.subsystems.DiffTalonSubsystem; @@ -46,6 +49,8 @@ public class Robot extends TimedRobot { Compressor pcmCompressor = new Compressor(1, PneumaticsModuleType.CTREPCM); + private boolean m_firstDisableAtBootup = true; + @@ -97,8 +102,9 @@ public void robotInit() { } if (SubsystemChecker.canSubsystemConstruct(SubsystemType.SwerveSubsystem)) { + BlingSubsystem.getINSTANCE().setRed(); new SyncSteerEncoders().schedule(); - new WaitCommand(3).andThen(new SyncArmEncoders()).schedule(); + new SyncArmEncodersV2().schedule(); } // Add CommandScheduler to shuffleboard so we can display what commands are scheduled @@ -131,7 +137,15 @@ public void robotPeriodic() { @Override public void disabledInit() { if (brakeModeDisabledCommand != null) - brakeModeDisabledCommand.schedule(); + brakeModeDisabledCommand.schedule(); + + if (SubsystemChecker.canSubsystemConstruct(SubsystemType.BlingSubsystem)) { + if (m_firstDisableAtBootup) { + m_firstDisableAtBootup = false; + } else { + BlingSubsystem.getINSTANCE().setDisabled(); + } + } } @Override diff --git a/src/main/java/frc/robot/SubsystemChecker.java b/src/main/java/frc/robot/SubsystemChecker.java index 408719b..1cc96d1 100644 --- a/src/main/java/frc/robot/SubsystemChecker.java +++ b/src/main/java/frc/robot/SubsystemChecker.java @@ -34,11 +34,12 @@ public enum SubsystemType { */ // RobotID: 0, 2023 Competition robot, unnamed private static SubsystemType[] compBotId0 = new SubsystemType[] { - SubsystemType.SwerveSubsystem, // Chassis unknown + SubsystemType.SwerveSubsystem, SubsystemType.ArmSubsystem, SubsystemType.RelaySubsystem, SubsystemType.VisionNTSubsystem, SubsystemType.GripperSubsystem, + SubsystemType.BlingSubsystem, }; // RobotID: 1, 2022 robot, RapidReact, Clutch diff --git a/src/main/java/frc/robot/commands/PhotonMoveToTarget.java b/src/main/java/frc/robot/commands/PhotonMoveToTarget.java new file mode 100644 index 0000000..498b363 --- /dev/null +++ b/src/main/java/frc/robot/commands/PhotonMoveToTarget.java @@ -0,0 +1,108 @@ +// 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 frc.robot.commands; + +import java.util.List; + +import org.photonvision.*; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.LinearFilter; +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.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DiffTalonSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +public class PhotonMoveToTarget extends CommandBase { + double EXAMPLE_SIZE_HEIGHT = 104.6; + double EXAMPLE_DISTANCE = 1.000; + Pose2d cameraOffset = new Pose2d(new Translation2d(0,0.3), Rotation2d.fromDegrees(0)); + //height of april = 1 foot 3 and 1/4 to bottom of black + /** Creates a new ExampleCommand. */ + DoubleArrayPublisher pubSetPoint; + PhotonCamera camera1; + PIDController xController; + PIDController yController; + PIDController yawController; + Translation2d setPoint; + Rotation2d rotationSetPoint; + LinearFilter filteryaw = LinearFilter.movingAverage(10); + LinearFilter filterX = LinearFilter.movingAverage(10); + LinearFilter filterY = LinearFilter.movingAverage(10); + + + public PhotonMoveToTarget() { + camera1 = new PhotonCamera("OV9281"); + xController = new PIDController(0.5, 0.01, 0); + yController = new PIDController(0.5, 0.01, 0); + yawController = new PIDController(0.02, 0, 0); + pubSetPoint = NetworkTableInstance.getDefault().getTable(("PhotonCamera")).getDoubleArrayTopic("PhotonAprilPoint").publish(PubSubOption.periodic(0.02)); + addRequirements(SwerveSubsystem.getInstance()); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + filterX.reset(); + filterY.reset(); + filteryaw.reset(); + rotationSetPoint = SwerveSubsystem.getInstance().getPose().getRotation(); + setPoint = SwerveSubsystem.getInstance().getPose().getTranslation(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + var result = camera1.getLatestResult(); + Pose2d odometryPose = SwerveSubsystem.getInstance().getPose(); + if (result.hasTargets()){ + List corners = result.getBestTarget().getDetectedCorners(); + double heightSize = (corners.get(0).y - corners.get(3).y + corners.get(1).y - corners.get(2).y)/2; + //System.out.println("tagsize "+heightSize); + double range = EXAMPLE_SIZE_HEIGHT*EXAMPLE_DISTANCE/heightSize; + //System.out.println("range"+range); + Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().getYaw()); + + Translation2d visionXY = new Translation2d(range, yaw); + Translation2d robotRotated = visionXY.rotateBy(cameraOffset.getRotation()); + Translation2d robotToTargetRELATIVE = robotRotated.plus(cameraOffset.getTranslation()); + Translation2d robotToTarget = robotToTargetRELATIVE.rotateBy(odometryPose.getRotation()); + Translation2d feildToTarget = robotToTarget.plus(odometryPose.getTranslation()); + //System.out.println("robot to target "+ robotToTargetRELATIVE.toString()); + //System.out.println("odometry"+ odometryPose.toString()); + Rotation2d fieldOrientedTarget = yaw.rotateBy(odometryPose.getRotation()); + + setPoint = new Translation2d(filterX.calculate(feildToTarget.getX())-1,filterY.calculate(feildToTarget.getY())); + rotationSetPoint = Rotation2d.fromDegrees(filteryaw.calculate(fieldOrientedTarget.getDegrees())); + pubSetPoint.accept(new double[]{setPoint.getX(),setPoint.getY(),rotationSetPoint.getRadians()}); + } + //System.out.println("set "+setPoint); + double xSpeed = xController.calculate(odometryPose.getX(), setPoint.getX()); + double yspeed = yController.calculate(odometryPose.getY(), setPoint.getY()); + double rotation = yawController.calculate(odometryPose.getRotation().getDegrees(), rotationSetPoint.getDegrees()); + SwerveSubsystem.getInstance().drive(xSpeed, yspeed, rotation, true, false); + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + SwerveSubsystem.getInstance().stopMotors(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ResetGyro.java b/src/main/java/frc/robot/commands/ResetGyro.java index abaa116..9892b93 100644 --- a/src/main/java/frc/robot/commands/ResetGyro.java +++ b/src/main/java/frc/robot/commands/ResetGyro.java @@ -6,6 +6,7 @@ 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.SwerveDriveOdometry; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.SwerveSubsystem; @@ -24,8 +25,7 @@ public ResetGyro() { // Called when the command is initially scheduled. @Override public void initialize() { - Pose2d pose = SwerveSubsystem.getInstance().getPose(); - Pose2d newPose = new Pose2d(pose.getTranslation(), new Rotation2d(0)); + Pose2d newPose = new Pose2d(new Translation2d(3, 3), new Rotation2d(0)); SwerveSubsystem.getInstance().resetOdometry(newPose); } diff --git a/src/main/java/frc/robot/commands/SyncArmEncodersV2.java b/src/main/java/frc/robot/commands/SyncArmEncodersV2.java new file mode 100644 index 0000000..3df9a70 --- /dev/null +++ b/src/main/java/frc/robot/commands/SyncArmEncodersV2.java @@ -0,0 +1,150 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.config.ArmConfig; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.BlingSubsystem; + +public class SyncArmEncodersV2 extends CommandBase { + private Timer m_smallTimer = new Timer(); + private Timer m_permanantTimer = new Timer(); + private int m_commandState = 0; + private int m_numSamples = 0; + private double m_sumBotSamples = 0; + private double m_sumTopSamples = 0; + + /** Creates a new SyncSteerEncoders. */ + public SyncArmEncodersV2() { + addRequirements(ArmSubsystem.getInstance()); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_smallTimer.restart(); + m_permanantTimer.restart(); + m_numSamples = 0; + m_sumBotSamples = 0; + m_sumTopSamples = 0; + m_commandState = 0; + + BlingSubsystem.getINSTANCE().noEncodersSynced(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_smallTimer.get() > ArmConfig.ENCODER_SYNCING_PERIOD_V2) { + m_smallTimer.reset(); + + if (m_commandState == 0 && m_permanantTimer.hasElapsed(1)) { + DriverStation.reportWarning( + String.format("Arm encoders are not synced, collecting arm encoder samples... (%.1fs)", m_permanantTimer.get()), + false); + + m_commandState = 1; + + } else if (m_commandState == 1) { + m_sumBotSamples += ArmSubsystem.getInstance().getAbsoluteBottom(); + m_sumTopSamples += ArmSubsystem.getInstance().getAbsoluteTop(); + m_numSamples++; + + if (m_numSamples >= ArmConfig.NUM_SYNCING_SAMPLES) { + m_commandState = 2; + } + } else if (m_commandState == 2) { + ArmSubsystem.getInstance().resetEncoder( + m_sumBotSamples / m_numSamples, + m_sumTopSamples / m_numSamples + ); + + if (ArmSubsystem.getInstance().areEncodersSynced() == false) { + DriverStation.reportWarning( + String.format("Arm encoders are not synced, attempting to sync them... (%.1fs)", m_permanantTimer.get()), + false); + + } else { + m_commandState = 99; // End the command, the encoders are synced + } + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (m_commandState == 99) { + DriverStation.reportWarning( + String.format("Arm encoders are synced (%.1f) \n", m_permanantTimer.get()), + false); + + BlingSubsystem.getINSTANCE().armEncodersSynced(); + + new WaitCommand(1).andThen( + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced()), + new WaitCommand(1), + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced()) + ).schedule(); + + ArmSubsystem.getInstance().sparkMaxBurnFlash(); + } + + m_permanantTimer.stop(); + m_smallTimer.stop(); + + if ( + /** Temporary checks for current encoder setup. TODO: Remove once encoder wrapping is solved.*/ + ArmSubsystem.getInstance().getBottomPosition() < Math.toRadians(5) || + ArmSubsystem.getInstance().getBottomPosition() > Math.toRadians(170) || + ArmSubsystem.getInstance().getTopPosition() < Math.toRadians(5) || + ArmSubsystem.getInstance().getTopPosition() > Math.toRadians(70) || + ArmSubsystem.getInstance().areEncodersSynced() == false || + m_commandState != 99 + ) { + + DriverStation.reportError("Arm encoders were reset outside the proper range. " + + " Disabling the arm to prevent damage." + + " Make sure the code boots up with the arm in the reset position.", true); + + // This line below will prevent any commands from being scheduled to the ArmSubsystem. + // This line is temporary while we haven't properly setup the absolute encoders properly yet. + // If it's blocking you from using the arm, you just need to put the arm in the reset position + // and reboot the code (which will properly reset the NEO encoders the absolute encoders). + Commands.run(() -> ArmSubsystem.getInstance(), ArmSubsystem.getInstance()).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).ignoringDisable(true).schedule(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_permanantTimer.get() > ArmConfig.ENCODER_SYNCING_TIMEOUT) { + DriverStation.reportError( + String.format("Arm encoders are not synced. SyncArmEncodersV2 spent %.1fs trying to sync them and has timed out", + m_permanantTimer.get()), + false); + + return true; + } + return m_commandState == 99; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + DriverStation.reportWarning("Another ArmSubsystem command was scheduled while " + + "SyncArmEncodersV2 is still running. Cancelling the incoming command.", false); + return InterruptionBehavior.kCancelIncoming; + } +} diff --git a/src/main/java/frc/robot/commands/SyncSteerEncoders.java b/src/main/java/frc/robot/commands/SyncSteerEncoders.java index 295f471..1a457e7 100644 --- a/src/main/java/frc/robot/commands/SyncSteerEncoders.java +++ b/src/main/java/frc/robot/commands/SyncSteerEncoders.java @@ -43,7 +43,7 @@ public void initialize() { @Override public void execute() { SmartDashboard.putNumber("SyncSteerState", state); - if (state == 0 && m_permanantTimer.hasElapsed(6)) { + if (state == 0 && m_permanantTimer.hasElapsed(1)) { state = 1; DriverStation.reportWarning( String.format("Starting to sync steer encoders (%.1fs)", m_permanantTimer.get()), false); @@ -57,14 +57,16 @@ public void execute() { false); SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); } else { - state = 2; + state = 99; m_smallTimer.restart(); } } - if (state == 2 && m_smallTimer.hasElapsed(5)) { - SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); - state = 99; - } + // if (state == 2 && m_smallTimer.hasElapsed(1)) { + // if (SwerveSubsystem.getInstance().checkSteeringEncoders() == false) { + // SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); + // } + // state = 99; + // } SmartDashboard.putNumber("SyncSteerState", state); } @@ -77,7 +79,11 @@ public void end(boolean interrupted) { false); BlingSubsystem.getINSTANCE().steerEncodersSynced(); - new WaitCommand(1).andThen(Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced())).schedule(); + new WaitCommand(1).andThen( + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced()), + new WaitCommand(1), + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced()) + ).schedule(); } m_permanantTimer.stop(); diff --git a/src/main/java/frc/robot/config/ArmConfig.java b/src/main/java/frc/robot/config/ArmConfig.java index f13c982..1101afa 100644 --- a/src/main/java/frc/robot/config/ArmConfig.java +++ b/src/main/java/frc/robot/config/ArmConfig.java @@ -204,6 +204,7 @@ public enum ArmPosition { // Syncing encoders public static double ENCODER_SYNCING_PERIOD = 0.4; // seconds + public static double ENCODER_SYNCING_PERIOD_V2 = 0.05; // seconds public static int ENCODER_SYNCING_TIMEOUT = 20; // seconds public static double ENCODER_SYNCING_TOLERANCE = 0.01; // radians public static int NUM_SYNCING_SAMPLES = 20; // num of samples needed to average diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 051b90b..55efa04 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -208,13 +208,16 @@ public static class Swerve{ // ~ Teleop speeds. Speeds should be portional to kMaxAttainableAngularSpeed. Angular speeds should be specified. + // REDUCING THE SPEED OF THE ROBOT SO WE DON"T DAMAGE SPONSER'S CARPET + public static final double OUTREACH_SPEED_REDUCTION = 0.4; + // Default Speeds - public static final double teleopDefaultSpeed = kMaxAttainableWheelSpeed;// Full speed - public static final double teleopDefaultAngularSpeed = Math.PI*3.0; // Old value: Math.PI*2.0 + public static final double teleopDefaultSpeed = kMaxAttainableWheelSpeed * OUTREACH_SPEED_REDUCTION;// Full speed + public static final double teleopDefaultAngularSpeed = Math.PI*3.0 * OUTREACH_SPEED_REDUCTION; // Old value: Math.PI*2.0 // Left Bumper speeds - public static final double teleopLeftBumperSpeed = kMaxAttainableWheelSpeed / 3.0;// A third of full speed - public static final double teleopLeftBumperAngularSpeed = Math.PI; // Can be a slower angular speed if wanted + public static final double teleopLeftBumperSpeed = kMaxAttainableWheelSpeed / 3.0 * OUTREACH_SPEED_REDUCTION;// A third of full speed + public static final double teleopLeftBumperAngularSpeed = Math.PI * OUTREACH_SPEED_REDUCTION; // Can be a slower angular speed if wanted // Right bumper speeds public static final double teleopRightBumperSpeed = 0.3; diff --git a/src/main/java/frc/robot/robotcontainers/CompRobotContainer.java b/src/main/java/frc/robot/robotcontainers/CompRobotContainer.java index cd55c90..098161e 100644 --- a/src/main/java/frc/robot/robotcontainers/CompRobotContainer.java +++ b/src/main/java/frc/robot/robotcontainers/CompRobotContainer.java @@ -27,6 +27,7 @@ import frc.robot.commands.ChargeStationLock; import frc.robot.commands.DriveArmAgainstBackstop; import frc.robot.commands.GripperCommand; +import frc.robot.commands.PhotonMoveToTarget; import frc.robot.commands.GripperCommand.GRIPPER_INSTRUCTION; import frc.robot.commands.ResetGyro; import frc.robot.commands.RotateAngleXY; @@ -92,8 +93,7 @@ private void configureButtonBindings() { driver.back().onTrue(new ResetGyro()); driver.y().whileTrue(new RotateAngleXY(driver, 0)); - driver.a().whileTrue(new RotateAngleXY(driver, Math.PI)); - + driver.a().whileTrue(new PhotonMoveToTarget()); driver.x().whileTrue(new ChargeStationLock()); driver.leftTrigger().whileTrue(new RotateXYSupplier(driver, NetworkTableInstance.getDefault().getTable("MergeVisionPipelineIntake22").getDoubleTopic("Yaw").subscribe(-99) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 3109d3f..433fa0b 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -242,8 +242,11 @@ public ArmSubsystem() { m_bottomArmFFTestingVolts = bottomArmDataTable.getDoubleTopic("VoltageSetInFFTesting").publish(); updatePIDSettings(); - updateFromAbsoluteTop(); - updateFromAbsoluteBottom(); + } + + public void sparkMaxBurnFlash() { + m_bottomArm.burnFlash(); + m_topArm.burnFlash(); } public void updatePIDSettings() { diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 487ea98..0420433 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -67,6 +67,24 @@ public class SwerveModule { */ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boolean turningInverted, int encoderCanID, double encoderOffset, String ModuleName) { + CANCoderConfiguration encoderConfiguration = new CANCoderConfiguration(); + encoderConfiguration.initializationStrategy = + SensorInitializationStrategy.BootToAbsolutePosition; + encoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond; + encoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; + encoderConfiguration.magnetOffsetDegrees = 0; + encoderConfiguration.sensorDirection = direction == Direction.CLOCKWISE; + + encoder = new CANCoder(encoderCanID); + + errCTRE(encoder.configFactoryDefault()); + CANCoderUtil.setCANCoderBusUsage(encoder, CCUsage.kMinimal); + errCTRE(encoder.configAllSettings(encoderConfiguration)); + + String tableName = "SwerveChassis/SwerveModule" + ModuleName; + swerveModuleTable = NetworkTableInstance.getDefault().getTable(tableName); + sub_offset = swerveModuleTable.getDoubleTopic("offset").subscribe(encoderOffset); + // CODE: Construct both CANSparkMax objects and set all the nessecary settings (CONSTANTS from Config or from the parameters of the constructor) m_driveMotor = new CANSparkMax(driveCanID, MotorType.kBrushless); @@ -109,25 +127,12 @@ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boo errREV(m_turningPIDController.setIZone(Config.Swerve.sub_steering_kIZone.get())); errREV(m_turningPIDController.setFF(Config.Swerve.sub_steering_kFF.get())); + updateSteeringFromCanCoder(); + resetLastAngle(); + errREV(m_driveMotor.burnFlash()); errREV(m_turningMotor.burnFlash()); - String tableName = "SwerveChassis/SwerveModule" + ModuleName; - swerveModuleTable = NetworkTableInstance.getDefault().getTable(tableName); - - CANCoderConfiguration encoderConfiguration = new CANCoderConfiguration(); - encoderConfiguration.initializationStrategy = - SensorInitializationStrategy.BootToAbsolutePosition; - encoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond; - encoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; - encoderConfiguration.magnetOffsetDegrees = 0; - encoderConfiguration.sensorDirection = direction == Direction.CLOCKWISE; - - encoder = new CANCoder(encoderCanID); - - errCTRE(encoder.configFactoryDefault()); - CANCoderUtil.setCANCoderBusUsage(encoder, CCUsage.kMinimal); - errCTRE(encoder.configAllSettings(encoderConfiguration)); desiredSpeedEntry = swerveModuleTable.getDoubleTopic("Desired speed (mps)").publish(); desiredAngleEntry = swerveModuleTable.getDoubleTopic("Desired angle (deg)").publish(); @@ -138,11 +143,10 @@ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boo canCoderEntry = swerveModuleTable.getDoubleTopic("CanCoder").publish(); desiredAngle360Range = swerveModuleTable.getDoubleTopic("Desired angle 360 range").publish(); currentAngle360Range = swerveModuleTable.getDoubleTopic("Current angle 360 range").publish(); - sub_offset = swerveModuleTable.getDoubleTopic("offset").subscribe(encoderOffset); - updateSteeringFromCanCoder(); - resetLastAngle(); + + } /** diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 12c65de..5a676ab 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -4,19 +4,27 @@ package frc.robot.subsystems; +import java.util.NoSuchElementException; +import java.util.Optional; + import com.ctre.phoenix.sensors.PigeonIMU; +import edu.wpi.first.math.MathSharedStore; +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.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -27,8 +35,7 @@ import frc.robot.config.Config; public class SwerveSubsystem extends SubsystemBase { - private Field2d m_field = new Field2d(); - + private Field2d m_field = new Field2d(); private NetworkTable table = NetworkTableInstance.getDefault().getTable("DriveTrain"); private DoublePublisher gyroEntry = table.getDoubleTopic("RawGyroYaw").publish(); @@ -38,7 +45,9 @@ public class SwerveSubsystem extends SubsystemBase { private DoublePublisher pitchPub = table.getDoubleTopic("Pitch").publish(); private DoublePublisher rollPub = table.getDoubleTopic("Roll").publish(); - + + private DoubleArrayPublisher pubOdometry = table.getDoubleArrayTopic("OdometryArr").publish(PubSubOption.periodic(0.02)); + // Instance for singleton class private static SwerveSubsystem instance; @@ -48,33 +57,66 @@ public class SwerveSubsystem extends SubsystemBase { private final SwerveModule m_frontRight; private final SwerveModule m_rearRight; // The gyro sensor - private final PigeonIMU m_pigeon; + private final PigeonIMU m_pigeon; // Odometry class for tracking robot pose private SwerveDriveOdometry m_odometry; + private final double BUFFER_DURATION = 1.5; + private TimeInterpolatableBuffer m_poseBuffer = + TimeInterpolatableBuffer.createBuffer(BUFFER_DURATION); + + // ProfiledPIDControllers for the pid control + ProfiledPIDController pidControlX; + double currentX; + double desiredX; + ProfiledPIDController pidControlY; + double currentY; + double desiredY; + ProfiledPIDController pidControlRotation; + double currentRotation; + double desiredRotation; + double tolerance = 0.01; + double angleTolerance = 0.01; + /** Get instance of singleton class */ public static SwerveSubsystem getInstance() { - if (instance == null){ + if (instance == null) { SubsystemChecker.subsystemConstructed(SubsystemType.SwerveSubsystem); instance = new SwerveSubsystem(); } return instance; } - + /** Creates a new DriveSubsystem. */ private SwerveSubsystem() { - m_frontLeft = new SwerveModule(Config.CANID.FRONT_LEFT_DRIVE, Config.Swerve.INVERTED_FRONT_LEFT_DRIVE, Config.CANID.FRONT_LEFT_STEERING, Config.Swerve.INVERTED_FRONT_LEFT_STEERING, Config.CANID.FRONT_LEFT_CANCODER, Config.Swerve.FL_ENCODER_OFFSET, "FL"); + m_frontLeft = new SwerveModule(Config.CANID.FRONT_LEFT_DRIVE, Config.Swerve.INVERTED_FRONT_LEFT_DRIVE, + Config.CANID.FRONT_LEFT_STEERING, Config.Swerve.INVERTED_FRONT_LEFT_STEERING, + Config.CANID.FRONT_LEFT_CANCODER, Config.Swerve.FL_ENCODER_OFFSET, "FL"); - m_rearLeft = new SwerveModule(Config.CANID.REAR_LEFT_DRIVE, Config.Swerve.INVERTED_REAR_LEFT_DRIVE, Config.CANID.REAR_LEFT_STEERING, Config.Swerve.INVERTED_REAR_LEFT_STEERING, Config.CANID.REAR_LEFT_CANCODER, Config.Swerve.RL_ENCODER_OFFSET, "RL"); + m_rearLeft = new SwerveModule(Config.CANID.REAR_LEFT_DRIVE, Config.Swerve.INVERTED_REAR_LEFT_DRIVE, + Config.CANID.REAR_LEFT_STEERING, Config.Swerve.INVERTED_REAR_LEFT_STEERING, + Config.CANID.REAR_LEFT_CANCODER, Config.Swerve.RL_ENCODER_OFFSET, "RL"); - m_frontRight = new SwerveModule(Config.CANID.FRONT_RIGHT_DRIVE, Config.Swerve.INVERTED_FRONT_RIGHT_DRIVE, Config.CANID.FRONT_RIGHT_STEERING, Config.Swerve.INVERTED_FRONT_RIGHT_STEERING, Config.CANID.FRONT_RIGHT_CANCODER, Config.Swerve.FR_ENCODER_OFFSET, "FR"); + m_frontRight = new SwerveModule(Config.CANID.FRONT_RIGHT_DRIVE, Config.Swerve.INVERTED_FRONT_RIGHT_DRIVE, + Config.CANID.FRONT_RIGHT_STEERING, Config.Swerve.INVERTED_FRONT_RIGHT_STEERING, + Config.CANID.FRONT_RIGHT_CANCODER, Config.Swerve.FR_ENCODER_OFFSET, "FR"); - m_rearRight = new SwerveModule(Config.CANID.REAR_RIGHT_DRIVE, Config.Swerve.INVERTED_REAR_RIGHT_DRIVE, Config.CANID.REAR_RIGHT_STEERING, Config.Swerve.INVERTED_REAR_RIGHT_STEERING, Config.CANID.REAR_RIGHT_CANCODER, Config.Swerve.RR_ENCODER_OFFSET, "RR"); + m_rearRight = new SwerveModule(Config.CANID.REAR_RIGHT_DRIVE, Config.Swerve.INVERTED_REAR_RIGHT_DRIVE, + Config.CANID.REAR_RIGHT_STEERING, Config.Swerve.INVERTED_REAR_RIGHT_STEERING, + Config.CANID.REAR_RIGHT_CANCODER, Config.Swerve.RR_ENCODER_OFFSET, "RR"); m_pigeon = new PigeonIMU(Config.CANID.PIGEON); - m_odometry = new SwerveDriveOdometry(Config.Swerve.kSwerveDriveKinematics, Rotation2d.fromDegrees(getGyro()), getPosition(), new Pose2d(0, 0, Rotation2d.fromDegrees(rotEntry.get()))); + m_odometry = new SwerveDriveOdometry(Config.Swerve.kSwerveDriveKinematics, Rotation2d.fromDegrees(getGyro()), + getPosition(), new Pose2d(0, 0, Rotation2d.fromDegrees(rotEntry.get()))); SmartDashboard.putData("Field", m_field); + + pidControlX = new ProfiledPIDController(1, 0.0, 0.2, + new TrapezoidProfile.Constraints(1, 1)); + pidControlY = new ProfiledPIDController(1, 0.0, 0.2, + new TrapezoidProfile.Constraints(1, 1)); + pidControlRotation = new ProfiledPIDController(5.0, 0, 0.4, + new TrapezoidProfile.Constraints(4 * Math.PI, 8 * Math.PI)); } @Override @@ -86,13 +128,13 @@ public void periodic() { m_rearLeft.updateNT(); m_rearRight.updateNT(); - // Update the odometry in the periodic block m_odometry.update( Rotation2d.fromDegrees(currentGyro), - getPosition() - ); - + getPosition()); + + m_poseBuffer.addSample(MathSharedStore.getTimestamp(), getPose()); + gyroEntry.accept(currentGyro); xEntry.accept(getPose().getX()); yEntry.accept(getPose().getY()); @@ -102,17 +144,19 @@ public void periodic() { pitchPub.accept(getPitch()); rollPub.accept(getRoll()); + + pubOdometry.accept(new double[]{getPose().getX(), getPose().getY(), getPose().getRotation().getRadians()}); } private SwerveModulePosition[] getPosition() { return new SwerveModulePosition[] { - m_frontLeft.getModulePosition(), - m_frontRight.getModulePosition(), - m_rearLeft.getModulePosition(), - m_rearRight.getModulePosition()}; + m_frontLeft.getModulePosition(), + m_frontRight.getModulePosition(), + m_rearLeft.getModulePosition(), + m_rearRight.getModulePosition() }; } - public void setTrajectory(Trajectory traj){ + public void setTrajectory(Trajectory traj) { m_field.getObject("traj").setTrajectory(traj); } @@ -125,6 +169,26 @@ public Pose2d getPose() { return m_odometry.getPoseMeters(); } + /** + * Get the odometry pose at a given timestamp. + * Can only go 1.5 seconds into the past {@link BUFFER_DURATION} + * + * @param timestampSeconds The timestamp in seconds, + * matches PoseEstimator and similar timestamps + * @return An Optional with the Pose2d, or an empty optional. + */ + public Optional getPoseAtTimestamp(double timestampSeconds) { + try { + if (m_poseBuffer.getInternalBuffer().lastKey() - BUFFER_DURATION > timestampSeconds) { + return Optional.empty(); + } + } catch (NoSuchElementException ex) { + return Optional.empty(); + } + + return m_poseBuffer.getSample(timestampSeconds); + } + /** * Resets the odometry to the specified pose. * @@ -133,6 +197,7 @@ public Pose2d getPose() { public void resetOdometry(Pose2d pose) { System.out.println(pose.toString()); m_odometry.resetPosition(Rotation2d.fromDegrees(getGyro()), getPosition(), pose); + m_poseBuffer.clear(); } /** @@ -148,18 +213,20 @@ public void resetOdometry(Pose2d pose) { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { SwerveModuleState[] swerveModuleStates; if (fieldRelative) { - swerveModuleStates = Config.Swerve.kSwerveDriveKinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, getHeading())); + swerveModuleStates = Config.Swerve.kSwerveDriveKinematics + .toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, getHeading())); } else { - swerveModuleStates = Config.Swerve.kSwerveDriveKinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot)); + swerveModuleStates = Config.Swerve.kSwerveDriveKinematics + .toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot)); } SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Config.Swerve.kMaxAttainableWheelSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0], isOpenLoop); m_frontRight.setDesiredState(swerveModuleStates[1], isOpenLoop); m_rearLeft.setDesiredState(swerveModuleStates[2], isOpenLoop); - m_rearRight.setDesiredState(swerveModuleStates[3],isOpenLoop); + m_rearRight.setDesiredState(swerveModuleStates[3], isOpenLoop); } - public void setModuleStatesNoAntiJitter(SwerveModuleState[] desiredStates, boolean isOpenLoop){ + public void setModuleStatesNoAntiJitter(SwerveModuleState[] desiredStates, boolean isOpenLoop) { m_frontLeft.setDesiredState(desiredStates[0], isOpenLoop, false); m_frontRight.setDesiredState(desiredStates[1], isOpenLoop, false); m_rearLeft.setDesiredState(desiredStates[2], isOpenLoop, false); @@ -188,12 +255,13 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo public void setModuleStatesAuto(SwerveModuleState[] desiredStates) { setModuleStates(desiredStates, false); } - + /** * Returns the heading of the robot. * - * This is private because only the odoemtry get's the raw gyro value. - * Everything else get's the gyro value from the odometry since it does an offset. + * This is private because only the odoemtry get's the raw gyro value. + * Everything else get's the gyro value from the odometry since it does an + * offset. * * @return the robot's heading in degrees, from -180 to 180 */ @@ -204,7 +272,8 @@ private double getGyro() { /** * Returns the heading of the robot. * - * Uses this method for heading. Odometry does an offset to ensure this has the correct origin. + * Uses this method for heading. Odometry does an offset to ensure this has the + * correct origin. * * @return the robot's heading in degrees, from -180 to 180 */ @@ -222,7 +291,7 @@ public void stopMotors() { m_rearRight.stopMotors(); } - public void updateModulesPID(){ + public void updateModulesPID() { m_frontLeft.updatePIDValues(); m_frontRight.updatePIDValues(); m_rearLeft.updatePIDValues(); @@ -257,10 +326,40 @@ public void resetLastAngles() { } public double getRoll() { - return(m_pigeon.getRoll()); + return (m_pigeon.getRoll()); } public double getPitch() { - return(m_pigeon.getPitch()); + return (m_pigeon.getPitch()); + } + + // Swerve actual driving methods + public void resetDriveToPose() { + // reset current positions + pidControlX.reset(currentX); + pidControlY.reset(currentY); + pidControlRotation.reset(currentRotation); + } + + public void driveToPose(Pose2d pose) { + //update the currentX and currentY + currentX = getPose().getX(); + currentY = getPose().getY(); + currentRotation = getHeading().getRadians(); + + desiredX = pose.getX(); + desiredY = pose.getY(); + desiredRotation = pose.getRotation().getRadians(); + + double x = pidControlX.calculate(currentX, desiredX); + double y = pidControlY.calculate(currentY, desiredY); + double rot = pidControlRotation.calculate(currentRotation, desiredRotation); + + drive(x, y, rot, true, false); + } + + public boolean isAtPose() { + return Math.abs(currentX - desiredX) < tolerance && Math.abs(currentY - desiredY) < tolerance + && Math.abs(currentRotation - desiredRotation) < angleTolerance; } } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file