From 3fd98cd82ba22df317039015aa0bf94e6fe10319 Mon Sep 17 00:00:00 2001 From: Neo-Aditya Date: Mon, 4 Apr 2022 19:50:59 -0400 Subject: [PATCH 01/10] y --- .../frc/robot/subsystems/IMUSubsystem.java | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/IMUSubsystem.java 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..7afea0b0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -0,0 +1,46 @@ +// 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 com.ctre.phoenix.sensors.PigeonIMU; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.SmartShuffleboard; + + +public class IMUSubsystem extends SubsystemBase { + /** Creates a new IMUSubststem. */ + private PigeonIMU gyro; + + public IMUSubsystem() { + + } + + public void resetGyro() { + gyro.setFusedHeading(0); + } + + public double getAngle() { + return Math.IEEEremainder(gyro.getFusedHeading(), 360); + } + + public double getXaccel() { + return gyro.getAccelY(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From b70fcd6b8cd678b470547b3187bb43fada75c97f Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Mon, 4 Apr 2022 21:03:08 -0400 Subject: [PATCH 02/10] IMUSubsystem --- .../frc/robot/subsystems/IMUSubsystem.java | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IMUSubsystem.java b/src/main/java/frc/robot/subsystems/IMUSubsystem.java index 7afea0b0..769681f1 100644 --- a/src/main/java/frc/robot/subsystems/IMUSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -21,22 +21,35 @@ public class IMUSubsystem extends SubsystemBase { /** Creates a new IMUSubststem. */ - private PigeonIMU gyro; + 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 calibrate() { + gyro.calibrate(); + } + public double getAccelY() { + return gyro.getAccelY(); } - public void resetGyro() { - gyro.setFusedHeading(0); + public double getAccelX() { + return gyro.getAccelX(); + } + + public double getAccelZ() { + return gyro.getAccelZ(); } public double getAngle() { - return Math.IEEEremainder(gyro.getFusedHeading(), 360); + return -gyro.getAngle(); } - public double getXaccel() { - return gyro.getAccelY(); + public ADIS16470_IMU getGyro(){ + return gyro; } @Override From e9783a7ba242a7cb81fae55bee604c2df408f87c Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 18:33:10 -0400 Subject: [PATCH 03/10] practice --- src/main/java/frc/robot/subsystems/IMUSubsystem.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/IMUSubsystem.java b/src/main/java/frc/robot/subsystems/IMUSubsystem.java index 769681f1..06a0a436 100644 --- a/src/main/java/frc/robot/subsystems/IMUSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -45,13 +45,17 @@ public double getAccelZ() { } public double getAngle() { - return -gyro.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 From 920bfae0a621993c63108e0ea6c691fab955cb6d Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 18:54:44 -0400 Subject: [PATCH 04/10] Cleaned it up a bit --- .../frc/robot/subsystems/IMUSubsystem.java | 24 ++++++------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IMUSubsystem.java b/src/main/java/frc/robot/subsystems/IMUSubsystem.java index 06a0a436..814463a5 100644 --- a/src/main/java/frc/robot/subsystems/IMUSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -4,31 +4,21 @@ package frc.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.utils.SmartShuffleboard; - public class IMUSubsystem extends SubsystemBase { /** Creates a new IMUSubststem. */ private ADIS16470_IMU gyro; public IMUSubsystem() { - gyro = new ADIS16470_IMU(); + gyro = new ADIS16470_IMU(); } - //add @params Please do this before a competition or while the robot is perfectly still - public void calibrate() { + // add @params Please do this before a competition or while the robot is + // perfectly still + public void resetGyro() { + gyro.reset(); gyro.calibrate(); } @@ -48,12 +38,12 @@ public double getAngle() { return gyro.getAngle(); } - public ADIS16470_IMU getGyro(){ + public ADIS16470_IMU getGyro() { return gyro; } public void name() { - + } @Override From 436863d589642f0534960caf006c6b60e6118b2f Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 19:18:04 -0400 Subject: [PATCH 05/10] Implemented IMU Subsystem everywhere - Autochooser --- src/main/java/frc/robot/RobotContainer.java | 12 ++++----- .../Autonomous/ThreeShotSequenceRight.java | 5 ++-- .../commands/DriveCommands/TurnDegrees.java | 8 ++++-- .../java/frc/robot/subsystems/DriveTrain.java | 26 +++---------------- 4 files changed, 17 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ef988dcd..7f178d90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,21 +12,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.ShooterCommands.*; +import frc.robot.commands.LogError; import frc.robot.commands.ToggleBlockerPiston; import frc.robot.commands.Autonomous.AutoSetShootingPosition; import frc.robot.commands.Autonomous.AutoSetTurretPosition; import frc.robot.commands.Autonomous.CrossTheLineSequence; import frc.robot.commands.Autonomous.DoNothingSequence; +import frc.robot.commands.Autonomous.OneShotSequenceMiddle; import frc.robot.commands.Autonomous.TwoShotSequenceLeft; import frc.robot.commands.Autonomous.TwoShotSequenceRight; -import frc.robot.commands.Autonomous.OneShotSequenceMiddle; -import frc.robot.commands.LogError; -import frc.robot.commands.ToggleBlockerPiston; import frc.robot.commands.ClimberCommands.Climb3Inches; import frc.robot.commands.ClimberCommands.ManualMoveClimberArm; import frc.robot.commands.ClimberCommands.ManualMoveClimberWinch; -import frc.robot.commands.ClimberCommands.ToggleClimberSolenoid; import frc.robot.commands.ClimberCommands.WinchExtend; import frc.robot.commands.DriveCommands.Drive; import frc.robot.commands.DriveCommands.TurnDegrees; @@ -62,12 +59,12 @@ import frc.robot.commands.TurretCommands.CalibrateTurretEncoderSequence; import frc.robot.commands.TurretCommands.MoveTurretDashboard; import frc.robot.commands.TurretCommands.RunTurretUntilLimitSwitch; -import frc.robot.commands.TurretCommands.TurretAuto; import frc.robot.commands.TurretCommands.RunTurretUntilTarget; import frc.robot.commands.TurretCommands.TurretManualCommand; 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; @@ -120,6 +117,7 @@ public class RobotContainer { private final PowerDistribution m_PowerDistPanel = new PowerDistribution(); private final ClimberArmSubsystem climberArmSubsystem = new ClimberArmSubsystem(m_PowerDistPanel); private final ClimberWinchSubsystem climberWinchSubsystem = new ClimberWinchSubsystem(); + private final IMUSubsystem IMUSubsystem = new IMUSubsystem(); private final Hood hood = new Hood(); private final TurretSubsystem turretSubsystem= new TurretSubsystem(); @@ -255,7 +253,7 @@ 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("Turn", "Turn Degrees", new TurnDegrees(driveTrain, 90, 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/TurnDegrees.java b/src/main/java/frc/robot/commands/DriveCommands/TurnDegrees.java index 39241ae9..0e42672e 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 f3031a71..2a04d4f2 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -10,7 +9,6 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.utils.SmartShuffleboard; -import frc.robot.utils.diag.DiagPigeon; import frc.robot.utils.diag.DiagSparkMaxEncoder; public class DriveTrain extends SubsystemBase { @@ -20,7 +18,7 @@ public class DriveTrain extends SubsystemBase { private CANSparkMax right2; private RelativeEncoder leftEncoder; private RelativeEncoder rightEncoder; - private PigeonIMU gyro; + private IMUSubsystem IMUSubsystem; public DriveTrain(){ left1 = new CANSparkMax(Constants.DRIVE_LEFT1_ID, MotorType.kBrushless); @@ -51,13 +49,11 @@ public DriveTrain(){ right1.setIdleMode(IdleMode.kBrake); right2.setIdleMode(IdleMode.kBrake); - gyro = new PigeonIMU(Constants.PIGEON_CAN_ID); - resetGyro(); + IMUSubsystem.resetGyro(); Robot.getDiagnostics().addDiagnosable(new DiagSparkMaxEncoder("Left Drive Encoder", 10, left1)); Robot.getDiagnostics().addDiagnosable(new DiagSparkMaxEncoder("Right Drive Encoder", 10, right1)); - Robot.getDiagnostics().addDiagnosable(new DiagPigeon("Pigeon", 10, gyro)); } public void drive(double speedLeft, double speedRight, boolean isSquared) { @@ -71,29 +67,13 @@ public void drive(double speedLeft, double speedRight, boolean isSquared) { right1.set(speedRight); } - /** - * Resets the Gyro - */ - public void resetGyro() { - gyro.setFusedHeading(0); - } - - /** - * Gets the angle of the robot - * - * @return angle of robot between -180-180 - */ - public double getAngle() { - return Math.IEEEremainder(gyro.getFusedHeading(), 360); - } - @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", "Gyro", getAngle()); + SmartShuffleboard.put("Drive", "Gyro", "Gyro", IMUSubsystem.getAngle()); } } From efb4015b04e79241400942e5ca4f93ed8ad10322 Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 19:25:14 -0400 Subject: [PATCH 06/10] Implemented IMU values on shuffleboard --- src/main/java/frc/robot/subsystems/DriveTrain.java | 1 - src/main/java/frc/robot/subsystems/IMUSubsystem.java | 8 ++++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 2a04d4f2..e4a8399f 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -73,7 +73,6 @@ public void periodic() { if (Constants.ENABLE_DEBUG) { SmartShuffleboard.put("Drive", "Encoders", "L", getLeftEncoder()); SmartShuffleboard.put("Drive", "Encoders", "R", getRightEncoder()); - SmartShuffleboard.put("Drive", "Gyro", "Gyro", IMUSubsystem.getAngle()); } } diff --git a/src/main/java/frc/robot/subsystems/IMUSubsystem.java b/src/main/java/frc/robot/subsystems/IMUSubsystem.java index 814463a5..192fa409 100644 --- a/src/main/java/frc/robot/subsystems/IMUSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IMUSubsystem.java @@ -6,6 +6,8 @@ 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. */ @@ -49,5 +51,11 @@ 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()); + } } } From 2447f4632c3a9111eb40566498b69112c75399a6 Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 19:30:43 -0400 Subject: [PATCH 07/10] Added it to autochooser --- src/main/java/frc/robot/AutoChooser.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/AutoChooser.java b/src/main/java/frc/robot/AutoChooser.java index 1ebcbc63..236874da 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);} From 17e8e6a447ff22297e6a853133d22f7ffeb70115 Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 19:31:28 -0400 Subject: [PATCH 08/10] Forgot this --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f178d90..f88b4e3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -122,7 +122,7 @@ public class RobotContainer { 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 TurretManualCommand turretCommand= new TurretManualCommand(turretSubsystem, () -> -xboxController.getLeftX()); From 3870589e1a98b5ff9204a61f6ecc8178449ac03c Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 20:47:42 -0400 Subject: [PATCH 09/10] Forgot this --- .../robot/commands/DriveCommands/AutoTurnDegrees.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java index 46febbe5..b4eb993e 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) { From 41d5caebeece8d3990c1574ee077cb0916e943a9 Mon Sep 17 00:00:00 2001 From: Neo-Adityae Date: Tue, 5 Apr 2022 20:48:32 -0400 Subject: [PATCH 10/10] Fixed an error in AutoTurnDegrees --- .../java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java index b4eb993e..a97a24de 100644 --- a/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java +++ b/src/main/java/frc/robot/commands/DriveCommands/AutoTurnDegrees.java @@ -53,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); }