diff --git a/src/main/java/frc/robot/AutoChooser.java b/src/main/java/frc/robot/AutoChooser.java index f823764f..938cd81a 100644 --- a/src/main/java/frc/robot/AutoChooser.java +++ b/src/main/java/frc/robot/AutoChooser.java @@ -6,25 +6,25 @@ /*----------------------------------------------------------------------------*/ package frc.robot; +//import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.Autonomous.CrossTheLineSequence; import frc.robot.commands.Autonomous.DoNothingSequence; -import frc.robot.commands.Autonomous.TwoShotSequenceLeft; import frc.robot.commands.Autonomous.OneShotSequenceMiddle; import frc.robot.commands.Autonomous.ThreeShotSequenceRight; +import frc.robot.commands.Autonomous.TwoShotSequenceLeft; import frc.robot.commands.Autonomous.TwoShotSequenceRight; -import frc.robot.commands.DriveCommands.Drive; -import frc.robot.commands.IntakeCommand.IntakeSequence; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.Hood; +import frc.robot.subsystems.IMUSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.limelight.LimeLightVision; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.shuffleboard.*; /** * Add your docs here. @@ -38,6 +38,7 @@ public class AutoChooser { private TurretSubsystem turretSubsystem; private LimeLightVision limeLightVision; private Hood hood; + private IMUSubsystem IMUSubsystem; // all actions driver choose at beginning of match enum Action { @@ -45,7 +46,7 @@ enum Action { } - public AutoChooser(IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, LimeLightVision limeLightVision, Hood hood) { + public AutoChooser(IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, LimeLightVision limeLightVision, Hood hood, IMUSubsystem IMUSubsystem) { actionChooser = new SendableChooser(); this.intakeSubsystem = intakeSubsystem; this.driveTrain = driveTrain; @@ -53,6 +54,7 @@ public AutoChooser(IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, Shoot this.turretSubsystem = turretSubsystem; this.limeLightVision = limeLightVision; this.hood = hood; + this.IMUSubsystem = IMUSubsystem; } public void addOptions() { @@ -88,7 +90,7 @@ public Command getAutonomousCommand(Action a) { Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);} else if (a == Action.THREE_SHOT) {return new ThreeShotSequenceRight(turretSubsystem, Constants.AUTO_TURRET_SPEED, intakeSubsystem, driveTrain, Constants.AUTO_MOVE_SPEED, - Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);} + Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood, IMUSubsystem);} else if (a == Action.TWO_SHOT_LEFT) {return new TwoShotSequenceLeft(turretSubsystem, Constants.AUTO_TURRET_SPEED, intakeSubsystem, driveTrain, Constants.AUTO_MOVE_SPEED, Constants.AUTO_DISTANCE_INCHES, shooterSubsystem, limeLightVision, hood);} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e86869bb..b3ec84c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,6 +73,7 @@ import frc.robot.commands.TurretCommands.TurretSweepSequence; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.Hood; +import frc.robot.subsystems.IMUSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LimelightSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -130,12 +131,13 @@ public class RobotContainer { private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); private final PowerDistribution m_PowerDistPanel = new PowerDistribution(); private final ClimberArmSubsystem climberArmSubsystem = new ClimberArmSubsystem(m_PowerDistPanel); + private final IMUSubsystem IMUSubsystem = new IMUSubsystem(); private final ClimberWinchSubsystem climberWinchSubsystem = new ClimberWinchSubsystem(m_PowerDistPanel); private final Hood hood = new Hood(); private final TurretSubsystem turretSubsystem= new TurretSubsystem(); - public AutoChooser autoChooser = new AutoChooser(intakeSubsystem, driveTrain, shooterSubsystem, turretSubsystem, limeLightVision.getLimeLightVision(), hood); + public AutoChooser autoChooser = new AutoChooser(intakeSubsystem, driveTrain, shooterSubsystem, turretSubsystem, limeLightVision.getLimeLightVision(), hood, IMUSubsystem); private final Drive driveCommand = new Drive(driveTrain, () -> joyLeft.getY(), () -> joyRight.getY()); private final TurretContinousTarget turretCommand= new TurretContinousTarget(turretSubsystem, limeLightVision.getLimeLightVision(), () -> -xboxController.getLeftX()); @@ -286,13 +288,13 @@ public void installCommandsOnShuffleboard() { SmartShuffleboard.putCommand("Miscellaneous", "Set LED On", new SetLEDOn()); SmartShuffleboard.putCommand("Miscellaneous", "Set Pipeline to 0", new SetPipeline(0)); SmartShuffleboard.putCommand("Miscellaneous", "Set Pipeline to 1", new SetPipeline(1)); - SmartShuffleboard.putCommand("Turn", "Turn Degrees", new TurnDegrees(driveTrain, 90)); - SmartShuffleboard.putCommand("Drive", "Turn 30 Degrees", new AutoTurnDegrees(driveTrain, 30)); - SmartShuffleboard.putCommand("Drive", "Turn -30 Degrees", new AutoTurnDegrees(driveTrain, -30)); - SmartShuffleboard.putCommand("Drive", "Turn 90 Degrees", new AutoTurnDegrees(driveTrain, 90)); - SmartShuffleboard.putCommand("Drive", "Turn 120 Degrees", new AutoTurnDegrees(driveTrain, 120)); - SmartShuffleboard.putCommand("Drive", "Turn 150 Degrees", new AutoTurnDegrees(driveTrain, 150)); - SmartShuffleboard.putCommand("Drive", "Turn 180 Degrees", new AutoTurnDegrees(driveTrain, 180)); + SmartShuffleboard.putCommand("Turn", "Turn Degrees", new TurnDegrees(driveTrain, 90, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn 30 Degrees", new AutoTurnDegrees(driveTrain, 30, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn -30 Degrees", new AutoTurnDegrees(driveTrain, -30, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn 90 Degrees", new AutoTurnDegrees(driveTrain, 90, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn 120 Degrees", new AutoTurnDegrees(driveTrain, 120, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn 150 Degrees", new AutoTurnDegrees(driveTrain, 150, IMUSubsystem)); + SmartShuffleboard.putCommand("Drive", "Turn 180 Degrees", new AutoTurnDegrees(driveTrain, 180, IMUSubsystem)); SmartShuffleboard.putCommand("Hood", "Move Hood Down", new MoveHoodDown(hood)); diff --git a/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java b/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java index e1fbee2c..d060a62d 100644 --- a/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java +++ b/src/main/java/frc/robot/commands/Autonomous/ThreeShotSequenceRight.java @@ -12,6 +12,7 @@ import frc.robot.commands.ShooterCommands.ShooterSequeunce; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.Hood; +import frc.robot.subsystems.IMUSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -24,7 +25,7 @@ public class ThreeShotSequenceRight extends SequentialCommandGroup { /** Creates a new TwoShotSequenceRight. * @param vision * @param hoodSubsystem */ - public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpeed, IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, double speed, double distanceInches, ShooterSubsystem shooterSubsystem, LimeLightVision limeLightVision, Hood hood) { + public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpeed, IntakeSubsystem intakeSubsystem, DriveTrain driveTrain, double speed, double distanceInches, ShooterSubsystem shooterSubsystem, LimeLightVision limeLightVision, Hood hood, IMUSubsystem IMUSubsystem) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( @@ -34,7 +35,7 @@ public ThreeShotSequenceRight(TurretSubsystem turretSubsystem, double turretSpee new ShooterSequeunce(shooterSubsystem, limeLightVision), new WaitCommand(0.8), new ShooterSequeunce(shooterSubsystem, limeLightVision), - new TurnDegrees(driveTrain, 105), + new TurnDegrees(driveTrain, 105, IMUSubsystem), new ParralelMoveAndIntake(driveTrain, speed, 60, turretSubsystem, turretSpeed, intakeSubsystem, hood, turretSubsystem), new AutoTargetSequence(turretSubsystem, limeLightVision, hood), diff --git a/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java index 46febbe5..a97a24de 100644 --- a/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java +++ b/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; import frc.robot.subsystems.DriveTrain; -import frc.robot.utils.SmartShuffleboard; +import frc.robot.subsystems.IMUSubsystem; public class AutoTurnDegrees extends CommandBase{ @@ -13,22 +13,25 @@ public class AutoTurnDegrees extends CommandBase{ private double startTime; private double startDegrees; private double turnSpeed; + private IMUSubsystem IMUSubsystem; - public AutoTurnDegrees(DriveTrain driveTrain, double angleToTurn) { + public AutoTurnDegrees(DriveTrain driveTrain, double angleToTurn, IMUSubsystem IMUSubsystem) { this.driveTrain = driveTrain; this.turnDegrees = angleToTurn; + this.IMUSubsystem = IMUSubsystem; addRequirements(driveTrain); + addRequirements(IMUSubsystem); } @Override public void initialize() { this.startTime = Timer.getFPGATimestamp(); - this.startDegrees = driveTrain.getAngle(); + this.startDegrees = IMUSubsystem.getAngle(); } @Override public void execute() { - double rawError = (startDegrees + turnDegrees - driveTrain.getAngle()); + double rawError = (startDegrees + turnDegrees - IMUSubsystem.getAngle()); double error = Math.abs(rawError); double direction = Math.signum(rawError); if (error > Constants.AUTO_MOVE_TURN_SLOWDOWN_ERROR) { @@ -50,7 +53,7 @@ public boolean isFinished() { if (Timer.getFPGATimestamp() - startTime >= Constants.AUTO_MOVE_TURN_TIMEOUT) { return true; } - double error = Math.abs(startDegrees + turnDegrees - driveTrain.getAngle()); + double error = Math.abs(startDegrees + turnDegrees - IMUSubsystem.getAngle()); return (error <= Constants.AUTO_MOVE_TURN_THRESHOLD); } diff --git a/src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java b/src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java index 57ec3ad4..2ae74579 100644 --- a/src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java +++ b/src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import frc.robot.commands.LoggedCommandBase; import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.IMUSubsystem; import frc.robot.utils.SmartShuffleboard; @@ -14,16 +15,19 @@ public class TurnDegrees extends LoggedCommandBase{ private final int SLOWDOWN_ANGLE = 45; private final double MAXIMUM_TIME_S = 10; private DriveTrain driveTrain; + private IMUSubsystem IMUSubsystem; private double requiredAngle; private double currAngle; private double speed = 0.0; private double startTime; - public TurnDegrees(DriveTrain driveTrain, int requiredAngle){ + public TurnDegrees(DriveTrain driveTrain, int requiredAngle, IMUSubsystem IMUSubsystem){ this.driveTrain = driveTrain; this.requiredAngle = requiredAngle; + this.IMUSubsystem = IMUSubsystem; addRequirements(driveTrain); + addRequirements(IMUSubsystem); addLog(requiredAngle); } @@ -32,7 +36,7 @@ public void initialize(){ } public void execute(){ - currAngle = driveTrain.getAngle(); + currAngle = IMUSubsystem.getAngle(); double angleError = requiredAngle-currAngle; if (Math.abs(angleError) <= ANGLE_THRESHOLD) { diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 86936078..71d21a6f 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -5,7 +5,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; @@ -19,8 +18,7 @@ public class DriveTrain extends SubsystemBase { private CANSparkMax right2; private RelativeEncoder leftEncoder; private RelativeEncoder rightEncoder; - - private final ADIS16470_IMU imu; + private IMUSubsystem IMUSubsystem; public DriveTrain(){ left1 = new CANSparkMax(Constants.DRIVE_LEFT1_ID, MotorType.kBrushless); @@ -28,7 +26,6 @@ public DriveTrain(){ right1 = new CANSparkMax(Constants.DRIVE_RIGHT1_ID, MotorType.kBrushless); right2 = new CANSparkMax(Constants.DRIVE_RIGHT2_ID, MotorType.kBrushless); - imu = new ADIS16470_IMU(); left1.restoreFactoryDefaults(); left2.restoreFactoryDefaults(); @@ -53,7 +50,7 @@ public DriveTrain(){ right1.setIdleMode(IdleMode.kBrake); right2.setIdleMode(IdleMode.kBrake); - resetGyro(); + IMUSubsystem.resetGyro(); Robot.getDiagnostics().addDiagnosable(new DiagSparkMaxEncoder("Left Drive Encoder", 10, left1)); @@ -72,35 +69,12 @@ public void drive(double speedLeft, double speedRight, boolean isSquared) { right1.set(speedRight); } - /** - * Resets the Gyro - */ - public void resetGyro() { - imu.reset(); - imu.calibrate(); - } - - /** - * Gets the angle of the robot - * - * @return angle of robot between -180-180 - */ - public double getAngle() { - return imu.getAngle(); - } - @Override public void periodic() { // This method will be called once per scheduler run if (Constants.ENABLE_DEBUG) { SmartShuffleboard.put("Drive", "Encoders", "L", getLeftEncoder()); SmartShuffleboard.put("Drive", "Encoders", "R", getRightEncoder()); - SmartShuffleboard.put("Drive", "Gyro", "Angle", getAngle()); - SmartShuffleboard.put("Drive", "Gyro", "Raw Angle", imu.getAngle()); - SmartShuffleboard.put("Drive", "Gyro", "X Comp Angle", imu.getXComplementaryAngle()); - SmartShuffleboard.put("Drive", "Gyro", "Y Comp Angle", imu.getYComplementaryAngle()); - SmartShuffleboard.put("Drive", "Gyro", "X filtered acceleration angle", imu.getXFilteredAccelAngle()); - SmartShuffleboard.put("Drive", "Gyro", "Y filtered acceleration angle", imu.getYFilteredAccelAngle()); } } diff --git a/src/main/java/frc/robot/subsystems/IMUSubsystem.java b/src/main/java/frc/robot/subsystems/IMUSubsystem.java new file mode 100644 index 00000000..192fa409 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -0,0 +1,61 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.utils.SmartShuffleboard; + +public class IMUSubsystem extends SubsystemBase { + /** Creates a new IMUSubststem. */ + private ADIS16470_IMU gyro; + + public IMUSubsystem() { + gyro = new ADIS16470_IMU(); + } + + // add @params Please do this before a competition or while the robot is + // perfectly still + public void resetGyro() { + gyro.reset(); + gyro.calibrate(); + } + + public double getAccelY() { + return gyro.getAccelY(); + } + + public double getAccelX() { + return gyro.getAccelX(); + } + + public double getAccelZ() { + return gyro.getAccelZ(); + } + + public double getAngle() { + return gyro.getAngle(); + } + + public ADIS16470_IMU getGyro() { + return gyro; + } + + public void name() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + if (Constants.ENABLE_DEBUG) { + SmartShuffleboard.put("Gyro", "Angle", getAngle()); + SmartShuffleboard.put("Gyro", "Z accelerated angle", getAccelZ()); + SmartShuffleboard.put("Gyro", "Y accelerated angle", getAccelY()); + SmartShuffleboard.put("Gyro", "X accelerated angle", getAccelX()); + } + } +}