diff --git a/src/main/java/frc/robot/DriveController.java b/src/main/java/frc/robot/DriveController.java index 28eeed8..67e1f72 100644 --- a/src/main/java/frc/robot/DriveController.java +++ b/src/main/java/frc/robot/DriveController.java @@ -1,21 +1,24 @@ package frc.robot; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.climber.LoosenClimber; import frc.robot.subsystems.climber.TightenClimber; import frc.robot.subsystems.swerve.SwerveTurnTo; +import java.util.Arrays; + /** *

This is the specific controller that controls Swerve due to the fact that swerve requires 2 separate joysticks and buttons to rezero the robot's gyro/position. *

This also contains controls for the climber since movement and the climber go hand in hand. *

This also contains Vision buttons (that currently do nothing) and a WestCoast control (that does not work, but was useful for testing with a different robot) */ -public class DriveController extends XboxController { +public class DriveController extends CommandXboxController { - private RobotMap map; - private RobocketsShuffleboard shuffleboard; + private final RobotMap map; + private final RobocketsShuffleboard shuffleboard; /** * Initializes the Shooter Controller and makes an internal copy of the RobotMap to save performance. @@ -27,6 +30,28 @@ public DriveController(int port, RobotMap map, RobocketsShuffleboard shuffleboar super(port); this.map = map; this.shuffleboard = shuffleboard; + + // TODO: this does not really do anything + a().toggleOnTrue(Commands.runOnce(() -> System.out.println(map.vision))); + + x().toggleOnTrue(Commands.runOnce(map.swerve::zeroGyro)); + y().toggleOnTrue(Commands.runOnce(map.swerve::resetPose)); + + rightBumper().toggleOnTrue( + Commands.startEnd( + this::loosenClimber, + this::tightenClimber, + map.climber + ) + ); + } + + private void loosenClimber() { + CommandScheduler.getInstance().schedule(new LoosenClimber()); + } + + private void tightenClimber() { + CommandScheduler.getInstance().schedule(new TightenClimber()); } /** @@ -50,9 +75,9 @@ public static double deadzone (double value, double deadzone) { private final int SMOOTH_FRAME_LENGTH = 5; private int smoothNextFrameToWrite = 0; - private double[] smoothLeftX = new double[SMOOTH_FRAME_LENGTH]; - private double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH]; - private double[] smoothRightX = new double[SMOOTH_FRAME_LENGTH]; + private final double[] smoothLeftX = new double[SMOOTH_FRAME_LENGTH]; + private final double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH]; + private final double[] smoothRightX = new double[SMOOTH_FRAME_LENGTH]; /** *

Applies input smoothing. @@ -61,17 +86,12 @@ public static double deadzone (double value, double deadzone) { * @return The average (mean) value of the input list of doubles. This will be the average value of a joystick axis. */ private double smooth(double[] history) { - double average = 0; - for(int i = 0; i < history.length; i++) { - average += history[i]; - } - average /= (double)history.length; - return average; + return Arrays.stream(history).average().orElse(0.0); } /** *

This should run during the Robot.java's teleopPeriodic method. - *

This applies input smoothing to the joystick axises to make them smoother. + *

This applies input smoothing to the joystick axex to make them smoother. *

This also checks for all button pushes and runs their respected Swerve commands/functions. */ public void teleopPeriodic() { @@ -87,60 +107,35 @@ public void teleopPeriodic() { double RightX = smooth(smoothRightX); // Swerve - if (map.swerve != null) { - double xyCof = 1;//0.75/Math.max(0.001, Math.sqrt(Math.pow(deadzone(controller.getLeftX(), 0.1), 2)+Math.pow(deadzone(controller.getLeftY(), 0.1), 2))); - - // ROBOT RELATIVE - // map.swerve.swerveDriveR(new ChassisSpeeds( - // SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards - // SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftX, 0.1), // Left/Right - // SmartDashboard.getNumber("Swerve Speed", 0.7) * deadzone(RightX, 0.08) // Rotation - // )); - - // FIELD RELATIVE - if (RightX==0) { - map.swerve.setDriveFXY( - // On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent. - shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards - shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right - true); //square inputs to ease small adjustments - map.swerve.setDriveRot(0, false); // Should not be rotating if not rotating lol - } else { - map.swerve.swerveDriveF( - // On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent. - shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards - shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right - shuffleboard.getSettingNum("Rotation Speed") * deadzone(RightX, 0.08), // Rotation - true); //square inputs to ease small adjustments - } - - if (getXButtonPressed()) { - map.swerve.zeroGyro(); - } - if (getYButtonPressed()) { - map.swerve.resetPose(); - } - - // turn to align with gyro - if(getPOV()!=-1) { - CommandScheduler.getInstance().schedule(new SwerveTurnTo(map.swerve, new Rotation2d(-getPOV()*0.01745329))); - } - } - - if (map.climber != null) { - if (getRightBumperPressed()) { - CommandScheduler.getInstance().schedule(new LoosenClimber()); - } - if (getRightBumperReleased()) { - CommandScheduler.getInstance().schedule(new TightenClimber()); - } + double xyCof = 1;//0.75/Math.max(0.001, Math.sqrt(Math.pow(deadzone(controller.getLeftX(), 0.1), 2)+Math.pow(deadzone(controller.getLeftY(), 0.1), 2))); + + // ROBOT RELATIVE + // map.swerve.swerveDriveR(new ChassisSpeeds( + // SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards + // SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftX, 0.1), // Left/Right + // SmartDashboard.getNumber("Swerve Speed", 0.7) * deadzone(RightX, 0.08) // Rotation + // )); + + // FIELD RELATIVE + if (RightX==0) { + map.swerve.setDriveFXY( + // On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent. + shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards + shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right + true); //square inputs to ease small adjustments + map.swerve.setDriveRot(0, false); // Should not be rotating if not rotating lol + } else { + map.swerve.swerveDriveF( + // On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent. + shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards + shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right + shuffleboard.getSettingNum("Rotation Speed") * deadzone(RightX, 0.08), // Rotation + true); //square inputs to ease small adjustments } - // Vision - if (map.vision != null) { - if(getAButtonPressed()){ - map.vision.toString(); - } + // turn to align with gyro + if(getHID().getPOV() !=-1) { + CommandScheduler.getInstance().schedule(new SwerveTurnTo(map.swerve, new Rotation2d(-getHID().getPOV() *0.01745329))); } // West Coast @@ -148,4 +143,6 @@ public void teleopPeriodic() { map.westcoast.arcadeDrive(getLeftY(), getRightX()); } } + + } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index d3e1568..4efbb44 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -2,12 +2,17 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.subsystems.climber.ClimberSubsystem; +import frc.robot.subsystems.climber.ClimberSubsystemInterface; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystemInterface; import frc.robot.subsystems.leds.LedSubsystem; +import frc.robot.subsystems.leds.LedSubsystemInterface; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystemInterface; import frc.robot.subsystems.swerve.SwerveDriveSubsystem; -import frc.robot.subsystems.swerve.SwerveModuleNeo; +import frc.robot.subsystems.swerve.SwerveDriveSubsystemInterface; import frc.robot.subsystems.vision.VisionSubsystem; +import frc.robot.subsystems.vision.VisionSubsystemInterface; import frc.robot.subsystems.westcoast.WestCoastSubsystem; /** @@ -18,24 +23,14 @@ */ public class RobotMap { - - // Swerve - // Gian: why are these neos here if we end up just making new ones in the swervedrivesubsystem? - public SwerveModuleNeo swerve_frontLeftModule; - public SwerveModuleNeo swerve_frontRightModule; - public SwerveModuleNeo swerve_backLeftModule; - public SwerveModuleNeo swerve_backRightModule; - - public SwerveDriveSubsystem swerve = null; - public VisionSubsystem vision = null; - public IntakeSubsystem intake = null; - public ShooterSubsystem shooter = null; + public final SwerveDriveSubsystemInterface swerve; + public final VisionSubsystemInterface vision; + public final IntakeSubsystemInterface intake; + public final ShooterSubsystemInterface shooter; public WestCoastSubsystem westcoast = null; - public LedSubsystem leds = null; - public ClimberSubsystem climber = null; + public final LedSubsystemInterface leds; + public final ClimberSubsystemInterface climber; - // Gian: Ok neat system, this is not something I did on the team - // But why is the swerve drive commented out? /** *

Contains all the subsystems for the robot to avoid static initialization order problems. *

Various subsystems will be constantly commented out for testing as not all subsystems exist on the robot at one time. @@ -44,12 +39,12 @@ public class RobotMap */ public RobotMap() { - // intake = new IntakeSubsystem(); - // swerve = new SwerveDriveSubsystem(new Translation2d(0.31115, 0.31115), new Translation2d(0.31115, -0.31115), new Translation2d(-0.31115, 0.31115), new Translation2d(-0.31115, -0.31115)); // All translations are the swerve module positions relative to the center of the bot - // vision = new VisionSubsystem(); - shooter = new ShooterSubsystem(); - // leds = new LedSubsystem(); - // climber = new ClimberSubsystem(); + intake = IntakeSubsystem.create(); + swerve = SwerveDriveSubsystem.create(new Translation2d(0.31115, 0.31115), new Translation2d(0.31115, -0.31115), new Translation2d(-0.31115, 0.31115), new Translation2d(-0.31115, -0.31115)); // All translations are the swerve module positions relative to the center of the bot + vision = VisionSubsystem.create(); + shooter = ShooterSubsystem.create(); + leds = LedSubsystem.create(); + climber = ClimberSubsystem.create(); // ONLY FOR TESTING // westcoast = new WestCoastSubsystem(); diff --git a/src/main/java/frc/robot/ShooterController.java b/src/main/java/frc/robot/ShooterController.java index 2e273b5..4311bad 100644 --- a/src/main/java/frc/robot/ShooterController.java +++ b/src/main/java/frc/robot/ShooterController.java @@ -1,14 +1,17 @@ package frc.robot; -import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +import java.util.Arrays; /** * This is the code for the controller that controls the shooter and the intake. */ -public class ShooterController extends XboxController { +public class ShooterController extends CommandXboxController { - private RobotMap map; - private RobocketsShuffleboard shuffleboard; + private final RobotMap map; + private final RobocketsShuffleboard shuffleboard; /** * Initializes the Shooter Controller and makes an internal copy of the RobotMap to save performance. @@ -20,6 +23,92 @@ public ShooterController(int port, RobotMap map, RobocketsShuffleboard shufflebo super(port); this.map = map; this.shuffleboard = shuffleboard; + + a().toggleOnTrue( + Commands.startEnd( + this::shooterIn, + this::stopShooter, + map.shooter + ) + ); + b().toggleOnTrue( + Commands.startEnd( + this::shooterOut, + this::stopShooter, + map.shooter + ) + ); + x().toggleOnTrue( + Commands.startEnd( + this::shooterIntakeIn, + this::stopShooterIntake, + map.shooter + ) + ); + y().toggleOnTrue( + Commands.startEnd( + this::shooterIntakeOut, + this::stopShooterIntake, + map.shooter + ) + ); + + leftBumper().toggleOnTrue( + Commands.startEnd( + this::intakeOn, + this::intakeOff, + map.intake, map.leds + ) + ); + rightBumper().toggleOnTrue( + Commands.startEnd( + this::outtakeOn, + this::outtakeOff, + map.intake, map.leds + ) + ); + } + + private void shooterIn() { + map.shooter.setShooterSpeed(shuffleboard.getSettingNum("Shooter In Speed")); + } + + private void shooterOut() { + map.shooter.setShooterSpeed(-shuffleboard.getSettingNum("Shooter Out Speed")); + } + + private void stopShooter() { + map.shooter.setShooterSpeed(0); + } + + private void shooterIntakeIn() { + map.shooter.setIntakeSpeed(shuffleboard.getSettingNum("Shooter Intake Speed")); + } + + private void shooterIntakeOut() { + map.shooter.setIntakeSpeed(-shuffleboard.getSettingNum("Shooter Outtake Speed")); + } + + private void stopShooterIntake() { + map.shooter.setIntakeSpeed(0); + } + + private void intakeOn() { + map.intake.intake(shuffleboard.getSettingNum("Intake Speed")); + map.leds.NoteIndicator(true); + } + + private void intakeOff() { + map.intake.intake(0); + } + + private void outtakeOn() { + map.intake.outtake(shuffleboard.getSettingNum("Outtake Speed")); + map.leds.NoteIndicator(false); + } + + private void outtakeOff() { + map.intake.outtake(0); } /** @@ -45,8 +134,8 @@ public static double deadzone (double value, double deadzone) { private final int SMOOTH_FRAME_LENGTH = 5; private int smoothNextFrameToWrite = 0; - private double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH]; // Contains the past {SMOOTH_FRAME_LENGTH} number of inputs. - private double[] smoothRightY = new double[SMOOTH_FRAME_LENGTH]; // Contains the past {SMOOTH_FRAME_LENGTH} number of inputs. + private final double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH]; // Contains the past {SMOOTH_FRAME_LENGTH} number of inputs. + private final double[] smoothRightY = new double[SMOOTH_FRAME_LENGTH]; // Contains the past {SMOOTH_FRAME_LENGTH} number of inputs. /** *

Applies input smoothing. @@ -55,12 +144,7 @@ public static double deadzone (double value, double deadzone) { * @return The average (mean) value of the input list of doubles. This will be the average value of a joystick axis. */ private double smooth(double[] history) { - double average = 0; - for(int i = 0; i < history.length; i++) { - average += history[i]; - } - average /= (double)history.length; - return average; + return Arrays.stream(history).average().orElse(0.0); } /** @@ -78,67 +162,9 @@ public void teleopPeriodic() { double RightY = smooth(smoothRightY); // Intake - if (map.intake != null) { - - map.intake.rotate(RightY); - - if (getLeftBumperPressed()) { - map.intake.intake(shuffleboard.getSettingNum("Intake Speed")); - } - else if (getRightBumperPressed()) { - map.intake.outtake(shuffleboard.getSettingNum("Outtake Speed")); - } - } + map.intake.rotate(RightY); // Shooter - if (map.shooter != null) { - - map.shooter.rotate(LeftY); - - if (getAButtonPressed()) { - //CommandScheduler.getInstance().schedule(new Shoot(SmartDashboard.getNumber("Shooter Speed", 0.5))); - map.shooter.setShooterSpeed(shuffleboard.getSettingNum("Shooter In Speed")); - } - if (getBButtonPressed()) { - //CommandScheduler.getInstance().schedule(new Shoot(-SmartDashboard.getNumber("Shooter Speed", 0.5))); - map.shooter.setShooterSpeed(-shuffleboard.getSettingNum("Shooter Out Speed")); - } - if (getYButtonPressed()) { - map.shooter.setIntakeSpeed(shuffleboard.getSettingNum("Shooter Intake Speed")); - } - if (getXButtonPressed()) { - map.shooter.setIntakeSpeed(-shuffleboard.getSettingNum("Shooter Outtake Speed")); - } - if (getAButtonReleased() || getBButtonReleased()) { - map.shooter.setShooterSpeed(0); - } - if (getXButtonReleased() || getYButtonReleased()) { - map.shooter.setIntakeSpeed(0); - } - } - - //LEDs - if (map.leds != null){ - //if the intake button is pressed it will turn the LEds to orange - if(getLeftBumperPressed()) - { - map.leds.NoteIndicator(true); - } - - if(getLeftBumperReleased()) - { - map.leds.NoteIndicator(true); - } - //if the outake button is pressed it will turn the LEDs off - if(getRightBumperPressed()) - { - map.leds.NoteIndicator(false); - } - //if the outake button is pressed it will turn the LEDs off - if(getAButtonPressed()) - { - map.leds.NoteIndicator(false); - } - } + map.shooter.rotate(LeftY); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 2f85da1..9d9fc07 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -9,14 +9,29 @@ *

This should be the simplest subsystem as all it does is run a motor full force in one direction or let it go slack. *

NOTE: The motor that is connected MUST NOT BE on break mode. */ -public class ClimberSubsystem { +public class ClimberSubsystem implements ClimberSubsystemInterface { private CANSparkMax motor; // The motor connected to the string inside the climber private double speed; // The speed which the motor is constantly set to. + // -------------------------------------------------- + // Factory pattern + // -------------------------------------------------- + + public static ClimberSubsystemInterface create() { + try { + return new ClimberSubsystem(); + } catch (Throwable t) { + t.printStackTrace(); + return new ClimberSubsystemMock(); + } + } + + // -------------------------------------------------- + /** *

Initializes the motor. That is all. */ - public ClimberSubsystem() { + private ClimberSubsystem() { motor = new CANSparkMax(Constants.CLIMBER_MOTOR_PORT, MotorType.kBrushless); } @@ -32,6 +47,7 @@ public void periodic() { *

This will be used to tighten and loosen the force on the string inside the climber. * @param speed The speed to set the motor at as a value between -1.0 and 1.0. */ + @Override public void setSpeed(double speed) { this.speed = speed; } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemInterface.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemInterface.java new file mode 100644 index 0000000..a4c9549 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemInterface.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface ClimberSubsystemInterface extends Subsystem { + void setSpeed(double speed); +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemMock.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemMock.java new file mode 100644 index 0000000..a7ea7af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystemMock.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimberSubsystemMock extends SubsystemBase implements ClimberSubsystemInterface { + @Override + public void setSpeed(double speed) { + + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/LoosenClimber.java b/src/main/java/frc/robot/subsystems/climber/LoosenClimber.java index 47f57d8..25e3120 100644 --- a/src/main/java/frc/robot/subsystems/climber/LoosenClimber.java +++ b/src/main/java/frc/robot/subsystems/climber/LoosenClimber.java @@ -8,7 +8,11 @@ *

This will be used to hook the climber onto the chain as it needs to be fully extended in order to go over the chain. */ public class LoosenClimber extends Command { - + + public LoosenClimber() { + addRequirements(Robot.getMap().climber); + } + /** *

Literally all this does is make sure the climber motor is slack and the climber can extend up. */ diff --git a/src/main/java/frc/robot/subsystems/climber/TightenClimber.java b/src/main/java/frc/robot/subsystems/climber/TightenClimber.java index 483a077..0bbabd4 100644 --- a/src/main/java/frc/robot/subsystems/climber/TightenClimber.java +++ b/src/main/java/frc/robot/subsystems/climber/TightenClimber.java @@ -8,7 +8,10 @@ *

This will also be used to actually climb. Once the climber is hooked up to the chain, it should be tightened to lift the robot up. */ public class TightenClimber extends Command { - + public TightenClimber() { + addRequirements(Robot.getMap().climber); + } + /** *

Literally all this does is make sure the climber motor speed is constantly running to pull it inside. */ diff --git a/src/main/java/frc/robot/subsystems/intake/FullIntake.java b/src/main/java/frc/robot/subsystems/intake/FullIntake.java index ba0ee1a..f5a3560 100644 --- a/src/main/java/frc/robot/subsystems/intake/FullIntake.java +++ b/src/main/java/frc/robot/subsystems/intake/FullIntake.java @@ -20,6 +20,7 @@ public class FullIntake extends Command { * @param map The RobotMap of the robot to improve performance. */ public FullIntake(double speed, RobotMap map) { + addRequirements(map.intake, map.shooter); this.speed = speed; this.map = map; this.timeOut = System.currentTimeMillis() + 5000; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index a80ae65..9cc8b5a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.intake; -import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -11,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -public class IntakeSubsystem extends SubsystemBase{ +public class IntakeSubsystem extends SubsystemBase implements IntakeSubsystemInterface { // Neos that actually intake (left or right facing forward) private CANSparkMax intakeL; private CANSparkMax intakeR; @@ -25,8 +23,16 @@ public class IntakeSubsystem extends SubsystemBase{ private static double INTAKE_ANGLE_OFFSET = 0.0; // Should be set such that when the arm is fully outstretched (perpendicular with the ground), the encoder measures 0 radians/degrees. This is in arbitrary encoder units. + public static IntakeSubsystemInterface create() { + try { + return new IntakeSubsystem(); + } catch (Throwable t) { + t.printStackTrace(); + return new IntakeSubsystemMock(); + } + } - public IntakeSubsystem() { + private IntakeSubsystem() { intakeL = new CANSparkMax(Constants.INTAKE_LEFT_PORT, MotorType.kBrushless); intakeR = new CANSparkMax(Constants.INTAKE_RIGHT_PORT, MotorType.kBrushless); angleMotorLeft = new CANSparkMax(Constants.INTAKE_ANGLE_LEFT_MOTOR_PORT, MotorType.kBrushless); @@ -45,6 +51,7 @@ public void periodic() { *

Gets the intake to {targetAngle} radians using PID and Feed Forward. *

This must be called during the periodic function to work. */ + @Override public void getIntakeToSetAngle() { double currentAngle = getIntakeAngle().getRadians(); double currentVelocity = getIntakeAngleVelocity(); @@ -58,6 +65,7 @@ public void getIntakeToSetAngle() { *

This runs the intake motors so that it actually intakes. * @param speed The speed to run the motors as a number between 0.0 to 1.0 */ + @Override public void intake(double speed) { intakeL.set(speed); intakeR.set(-speed); @@ -67,6 +75,7 @@ public void intake(double speed) { *

This runs the outtake motors so that it spits out whatever it has in it. * @param speed The speed to run the motors at as a number between 0.0 to 1.0 */ + @Override public void outtake(double speed) { intakeL.set(-speed); intakeR.set(speed); @@ -76,6 +85,7 @@ public void outtake(double speed) { *

This sets the target rotation to what it currently is plus the offset in radians where up is positive and down is negative. * @param offsetRadians */ + @Override public void rotate(double offsetRadians) { targetAngle = new Rotation2d(targetAngle.getRadians() + offsetRadians); } @@ -84,6 +94,7 @@ public void rotate(double offsetRadians) { *

This sets the target rotation of the intake to {rotation} and will get to that rotation during its periodic function where up is positive and down is negative. * @param rotation The new rotation to get to. */ + @Override public void goToRotation(Rotation2d rotation) { targetAngle = rotation; } @@ -92,6 +103,7 @@ public void goToRotation(Rotation2d rotation) { *

Determines the angle of the shooter based off of the left motor's current position after applying an offset. * @return the angle of the shooter in radians where up is positive and 0 radians is perpendicular with the ground. */ + @Override public Rotation2d getIntakeAngle() { return new Rotation2d((angleMotorLeft.getEncoder().getPosition() - INTAKE_ANGLE_OFFSET) * Constants.NEO_UNITS_TO_RADIANS); } @@ -100,6 +112,7 @@ public Rotation2d getIntakeAngle() { *

Gets the current moving velocity of the angle mechanism of the shooter. * @return The speed at which the shooter's angle changes in meters per second. */ + @Override public double getIntakeAngleVelocity() { return (angleMotorLeft.getEncoder().getVelocity() * Constants.SHOOTER_RPM_TO_MPS); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemInterface.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemInterface.java new file mode 100644 index 0000000..e30b446 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemInterface.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface IntakeSubsystemInterface extends Subsystem { + void getIntakeToSetAngle(); + + void intake(double speed); + + void outtake(double speed); + + void rotate(double offsetRadians); + + void goToRotation(Rotation2d rotation); + + Rotation2d getIntakeAngle(); + + double getIntakeAngleVelocity(); +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemMock.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemMock.java new file mode 100644 index 0000000..b5e9edf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystemMock.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakeSubsystemMock extends SubsystemBase implements IntakeSubsystemInterface { + Rotation2d rotate2d = new Rotation2d(); + @Override + public void getIntakeToSetAngle() { + } + + @Override + public void intake(double speed) { + + } + + @Override + public void outtake(double speed) { + + } + + @Override + public void rotate(double offsetRadians) { + + } + + @Override + public void goToRotation(Rotation2d rotation) { + rotate2d = rotation; + } + + @Override + public Rotation2d getIntakeAngle() { + return rotate2d; + } + + @Override + public double getIntakeAngleVelocity() { + return 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/leds/LedSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LedSubsystem.java index 1c0523d..0b7b0aa 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/leds/LedSubsystem.java @@ -2,98 +2,44 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import javax.imageio.ImageIO; -import java.io.File; -import java.io.IOException; -import java.awt.image.BufferedImage; - - - -public class LedSubsystem extends SubsystemBase { +public class LedSubsystem extends SubsystemBase implements LedSubsystemInterface { public static final int LED_WIDTH=8; public static final int LED_LENGTH=32; public static final int LED_SIZE = LED_WIDTH*LED_LENGTH; - int[] RGB = new int[LED_SIZE * 3]; - BufferedImage hello; - BufferedImage there; - int ticks; - int currentPort = 0; - + // -------------------------------------------------- + // Factory pattern + // -------------------------------------------------- + public static LedSubsystemInterface create() { + try { + return new LedSubsystem(); + } catch (Throwable t) { + t.printStackTrace(); + return new LedSubsystemMock(); + } + } + + // -------------------------------------------------- AddressableLED leds; AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(LED_SIZE); - public LedSubsystem() { - + private LedSubsystem() { try { leds = new AddressableLED((int)SmartDashboard.getNumber("LED Port", 0)); leds.setLength(LED_SIZE); leds.start(); - // hello = createImageIcon("hello.png"); - // there = createImageIcon("there.png"); - } + } catch (Exception e) { e.printStackTrace(); } } - public void DisplayImage(BufferedImage image) { - for (int xPos = 0; xPos < 32; xPos++) { - for (int yPos = 0; yPos < 8; yPos++) { - int pixelColor = image.getRGB(xPos, yPos); - RGB[(yPos * 32 + xPos) * 3] = (pixelColor & 0x00ff0000) >> 16; - RGB[(yPos * 32 + xPos) * 3 + 1] = (pixelColor & 0x0000ff00) >> 8; - RGB[(yPos * 32 + xPos) * 3 + 2] = (pixelColor & 0x000000ff); - } - } - - leds.setLength(LED_SIZE); - - for (int i = 0; i < LED_SIZE * 3; i += 3) { - ledBuffer.setRGB(i / 3, RGB[i], RGB[i + 1], RGB[i + 2]); - } - - leds.setData(ledBuffer); - - } - - private static BufferedImage createImageIcon(String name) { - File imgDir = new File(Filesystem.getDeployDirectory(), "images"); - File imgFile = new File(imgDir, name); - if (imgFile.exists()) { - try { - return ImageIO.read(imgFile); - } catch (IOException e) { - e.printStackTrace(); - } - } else { - System.err.println("*** Couldn't find file: " + imgFile.getAbsolutePath()); - } - return null; - } - - public void LED_SetFromTextFile() { - - leds.setLength(LED_SIZE); - - for (int i = 0; i < LED_SIZE * 3; i += 3) { - ledBuffer.setRGB(i, RGB[i], RGB[i + 1], RGB[i + 2]); - leds.setData(ledBuffer); - } - - } - - public void turnOff() { - SetAllColor(0,0,0); - leds.close(); - } - + @Override public void SetAllColor(int r, int g, int b) { for (int i = 0; i < LED_SIZE; i++) { ledBuffer.setRGB(i, r, g, b); @@ -101,7 +47,8 @@ public void SetAllColor(int r, int g, int b) { leds.setData(ledBuffer); } - public void SetRowColor(int row,int r, int g, int b) { + @Override + public void SetRowColor(int row, int r, int g, int b) { int start = row * LED_WIDTH; for (int i = 0; i < LED_WIDTH; ++i) { ledBuffer.setRGB(start + i, r, g, b); @@ -109,38 +56,25 @@ public void SetRowColor(int row,int r, int g, int b) { leds.setData(ledBuffer); } - public void StartColor() { - SetAllColor(30, 245, 30); - } - - public void Periodic(int databoardPort) { - if (databoardPort != currentPort) { - currentPort = databoardPort; - leds = new AddressableLED(currentPort); - } - ticks++; - if (ticks < 250){ - StartColor(); - } else if (ticks < 500) { - DisplayImage(hello); - } else { - DisplayImage(there); - } - if (ticks > 750) { - ticks = 250; - } - } //Indicator of having a Note and the LEDs beung that indidcator - public void NoteIndicator (boolean HaveNote) { - if (HaveNote) - { + @Override + public void NoteIndicator(boolean HaveNote) { + if (HaveNote) { SetAllColor(250, 90, 0); - } - else - { + } else { SetAllColor(0,0,0); } } + + @Override + public void NoteIndicatorOn() { + NoteIndicator(true); + } + + @Override + public void NoteIndicatorOff() { + NoteIndicator(false); + } } diff --git a/src/main/java/frc/robot/subsystems/leds/LedSubsystemInterface.java b/src/main/java/frc/robot/subsystems/leds/LedSubsystemInterface.java new file mode 100644 index 0000000..f5054bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LedSubsystemInterface.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface LedSubsystemInterface extends Subsystem { + void SetAllColor(int r, int g, int b); + + void SetRowColor(int row, int r, int g, int b); + + //Indicator of having a Note and the LEDs beung that indidcator + void NoteIndicator(boolean HaveNote); + + void NoteIndicatorOn(); + void NoteIndicatorOff(); +} diff --git a/src/main/java/frc/robot/subsystems/leds/LedSubsystemMock.java b/src/main/java/frc/robot/subsystems/leds/LedSubsystemMock.java new file mode 100644 index 0000000..4128331 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LedSubsystemMock.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LedSubsystemMock extends SubsystemBase implements LedSubsystemInterface { + @Override + public void SetAllColor(int r, int g, int b) { + + } + + @Override + public void SetRowColor(int row, int r, int g, int b) { + + } + + @Override + public void NoteIndicator(boolean HaveNote) { + + } + + @Override + public void NoteIndicatorOn() { + NoteIndicator(true); + } + + public void NoteIndicatorOff() { + NoteIndicator(false); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/GetShooterToAngle.java b/src/main/java/frc/robot/subsystems/shooter/GetShooterToAngle.java index 023e0b2..92d94fd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/GetShooterToAngle.java +++ b/src/main/java/frc/robot/subsystems/shooter/GetShooterToAngle.java @@ -8,7 +8,7 @@ */ public class GetShooterToAngle extends Command { private double angle; - private ShooterSubsystem shooter; + private ShooterSubsystemInterface shooter; /** *

Initializes a copy of the shooter subsystem for better performance as well as the angle it should be getting to in radians. @@ -17,6 +17,7 @@ public class GetShooterToAngle extends Command { public GetShooterToAngle(double angleRadians) { this.angle = angleRadians; this.shooter = Robot.getMap().shooter; + addRequirements(this.shooter); } /** @@ -34,8 +35,7 @@ public void initialize() { */ @Override public boolean isFinished() { - if (Math.abs(shooter.getShooterAngle() - angle) < 0.01) // If the shooter is within 0.01 radians of the target, stop. - return true; - return false; + // If the shooter is within 0.01 radians of the target, stop. + return Math.abs(shooter.getShooterAngle() - angle) < 0.01; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 340930f..4dc9466 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -public class ShooterSubsystem extends SubsystemBase { +public class ShooterSubsystem extends SubsystemBase implements ShooterSubsystemInterface { private TalonFX shooterLeft; // Motor for the left of the actual shooter, assuming that the front of the shooter is the forward direction private TalonFX shooterRight; // Motor for the right of the actual shooter, assuming that the front of the shooter is the forward direction private CANSparkMax intakeLeft; // Motor for the left intake of the shooter, assuming that the front of the shooter is the forward direction @@ -31,7 +31,19 @@ public class ShooterSubsystem extends SubsystemBase { private final double SHOOTER_ANGLE_OFFSET = 0.0; // Should be set such that when the arm is fully outstretched (perpendicular with the ground), the encoder measures 0 radians/degrees. This is in arbitrary encoder units. + // -------------------------------------------------- + // Factory pattern + // -------------------------------------------------- + public static ShooterSubsystemInterface create() { + try { + return new ShooterSubsystem(); + } catch (Throwable t) { + t.printStackTrace(); + } + return new ShooterSubsystemMock(); + } + // -------------------------------------------------- public ShooterSubsystem() { shooterLeft = new TalonFX(Constants.SHOOTER_LEFT_MOTOR_PORT); //top and bottom no? @@ -65,6 +77,7 @@ public void periodic() { *

Uses some basic PID and feedforwards to get the shooter to spin at a set speed in rotations per second. *

This must be called during the periodic function to work. */ + @Override public void getShooterToSetSpeed() { double avgEncoderSpd = (shooterLeft.getVelocity().getValueAsDouble()+shooterRight.getVelocity().getValueAsDouble())/2; //rotations per second allegedly @@ -82,6 +95,7 @@ public void getShooterToSetSpeed() { *

Gets the shooter to {targetAngle} radians using PID and Feed Forward. *

This must be called during the periodic function to work. */ + @Override public void getShooterToSetAngle() { double currentAngle = getShooterAngle(); double currentVelocity = getShooterAngleVelocity(); @@ -95,6 +109,7 @@ public void getShooterToSetAngle() { *

Sets the SHOOTING speed in rotations per second (hopefully), does not change until stated otherwise * @param speed The speed of the shooter in rotations per second. */ + @Override public void setShooterSpeed(double speed) { targetSpeed = speed; } @@ -103,6 +118,7 @@ public void setShooterSpeed(double speed) { *

Sets the target angle for the shooter to be oriented in (looking up or down) * @param angleRadians The new angle to get to in radians where 0 radians is fully outstretched and positive radians is upwards. */ + @Override public void setShooterAngle(double angleRadians) { targetAngle = angleRadians; } @@ -111,6 +127,7 @@ public void setShooterAngle(double angleRadians) { *

Sets the target angle for the shooter to what it currently is plus {angleRadians} where up is positive and down is negative * @param angleRadians The offset the shooter angle should get to in radians. */ + @Override public void rotate(double angleRadians) { targetAngle += angleRadians; } @@ -119,6 +136,7 @@ public void rotate(double angleRadians) { *

Sets the speed of the intake to the motor's speed. (the belt that pulls the game piece from the actual intake) * @param speed The speed of the intake as a number between -1.0 and 1.0 inclusive which represents 100% speed outtake and intake respectively. */ + @Override public void setIntakeSpeed(double speed) { intakeLeft.set(speed); intakeRight.set(-speed); @@ -128,6 +146,7 @@ public void setIntakeSpeed(double speed) { *

Determines the angle of the shooter based off of the left motor's current position after applying an offset. * @return the angle of the shooter in radians where up is positive and 0 radians is perpendicular with the ground. */ + @Override public double getShooterAngle() { return (angleMotorRight.getEncoder().getPosition() - SHOOTER_ANGLE_OFFSET) * Constants.NEO_UNITS_TO_RADIANS; } @@ -136,6 +155,7 @@ public double getShooterAngle() { *

Gets the current moving velocity of the angle mechanism of the shooter. * @return The speed at which the shooter's angle changes in meters per second. */ + @Override public double getShooterAngleVelocity() { return (angleMotorRight.getEncoder().getVelocity() * Constants.SHOOTER_RPM_TO_MPS); } @@ -144,6 +164,7 @@ public double getShooterAngleVelocity() { *

Using the break beam sensor, this returns whether or not there is a piece in the upper shooter. * @return True if there is a piece in the upper shooter. False if there is none. */ + @Override public boolean isPieceInUpperIntake() { return intakeUpperSensor.get(); } @@ -152,6 +173,7 @@ public boolean isPieceInUpperIntake() { *

Using the break beam sensor, this returns whether or not there is a piece in the lower shooter. * @return True if there is a piece in the lower shooter. False if there is none. */ + @Override public boolean isPieceInLowerIntake() { return intakeLowerSensor.get(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemInterface.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemInterface.java new file mode 100644 index 0000000..0d4ee65 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemInterface.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface ShooterSubsystemInterface extends Subsystem { + void getShooterToSetSpeed(); + + void getShooterToSetAngle(); + + void setShooterSpeed(double speed); + + void setShooterAngle(double angleRadians); + + void rotate(double angleRadians); + + void setIntakeSpeed(double speed); + + double getShooterAngle(); + + double getShooterAngleVelocity(); + + boolean isPieceInUpperIntake(); + + boolean isPieceInLowerIntake(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemMock.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemMock.java new file mode 100644 index 0000000..8766eb5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystemMock.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystemMock extends SubsystemBase implements ShooterSubsystemInterface { + @Override + public void getShooterToSetSpeed() { + } + + @Override + public void getShooterToSetAngle() { + + } + + @Override + public void setShooterSpeed(double speed) { + + } + + @Override + public void setShooterAngle(double angleRadians) { + + } + + @Override + public void rotate(double angleRadians) { + + } + + @Override + public void setIntakeSpeed(double speed) { + + } + + @Override + public double getShooterAngle() { + return 0; + } + + @Override + public double getShooterAngleVelocity() { + return 0; + } + + @Override + public boolean isPieceInUpperIntake() { + return false; + } + + @Override + public boolean isPieceInLowerIntake() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java index 906452f..b636f16 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java @@ -7,7 +7,6 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -36,7 +35,7 @@ *

All measurements are in meters and radians (but rotation generally uses Rotation2d which includes both radians and degrees) *

To make this consistent with PathPlanner and all other commands/odometry, the +x direction should be forwards and the +y direction should be left. */ -public class SwerveDriveSubsystem extends SubsystemBase { +public class SwerveDriveSubsystem extends SubsystemBase implements SwerveDriveSubsystemInterface { SwerveModuleState[] targetStates = new SwerveModuleState[4]; @@ -78,9 +77,22 @@ public class SwerveDriveSubsystem extends SubsystemBase { private double speedX = 0; private double speedY = 0; + // -------------------------------------------------- + // Factory pattern + // -------------------------------------------------- + public static SwerveDriveSubsystemInterface create(Translation2d fL, Translation2d fR, Translation2d bL, Translation2d bR) { + try { + return new SwerveDriveSubsystem(fL, fR, bL, bR); + } catch (Throwable t) { + t.printStackTrace(); + return new SwerveDriveSubsystemMock(); + } + } + + // -------------------------------------------------- // positions of the wheels relative to center in meters. - public SwerveDriveSubsystem (Translation2d fL, Translation2d fR, Translation2d bL, Translation2d bR) { + private SwerveDriveSubsystem(Translation2d fL, Translation2d fR, Translation2d bL, Translation2d bR) { m_kinematics = new SwerveDriveKinematics(fL, fR, bL, bR); // Load the relative positions of all our swerve modules (wheels) in relation to the origin. m_pose = new Pose2d(); // Starts the position at 0,0 m_odometry = new SwerveDriveOdometry(m_kinematics, getGyroRotation(), m_swervePositions, m_pose); // Start the odometry at 0,0 @@ -93,6 +105,7 @@ public SwerveDriveSubsystem (Translation2d fL, Translation2d fR, Translation2d b *

Holonomic (Swerve) autobuilder from pathplanner *

See https://pathplanner.dev/pplib-build-an-auto.html#configure-autobuilder for more */ + @Override public void configureAutoBuilder() { // This just gets the PID values of one motor. All 4 should be equal though!! // SparkPIDController spcd = m_backLeftModule.getDriveMotor().getPIDController(); // d for drive @@ -236,6 +249,7 @@ public void periodic() { // try applying acceleration cap to inputs in drives instead of on wheels // set how fast the swerve drive turns, rad/s allegedly + @Override public void setDriveRot(double sR, boolean squareInputs) { if (squareInputs) { sR=Math.signum(sR)*sR*sR; @@ -244,6 +258,7 @@ public void setDriveRot(double sR, boolean squareInputs) { } // set how fast the swerve drive goes, +x is forwards, +y is left and m/s hopefully + @Override public void setDriveFXY(double sX, double sY, boolean squareInputs) { isRobotRelative = false; @@ -259,6 +274,7 @@ public void setDriveFXY(double sX, double sY, boolean squareInputs) { private int lastDone = 10; // Cycles to sample rotation to make corrections to direction // Field Oriented swerve drive, m/s, m/s, rad/s or something, +x is forwards, +y is left + @Override public void swerveDriveF(double sX, double sY, double sR, boolean squareInputs) { isRobotRelative = false; @@ -280,6 +296,7 @@ public void swerveDriveF(double sX, double sY, double sR, boolean squareInputs) // Robot oriented swerve drive, m/s, m/s, rad/s // +speed is forwards, +strafe is left + @Override public void swerveDriveR(double speed, double strafe, double speedRot) { isRobotRelative = true; speedX = speed; @@ -288,6 +305,7 @@ public void swerveDriveR(double speed, double strafe, double speedRot) { // targetStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds(speed, -strafe, speedRot)); } + @Override public void swerveDriveR(ChassisSpeeds newTargetStates) { isRobotRelative = true; double relativeSpeed = 1; @@ -299,6 +317,7 @@ public void swerveDriveR(ChassisSpeeds newTargetStates) { SmartDashboard.putNumber("Relative SpeedRot", newTargetStates.omegaRadiansPerSecond); } + @Override public boolean isOnRedAlliance() { Optional ally = DriverStation.getAlliance(); if(ally.isPresent()) { @@ -308,6 +327,7 @@ public boolean isOnRedAlliance() { } // car, m/s, degrees + @Override public void carDrive(double speed, double turn) { turn = MathUtil.clamp(turn, -90, 90); @@ -320,12 +340,14 @@ public void carDrive(double speed, double turn) { } + @Override public void setTargetAngle(Rotation2d r) { pointDir = r; } // stuff // Degrees + @Override public double getGyroDegrees() { // Subtracted because the gyro was upside down meaning counter clockwise and clockwise were reversed... return getGyroRotation().getDegrees(); @@ -333,6 +355,7 @@ public double getGyroDegrees() { private final Rotation2d TWOPI = new Rotation2d(Math.PI*2); // TO BE REMOVED once the gyro is not upside down // Radians + @Override public Rotation2d getGyroRotation() { // Subtracted because the gyro was upside down meaning counter clockwise and clockwise were reversed... //return MathStuff.subtract(TWOPI,m_gyro.getAngle().minus(gyroOffset)); @@ -341,17 +364,20 @@ public Rotation2d getGyroRotation() { return new Rotation2d(m_gyro.getAngle()*0.01745329251).minus(gyroOffset); } + @Override public Pose2d getPose() { return m_pose; } //for on the go field oriented and stuff + @Override public void zeroGyro() { pointDir = pointDir.minus(getGyroRotation()); gyroOffset = gyroOffset.plus(getGyroRotation()); } // Reset the expected position of the bot + @Override public void resetPose() { m_odometry = new SwerveDriveOdometry(m_kinematics, getGyroRotation(), m_swervePositions); m_pose = new Pose2d(); @@ -359,6 +385,7 @@ public void resetPose() { // Reset the inputted pose // Only used by the holonomic builder + @Override public void resetPose(Pose2d pose2d) { pose2d = new Pose2d(); // m_odometry.resetPosition(getGyroRotation(), m_swervePositions,m_pose); @@ -367,6 +394,7 @@ public void resetPose(Pose2d pose2d) { // This is mainly just for testing. This is what the wiki says to do. + @Override public Pose2d getPoseForPathPlanner() { // System.out.println(m_odometry.getPoseMeters()); return m_pose; @@ -375,24 +403,28 @@ public Pose2d getPoseForPathPlanner() { } // This is mainly just for testing. This is what the wiki says to do. + @Override public void resetPoseForPathPlanner(Pose2d pose) { m_odometry.resetPosition(getGyroRotation(), m_swervePositions , m_pose); m_pose = pose; } // Returns the current robot-relative chasis speeds. + @Override public ChassisSpeeds getRobotRelativeSpeeds() { ChassisSpeeds cs = m_kinematics.toChassisSpeeds(m_swerveStates); // System.out.println("Speeds: " + cs); return cs; } + @Override public void stop() { m_frontLeftModule.setSpeeds(0, 0); m_frontRightModule.setSpeeds(0, 0); m_backLeftModule.setSpeeds(0, 0); m_backRightModule.setSpeeds(0, 0); } + @Override public void test() { targetStates[0] = new SwerveModuleState(0.2, new Rotation2d()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemInterface.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemInterface.java new file mode 100644 index 0000000..1484865 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemInterface.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface SwerveDriveSubsystemInterface extends Subsystem { + void configureAutoBuilder(); + + // set how fast the swerve drive turns, rad/s allegedly + void setDriveRot(double sR, boolean squareInputs); + + // set how fast the swerve drive goes, +x is forwards, +y is left and m/s hopefully + void setDriveFXY(double sX, double sY, boolean squareInputs); + + // Field Oriented swerve drive, m/s, m/s, rad/s or something, +x is forwards, +y is left + void swerveDriveF(double sX, double sY, double sR, boolean squareInputs); + + // Robot oriented swerve drive, m/s, m/s, rad/s + // +speed is forwards, +strafe is left + void swerveDriveR(double speed, double strafe, double speedRot); + + void swerveDriveR(ChassisSpeeds newTargetStates); + + boolean isOnRedAlliance(); + + // car, m/s, degrees + void carDrive(double speed, double turn); + + void setTargetAngle(Rotation2d r); + + // stuff + // Degrees + double getGyroDegrees(); + + // Radians + Rotation2d getGyroRotation(); + + Pose2d getPose(); + + //for on the go field oriented and stuff + void zeroGyro(); + + // Reset the expected position of the bot + void resetPose(); + + // Reset the inputted pose + // Only used by the holonomic builder + void resetPose(Pose2d pose2d); + + // This is mainly just for testing. This is what the wiki says to do. + Pose2d getPoseForPathPlanner(); + + // This is mainly just for testing. This is what the wiki says to do. + void resetPoseForPathPlanner(Pose2d pose); + + // Returns the current robot-relative chasis speeds. + ChassisSpeeds getRobotRelativeSpeeds(); + + void stop(); + + void test(); +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemMock.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemMock.java new file mode 100644 index 0000000..8809eb6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemMock.java @@ -0,0 +1,108 @@ +package frc.robot.subsystems.swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SwerveDriveSubsystemMock extends SubsystemBase implements SwerveDriveSubsystemInterface { + @Override + public void configureAutoBuilder() { + + } + + @Override + public void setDriveRot(double sR, boolean squareInputs) { + + } + + @Override + public void setDriveFXY(double sX, double sY, boolean squareInputs) { + + } + + @Override + public void swerveDriveF(double sX, double sY, double sR, boolean squareInputs) { + + } + + @Override + public void swerveDriveR(double speed, double strafe, double speedRot) { + + } + + @Override + public void swerveDriveR(ChassisSpeeds newTargetStates) { + + } + + @Override + public boolean isOnRedAlliance() { + return false; + } + + @Override + public void carDrive(double speed, double turn) { + + } + + @Override + public void setTargetAngle(Rotation2d r) { + + } + + @Override + public double getGyroDegrees() { + return 0; + } + + @Override + public Rotation2d getGyroRotation() { + return null; + } + + @Override + public Pose2d getPose() { + return null; + } + + @Override + public void zeroGyro() { + + } + + @Override + public void resetPose() { + + } + + @Override + public void resetPose(Pose2d pose2d) { + + } + + @Override + public Pose2d getPoseForPathPlanner() { + return null; + } + + @Override + public void resetPoseForPathPlanner(Pose2d pose) { + + } + + @Override + public ChassisSpeeds getRobotRelativeSpeeds() { + return null; + } + + @Override + public void stop() { + + } + + @Override + public void test() { + + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveGoCartesianF.java b/src/main/java/frc/robot/subsystems/swerve/SwerveGoCartesianF.java index 3a902ac..6024cc5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveGoCartesianF.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveGoCartesianF.java @@ -11,7 +11,7 @@ *

In this, the forwards direction is +x and the left direction is +y */ public class SwerveGoCartesianF extends Command { - private SwerveDriveSubsystem m_swerve; + private SwerveDriveSubsystemInterface m_swerve; private Translation2d target; private double Pvalue = 0; private double Ivalue = 0; @@ -25,7 +25,7 @@ public class SwerveGoCartesianF extends Command { * @param swerve A reference to the SwerveDriveSubsystem in the RobotMap to improve performance. * @param trans The offset from the current position. Example, Translation2d(1, 0) would move the robot forwards 1 meter regardless of where it currently is. */ - public SwerveGoCartesianF(SwerveDriveSubsystem swerve, Translation2d trans) { + public SwerveGoCartesianF(SwerveDriveSubsystemInterface swerve, Translation2d trans) { m_swerve = swerve; addRequirements(m_swerve); // Make it so no 2 commands can access the swerve subsystem at the same time (first come first swerve) target = m_swerve.getPose().getTranslation().plus(trans); // Set the target POSITION (not translation) @@ -38,7 +38,7 @@ public SwerveGoCartesianF(SwerveDriveSubsystem swerve, Translation2d trans) { * @param trans The offset from the current position. Example, Translation2d(1, 0) would move the robot forwards 1 meter regardless of where it currently is. * @param speedLimit The max speed that the robot can get to as a number between 0.0 - 1.0 as 0% speed to 100% speed. */ - public SwerveGoCartesianF(SwerveDriveSubsystem swerve, Translation2d trans, double speedLimit) { + public SwerveGoCartesianF(SwerveDriveSubsystemInterface swerve, Translation2d trans, double speedLimit) { m_swerve = swerve; addRequirements(m_swerve); target = m_swerve.getPose().getTranslation().plus(trans); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTurn.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTurn.java index 3a8d1bc..7532194 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveTurn.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTurn.java @@ -8,7 +8,7 @@ *

This does NOT turn to a specific rotation, this turns an offset from the current rotation. */ public class SwerveTurn extends Command { - private SwerveDriveSubsystem m_swerve; + private SwerveDriveSubsystemInterface m_swerve; private Rotation2d target; private double Pvalue = 0; @@ -18,7 +18,7 @@ public class SwerveTurn extends Command { * @param swerve Reference to the RobotMap's SwerveDriveSubsystem to improve performance. * @param rot The desired rotation to turn the robot by in degrees. Once again, this is NOT the desired final rotation, this is just an offset from the current rotation. */ - public SwerveTurn(SwerveDriveSubsystem swerve, Rotation2d rot) { + public SwerveTurn(SwerveDriveSubsystemInterface swerve, Rotation2d rot) { m_swerve = swerve; addRequirements(m_swerve); target = m_swerve.getGyroRotation().plus(rot); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTurnTo.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTurnTo.java index 952dd83..a38176f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveTurnTo.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTurnTo.java @@ -8,7 +8,7 @@ *

This does NOT turn to a specific rotation, this turns an offset from the current rotation. */ public class SwerveTurnTo extends Command { - private SwerveDriveSubsystem m_swerve; + private SwerveDriveSubsystemInterface m_swerve; private Rotation2d target; private double Pvalue = 0; @@ -18,7 +18,7 @@ public class SwerveTurnTo extends Command { * @param swerve Reference to the RobotMap's SwerveDriveSubsystem to improve performance. * @param rot The desired rotation to turn the robot to by in degrees/radians (it's a Rotation2d so it doesn't matter). */ - public SwerveTurnTo(SwerveDriveSubsystem swerve, Rotation2d rot) { + public SwerveTurnTo(SwerveDriveSubsystemInterface swerve, Rotation2d rot) { m_swerve = swerve; addRequirements(m_swerve); target = rot; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 9c62d71..444192a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -22,7 +22,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; -public class VisionSubsystem extends SubsystemBase { +public class VisionSubsystem extends SubsystemBase implements VisionSubsystemInterface { //**ALL ANGLES IN RADIANS ALL DISTANCES IN METERS**// PhotonCamera mCamera; @@ -35,6 +35,15 @@ public class VisionSubsystem extends SubsystemBase { private RobotMap map = Robot.getMap(); + public static VisionSubsystemInterface create() { + try { + return new VisionSubsystem(); + } catch (Throwable t) { + t.printStackTrace(); + return new VisionSubsystemMock(); + } + } + public VisionSubsystem(){ //Replace with name of cam mCamera = new PhotonCamera("Camera"); @@ -58,22 +67,27 @@ public void periodic() { } } + @Override public Pose3d getTagPose(int id) { return tagFieldLayout.getTagPose(id).orElse(null); } + @Override public PhotonPipelineResult getLatestResult(){ return mCamera.getLatestResult(); } + @Override public int getBestTagID(){ return mCamera.getLatestResult().getBestTarget().getFiducialId(); } + @Override public boolean hasTargets(){ return mCamera.getLatestResult().hasTargets(); } + @Override public void dance(){ double x = 0.0; double y = 0.0; @@ -81,8 +95,8 @@ public void dance(){ double[] smoothY = new double[]{0,0,0,0,0}; int currentSmooth = 0; - smoothX[currentSmooth] = map.vision.poseX; - smoothY[currentSmooth] = map.vision.poseY; + smoothX[currentSmooth] = poseX; + smoothY[currentSmooth] = poseY; currentSmooth++; currentSmooth%=5; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemInterface.java new file mode 100644 index 0000000..1469ff3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemInterface.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj2.command.Subsystem; +import org.photonvision.targeting.PhotonPipelineResult; + +public interface VisionSubsystemInterface extends Subsystem { + Pose3d getTagPose(int id); + + PhotonPipelineResult getLatestResult(); + + int getBestTagID(); + + boolean hasTargets(); + + void dance(); +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemMock.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemMock.java new file mode 100644 index 0000000..ac05601 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemMock.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.photonvision.targeting.PhotonPipelineResult; + +public class VisionSubsystemMock extends SubsystemBase implements VisionSubsystemInterface { + @Override + public Pose3d getTagPose(int id) { + return new Pose3d(); + } + + @Override + public PhotonPipelineResult getLatestResult() { + return new PhotonPipelineResult(); + } + + @Override + public int getBestTagID() { + return 0; + } + + @Override + public boolean hasTargets() { + return false; + } + + @Override + public void dance() { + } +}