diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e013e13..5e3b70e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,6 @@ import frc.robot.commands.arm.ArmUpCommand; import frc.robot.commands.arm.ClawCommand; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.GyroSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; import frc.robot.subsystems.arm.ClawSubsystem; import frc.robot.subsystems.arm.ElevatorSubsystem; @@ -52,23 +51,20 @@ public class RobotContainer { private final UltrasonicSubsystem m_ultrasonic1 = new UltrasonicSubsystem(Constants.ULTRASONIC1PORT); - private final GyroSubsystem m_gyroSubsystem = new GyroSubsystem(); - private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(m_gyroSubsystem); + private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem(); private final ClawSubsystem m_clawSubsystem = new ClawSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); - private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem, m_gyroSubsystem); - private final BalanceCommand m_balanceCommand = - new BalanceCommand(m_driveSubsystem, m_gyroSubsystem); + private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem); + private final BalanceCommand m_balanceCommand = new BalanceCommand(m_driveSubsystem); private final PlaceCommand m_placeCommand = new PlaceCommand(m_clawSubsystem, m_elevatorSubsystem); private final DefaultDrive m_defaultDrive = new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); private final StraightCommand m_straightCommand = - new StraightCommand( - m_driveSubsystem, m_gyroSubsystem, this::getControllerLeftY, this::getControllerRightY); + new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); private final ClawCommand m_clawCommand = new ClawCommand(m_clawSubsystem, m_flightStick::getY); private final ArmUpCommand m_armUpCommand = new ArmUpCommand(m_elevatorSubsystem); private final ArmDownCommand m_armDownCommand = new ArmDownCommand(m_elevatorSubsystem); diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java index 4f26d9d..1a01d94 100644 --- a/src/main/java/frc/robot/commands/AimCommand.java +++ b/src/main/java/frc/robot/commands/AimCommand.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.GyroSubsystem; import org.photonvision.PhotonCamera; import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; @@ -16,7 +15,6 @@ /** The Aim command that uses the camera + gyro to control the robot. */ public class AimCommand extends CommandBase { private final DriveSubsystem m_driveSubsystem; - private final GyroSubsystem m_gyroSubsystem; private final PhotonCamera m_camera; private final String CAMERANAME = "OV5647"; // Constants such as camera and target height stored. Change per robot and goal! @@ -32,17 +30,15 @@ public class AimCommand extends CommandBase { * Creates a new AimCommand. * * @param d_subsystem The drive subsystem used by this command. - * @param g_subsystem The gyro subsystem used by this command. */ - public AimCommand(DriveSubsystem d_subsystem, GyroSubsystem g_subsystem) { + public AimCommand(DriveSubsystem d_subsystem) { m_driveSubsystem = d_subsystem; - m_gyroSubsystem = g_subsystem; // Change this to match the name of your camera m_camera = new PhotonCamera(CAMERANAME); // Use addRequirements() here to declare subsystem dependencies. - addRequirements(d_subsystem, g_subsystem); + addRequirements(d_subsystem); } // Called when the command is initially scheduled. @@ -56,7 +52,7 @@ public void execute() { // will not work if cam is defined incorrectly, but will not tell you if (CamResult.hasTargets()) { SmartDashboard.putBoolean("CameraTargetDetected", true); - double angleGoal = m_gyroSubsystem.getYaw() + CamResult.getBestTarget().getYaw(); + double angleGoal = m_driveSubsystem.getYaw() + CamResult.getBestTarget().getYaw(); SmartDashboard.putNumber("CameraTargetPitch", angleGoal); double distanceFromTarget = PhotonUtils.calculateDistanceToTargetMeters( @@ -66,7 +62,7 @@ public void execute() { Units.degreesToRadians(CamResult.getBestTarget().getPitch())) - GOAL_RANGE_METERS; // turn and move towards target. - m_driveSubsystem.driveAndTurn(m_gyroSubsystem.getYaw(), angleGoal, distanceFromTarget); + m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget); // we reset the angle everytime as the target could change / move. m_driveSubsystem.turnSetGoal(angleGoal); m_driveSubsystem.distanceSetGoal(distanceFromTarget); diff --git a/src/main/java/frc/robot/commands/BalanceCommand.java b/src/main/java/frc/robot/commands/BalanceCommand.java index 683fee3..6354cd7 100644 --- a/src/main/java/frc/robot/commands/BalanceCommand.java +++ b/src/main/java/frc/robot/commands/BalanceCommand.java @@ -6,24 +6,20 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.GyroSubsystem; /** A Balancing command that uses the gyro subsystem. */ public class BalanceCommand extends CommandBase { - private final GyroSubsystem m_gyroSubsystem; private final DriveSubsystem m_driveSubsystem; /** * Creates a new BalanceCommand. * * @param d_subsystem The drive subsystem used by this command. - * @param g_subsystem The gyro subsystem used by this command. */ - public BalanceCommand(DriveSubsystem d_subsystem, GyroSubsystem g_subsystem) { + public BalanceCommand(DriveSubsystem d_subsystem) { m_driveSubsystem = d_subsystem; - m_gyroSubsystem = g_subsystem; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_driveSubsystem, m_gyroSubsystem); + addRequirements(m_driveSubsystem); } // Called when the command is initially scheduled. @@ -35,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_driveSubsystem.balanceCorrection(m_gyroSubsystem.getPitch()); + m_driveSubsystem.balanceCorrection(m_driveSubsystem.getPitch()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/StraightCommand.java b/src/main/java/frc/robot/commands/StraightCommand.java index 6962530..32fc813 100644 --- a/src/main/java/frc/robot/commands/StraightCommand.java +++ b/src/main/java/frc/robot/commands/StraightCommand.java @@ -6,36 +6,29 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.GyroSubsystem; import java.util.function.DoubleSupplier; /** An example command that uses an example subsystem. */ public class StraightCommand extends CommandBase { private final DriveSubsystem m_driveSubsystem; - private final GyroSubsystem m_gyroSubsystem; private final DoubleSupplier m_left_y; // this gives us the left y axis for current controller private final DoubleSupplier m_right_y; // this gives us the right y axis for current controller /** * Creates a new StraightCommand. * * @param d_subsystem The drive subsystem used by this command. - * @param g_subsystem The gyro subsystem used by this command. * @param xbox_left_y A function that returns the value of the left y axis for the joystick. * @param xbox_right_y A function that returns the value of the right Y axis for the joystick. */ public StraightCommand( - DriveSubsystem d_subsystem, - GyroSubsystem g_subsystem, - DoubleSupplier xbox_left_y, - DoubleSupplier xbox_right_y) { + DriveSubsystem d_subsystem, DoubleSupplier xbox_left_y, DoubleSupplier xbox_right_y) { m_driveSubsystem = d_subsystem; - m_gyroSubsystem = g_subsystem; m_left_y = xbox_left_y; m_right_y = xbox_right_y; // Change this to match the name of your camera // Use addRequirements() here to declare subsystem dependencies. - addRequirements(d_subsystem, g_subsystem); + addRequirements(d_subsystem); } // Called when the command is initially scheduled. @@ -48,8 +41,8 @@ public void initialize() { @Override public void execute() { m_driveSubsystem.driveStraight( - m_gyroSubsystem.getYaw(), - m_gyroSubsystem.getAccumYaw(), + m_driveSubsystem.getYaw(), + m_driveSubsystem.getAccumYaw(), (m_left_y.getAsDouble() + m_right_y.getAsDouble()) / 2); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 510881f..5d9d4f2 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -5,13 +5,16 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; // import motor & frc dependencies import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.MathUtil; 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.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -22,8 +25,8 @@ /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */ public class DriveSubsystem extends SubsystemBase { - // Gyro Subsystem - private final GyroSubsystem m_gyroSubsystem; + // Gyro + private final AHRS m_Gyro; // motors private final WPI_VictorSPX m_backLeft; @@ -101,8 +104,9 @@ public class DriveSubsystem extends SubsystemBase { MaxBalanceRateDegPerS, MaxBalanceAccelerationDegPerSSquared)); /** Creates a new DriveSubsystem. */ - public DriveSubsystem(final GyroSubsystem g_subsystem) { - m_gyroSubsystem = g_subsystem; + public DriveSubsystem() { + // Init gyro + m_Gyro = new AHRS(SPI.Port.kMXP); // init motors // rio means built into the roboRIO m_backLeft = new WPI_VictorSPX(CANConstants.MOTORBACKLEFTID); @@ -131,14 +135,13 @@ public DriveSubsystem(final GyroSubsystem g_subsystem) { m_encoderLeft.setMinRate(0.1); // min rate to be determined moving m_encoderRight.setMinRate(0.1); // min rate to be determined moving resetEncoders(); - m_gyroSubsystem.reset(); + // Every time this function is called, A dollar is taken out of Jack's savings. Aka do it more. + resetGyro(); // configure Odemetry m_driveOdometry = new DifferentialDriveOdometry( - m_gyroSubsystem.getRotation2d(), - m_encoderLeft.getDistance(), - m_encoderRight.getDistance()); + getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance()); // config turn pid controller. m_turnController.enableContinuousInput(-180.0f, 180.0f); @@ -239,10 +242,10 @@ public void turnSetGoal(double targetAngleDegrees) { m_turnController.setGoal(targetAngleDegrees); } - private void calcuateAngleRate(double gyroYawAngle, double TargetAngleDegrees) { + private void calcuateAngleRate(double gyroYawAngle, double targetAngleDegrees) { if (!turnControllerEnabled) { m_turnController.reset(gyroYawAngle); - m_turnController.setGoal(TargetAngleDegrees); + m_turnController.setGoal(targetAngleDegrees); turnControllerEnabled = true; } turnRotateToAngleRate = MathUtil.clamp(m_turnController.calculate(gyroYawAngle), -1.0, 1.0); @@ -323,16 +326,39 @@ public void SetCoastmode() { public void resetPose(Pose2d pose) { resetEncoders(); m_driveOdometry.resetPosition( - m_gyroSubsystem.getRotation2d(), - m_encoderLeft.getDistance(), - m_encoderRight.getDistance(), - pose); + getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance(), pose); } public void stop() { this.tankDrive(0, 0); } + public void calibrate() { + m_Gyro.calibrate(); + } + + public Rotation2d getRotation2d() { + return m_Gyro.getRotation2d(); + } + + // for balance correction + public double getPitch() { + return m_Gyro.getPitch(); // get pitch in degrees + } + + // for PID control (turn by degrees) + public double getAccumYaw() { + return m_Gyro.getAngle(); // get angle in degrees + } + + public double getYaw() { + return m_Gyro.getYaw(); + } + + public void resetGyro() { + m_Gyro.reset(); + } + @Override public void periodic() { // This method will be called once per scheduler run @@ -344,9 +370,11 @@ public void periodic() { SmartDashboard.putNumber("Current Robot Location Y axis", getPose().getY()); SmartDashboard.putNumber("Current Robot Rotation", getPose().getRotation().getDegrees()); SmartDashboard.putNumber("Average Distance Traveled", AverageDistance()); + SmartDashboard.putNumber("Current Gyro Pitch", getPitch()); + SmartDashboard.putNumber("Current Gyro Yaw", getYaw()); // Update the odometry in the periodic block m_driveOdometry.update( - m_gyroSubsystem.getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance()); + getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance()); } @Override diff --git a/src/main/java/frc/robot/subsystems/GyroSubsystem.java b/src/main/java/frc/robot/subsystems/GyroSubsystem.java deleted file mode 100644 index 4379042..0000000 --- a/src/main/java/frc/robot/subsystems/GyroSubsystem.java +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) Jack Nelson & Miami Beach Bots -// Source: https://www.maxbotix.com/firstrobotics - -package frc.robot.subsystems; - -import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** This Subsystem is what translates the output of the Gyro sensor to standard units. */ -public class GyroSubsystem extends SubsystemBase { - private final AHRS m_Gyro; - - /** Creates a new GyroSubsystem. */ - public GyroSubsystem() { - m_Gyro = new AHRS(SPI.Port.kMXP); - } - - public void calibrate() { - m_Gyro.calibrate(); - } - - public Rotation2d getRotation2d() { - return m_Gyro.getRotation2d(); - } - - // for balance correction - public double getPitch() { - return m_Gyro.getPitch(); // get pitch in degrees - } - - // for PID control (turn by degrees) - public double getAccumYaw() { - return m_Gyro.getAngle(); // get angle in degrees - } - - public double getYaw() { - return m_Gyro.getYaw(); - } - - public void reset() { - m_Gyro.reset(); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Current Gyro Pitch", getPitch()); - SmartDashboard.putNumber("Current Gyro Yaw", getYaw()); - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -}