From 925554ae5e60780cab9df414b53aa3581ab6da6f Mon Sep 17 00:00:00 2001 From: WTMC Robotics Date: Wed, 6 Mar 2024 14:46:52 -0500 Subject: [PATCH] Fix bugs (#36) * Add a way to backtrack stuck notes * Make backtrack work + add back autoshoot * Remove now-unnecessary config class * Add support for brushed & brushless motors * Temporarily remove intake code * Fix iterator bug * Fix motor IDs & settings * Fix prettier issues * fix bad naming * fix forgetting to f2 * FIx motor IDs * Fix inputtedDriverControls * Fix auton selection * add negative sign * Fix shooting * yes * Declare motor controllers not in RobotInit anymore --- src/main/java/frc/robot/Constants.java | 26 ++++-- .../frc/robot/InputtedCoDriverControls.java | 12 ++- src/main/java/frc/robot/Robot.java | 93 +++++++++++++------ .../frc/robot/RobotComponents/DriveTrain.java | 2 +- .../frc/robot/RobotComponents/Intake.java | 9 +- .../frc/robot/RobotComponents/Shooter.java | 5 +- src/main/java/frc/robot/RobotConfigs.java | 38 -------- .../java/frc/robot/auton/AutonMoveInches.java | 2 +- src/main/java/frc/robot/auton/AutonShoot.java | 6 +- .../frc/robot/auton/ParallelActionRunner.java | 2 +- .../java/frc/robot/motor/MotorController.java | 4 +- .../robot/motor/MotorControllerFactory.java | 8 +- .../frc/robot/motor/SparkMotorController.java | 10 +- 13 files changed, 115 insertions(+), 102 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotConfigs.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69abcb3..6a78db3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,36 +5,41 @@ public class Constants { /*DRIVE TRAIN MOTOR IDs*/ public static final int DRIVE_LEFT_PARENT_ID = 4; public static final int DRIVE_LEFT_CHILD_ID = 8; - public static final int DRIVE_RIGHT_PARENT_ID = 11; + public static final int DRIVE_RIGHT_PARENT_ID = 14; public static final int DRIVE_RIGHT_CHILD_ID = 3; - public static final int SHOOTER_LEFT_FLYWHEEL_ID = 6; + public static final int SHOOTER_LEFT_FLYWHEEL_ID = 9; public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 7; - public static final int SHOOTER_FEEDER_ID = 9; + public static final int SHOOTER_FEEDER_ID = 10; + public static final int INTAKE_ID = 6101; /*CLIMB MOTOR IDS */ - public static final int LEFT_CLIMB_ID = 31; - public static final int RIGHT_CLIMB_ID = 21; + public static final int LEFT_CLIMB_ID = 6; + public static final int RIGHT_CLIMB_ID = 5; /*SHOOTER CONFIG*/ /**the amount of time we let the wheels rev up when shooting in seconds */ - public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 1.0; + public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 0.5; + public static final double SHOOTER_LEFT_FLYWHEEL_SPEED = 1.0; + public static final double SHOOTER_RIGHT_FLYWHEEL_SPEED = 1.0; /*The amount of seconds that the feeder should wait before turning off after shooting */ - public static final double FEEDER_MOTOR_SHUTOFF_TIME = 0; + + public static final double FEEDER_MOTOR_SHUTOFF_TIME = 1.5; + public static final double SHOOTING_VELOCITY_RPM = 4000; /*CLIMB CONFIG */ public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5; public static final double CLIMB_RETRACTED_ENCODER_POSITION = 0.5; /**the % of max power we give to the arm motors while extending */ - public static final double CLIMB_EXTENSION_SPEED = .25; + public static final double CLIMB_EXTENSION_SPEED = .5; /**the % of max power we give to the arm motors while retracting */ public static final double CLIMB_RETRACTION_SPEED = -.25; /*INTAKE CONFIG */ public static final double INTAKE_SPEED = 0; - //TODO this + public static final int INTAKE_BEAM_BREAK_ID = 9; /*DRIVE TRAIN CONFIG */ @@ -72,7 +77,8 @@ public class Constants { public static final int PID_LOOP_IDX = 0; /** amount of time in ms to wait for confirmation */ public static final int TIMEOUT_MS = 30; - public static final double ENCODER_ROTATION = 4096.0; + + public static final int ENCODER_ROTATION = 4096; /*OTHER CONFIG */ public static final double MOTOR_BACKTRACK_SPEED_PERCENT = -0.5; diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java index 87a46fb..dfd1cd0 100644 --- a/src/main/java/frc/robot/InputtedCoDriverControls.java +++ b/src/main/java/frc/robot/InputtedCoDriverControls.java @@ -17,15 +17,19 @@ public static void onEveryFrame() { if (controller.getYButtonPressed()) { Shooter.startFeedMotors(); } - if (controller.getXButton()) { + if (controller.getXButtonPressed()) { Shooter.startShooterMotors(); - } else { + } else if (controller.getBButtonPressed()) { + Shooter.autoShootIntoSpeaker(); + } + if (controller.getXButtonReleased()) { Shooter.stopShooterMotors(); } - if (controller.getLeftBumper()) { + if (controller.getLeftBumperPressed()) { Intake.startFloorIntake(); - } else { + } + if (controller.getLeftBumperReleased()) { Intake.stopFloorIntake(); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a954f1e..18a5acc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,7 @@ import frc.robot.auton.SequentialActionRunner; import frc.robot.motor.MotorController; import frc.robot.motor.MotorControllerFactory; -import frc.robot.vision.AprilTagHighlighter; +// import frc.robot.vision.AprilTagHighlighter; import java.util.ArrayDeque; /** @@ -31,29 +31,48 @@ public class Robot extends TimedRobot { //hi // hello!!!!!!! - AprilTagHighlighter aprilTagHighlighter; + // AprilTagHighlighter aprilTagHighlighter; public static RobotMotors motors; - MotorController driveLeftParent = RobotConfigs.getLeftParent(); - MotorController driveLeftChild = RobotConfigs.getLeftChild(); - MotorController driveRightParent = RobotConfigs.getRightParent(); - MotorController driveRightChild = RobotConfigs.getRightChild(); + MotorController driveLeftParent = MotorControllerFactory.create( + Constants.DRIVE_LEFT_PARENT_ID, + MotorController.Type.SparkMaxBrushless + ); + MotorController driveLeftChild = MotorControllerFactory.create( + Constants.DRIVE_LEFT_CHILD_ID, + MotorController.Type.SparkMaxBrushless + ); + MotorController driveRightParent = MotorControllerFactory.create( + Constants.DRIVE_RIGHT_PARENT_ID, + MotorController.Type.SparkMaxBrushless + ); + MotorController driveRightChild = MotorControllerFactory.create( + Constants.DRIVE_RIGHT_CHILD_ID, + MotorController.Type.SparkMaxBrushless + ); MotorController leftFlywheel = MotorControllerFactory.create( Constants.SHOOTER_LEFT_FLYWHEEL_ID, - MotorController.Type.SparkMax + MotorController.Type.SparkMaxBrushless ); MotorController rightFlywheel = MotorControllerFactory.create( Constants.SHOOTER_RIGHT_FLYWHEEL_ID, - MotorController.Type.SparkMax + MotorController.Type.SparkMaxBrushless ); MotorController feederMotor = MotorControllerFactory.create( Constants.SHOOTER_FEEDER_ID, - MotorController.Type.SparkMax + MotorController.Type.SparkMaxBrushless + ); + MotorController leftClimb = MotorControllerFactory.create( + Constants.LEFT_CLIMB_ID, + MotorController.Type.SparkMaxBrushed ); - MotorController leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.Talon); - MotorController rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.Talon); - MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMax); + MotorController rightClimb = MotorControllerFactory.create( + Constants.RIGHT_CLIMB_ID, + MotorController.Type.SparkMaxBrushed + ); + // MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushless); + XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID); XboxController coDriverController = new XboxController(Constants.CODRIVER_CONTROLLER_ID); static AHRS navX = new AHRS(SPI.Port.kMXP); @@ -80,9 +99,14 @@ public void robotInit() { initializeSmartMotion(driveRightParent, Constants.NORMAL_ROBOT_GAINS); // aprilTagHighlighter = new AprilTagHighlighter(); - autonRouteChooser.addOption("move forward", "move forward"); - autonRouteChooser.addOption("move backwards", "move backwards"); - SmartDashboard.putData(autonRouteChooser); + autonRouteChooser.setDefaultOption("move forward", "moveforward"); + autonRouteChooser.addOption("move forward", "moveforward"); + autonRouteChooser.addOption("move backwards", "movebackwards"); + autonRouteChooser.addOption("explode hidden bomb", "explodehiddenbomb"); + autonRouteChooser.addOption("shoot backup intake forward shoot", "shootbackupintakeforwardshoot"); + autonRouteChooser.addOption("backup turn backup", "backupturnbackup"); + autonRouteChooser.addOption("shoot and back up", "shootandbackup"); + SmartDashboard.putData("Auton Routes", autonRouteChooser); driveLeftChild.follow(driveLeftParent); driveRightChild.follow(driveRightParent); @@ -91,14 +115,25 @@ public void robotInit() { driveRightParent.setInverted(false); feederMotor.setInverted(true); + //This false is required feederMotor.setBrakeMode(false); + rightFlywheel.setInverted(true); + leftFlywheel.setInverted(false); + System.out.println("Is drive right parent inverted? " + driveRightParent.getInverted()); - driveLeftChild.setBrakeMode(false); - driveLeftParent.setBrakeMode(false); - driveRightChild.setBrakeMode(false); - driveRightParent.setBrakeMode(false); + System.out.println( + "Do we have cool navX? " + getGyroscope().isAltitudeValid() + " temp: " + getGyroscope().getTempC() + ); + + driveLeftChild.setBrakeMode(true); + driveLeftParent.setBrakeMode(true); + driveRightChild.setBrakeMode(true); + driveRightParent.setBrakeMode(true); + + leftClimb.setBrakeMode(true); + rightClimb.setBrakeMode(true); motors = new RobotMotors() @@ -110,8 +145,8 @@ public void robotInit() { .leftFlywheel(leftFlywheel) .rightFlywheel(rightFlywheel) .leftClimb(leftClimb) - .rightClimb(rightClimb) - .intake(intake); + .rightClimb(rightClimb); + // .intake(intake); getGyroscope().reset(); System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES); SmartDashboard.putNumber("rotationGainsP", Constants.ROTATION_GAINS.P); @@ -163,14 +198,12 @@ public void autonomousInit() { ArrayDeque route = switch (autonRouteChooser.getSelected()) { - case "move forward" -> new ArrayDeque<>(AutonRoutes.GO_FORWARD_OUT_OF_STARTING_ZONE); - case "move backwards" -> new ArrayDeque<>(AutonRoutes.GO_BACKWARD_OUT_OF_STARTING_ZONE); - case "shoot and back up" -> new ArrayDeque<>(AutonRoutes.SHOOT_AND_BACK_UP); - case "backup turn backup" -> new ArrayDeque<>(AutonRoutes.BACKUP_TURN_BACKUP); - case "shoot backup intake forward shoot" -> new ArrayDeque<>( - AutonRoutes.SHOOT_BACKUP_INTAKE_FORWARD_SHOOT - ); - case "explode hidden bomb" -> new ArrayDeque<>(AutonRoutes.BOOM); + case "moveforward" -> new ArrayDeque<>(AutonRoutes.GO_FORWARD_OUT_OF_STARTING_ZONE); + case "movebackwards" -> new ArrayDeque<>(AutonRoutes.GO_BACKWARD_OUT_OF_STARTING_ZONE); + case "shootandbackup" -> new ArrayDeque<>(AutonRoutes.SHOOT_AND_BACK_UP); + case "backupturnbackup" -> new ArrayDeque<>(AutonRoutes.BACKUP_TURN_BACKUP); + case "shootbackupintakeforwardshoot" -> new ArrayDeque<>(AutonRoutes.SHOOT_BACKUP_INTAKE_FORWARD_SHOOT); + case "explodehiddenbomb" -> new ArrayDeque<>(AutonRoutes.BOOM); default -> new ArrayDeque(); }; System.out.println("Selected auton route: " + route); @@ -299,7 +332,7 @@ public void initializeSmartMotion(MotorController motorController, Gains gains) */ /* Set relevant frame periods to be at least as fast as periodic rate */ - motorController.setStatusFramePeriod(10); + // motorController.setStatusFramePeriod(10); /* Set the peak and nominal outputs */ // motorController.setOutputLimits(0, 0, gains.PEAK_OUTPUT, -gains.PEAK_OUTPUT); diff --git a/src/main/java/frc/robot/RobotComponents/DriveTrain.java b/src/main/java/frc/robot/RobotComponents/DriveTrain.java index bd89233..07152e8 100644 --- a/src/main/java/frc/robot/RobotComponents/DriveTrain.java +++ b/src/main/java/frc/robot/RobotComponents/DriveTrain.java @@ -9,7 +9,7 @@ public class DriveTrain { public static void driveTank(double leftY, double rightY) { QuickActions.stopDriveMotors(); if (Math.abs(leftY) > Constants.CONTROLLER_DEADZONE) { - Robot.motors.getDriveLeftChild().set(leftY); + Robot.motors.getDriveLeftParent().set(leftY); } if (Math.abs(rightY) > Constants.CONTROLLER_DEADZONE) { Robot.motors.getDriveRightParent().set(rightY); diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java index 16c25bb..a593741 100644 --- a/src/main/java/frc/robot/RobotComponents/Intake.java +++ b/src/main/java/frc/robot/RobotComponents/Intake.java @@ -1,20 +1,17 @@ package frc.robot.robotcomponents; -import frc.robot.Constants; -import frc.robot.Robot; - public class Intake { public static void startFloorIntake() { - Robot.motors.getIntake().set(Constants.INTAKE_SPEED); + // Robot.motors.getIntake().set(Constants.INTAKE_SPEED); } /* This will spin the motor backwards in an attempt to eject a stuck note*/ public static void backtrack() { - Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); + // Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); } public static void stopFloorIntake() { - Robot.motors.getIntake().stopMotor(); + // Robot.motors.getIntake().stopMotor(); } } diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java index 9d8162b..d91e35e 100644 --- a/src/main/java/frc/robot/RobotComponents/Shooter.java +++ b/src/main/java/frc/robot/RobotComponents/Shooter.java @@ -8,8 +8,8 @@ public class Shooter { public static void startShooterMotors() { - Robot.motors.getLeftFlywheel().setVelocity(Constants.SHOOTING_VELOCITY_RPM); - Robot.motors.getRightFlywheel().setVelocity(Constants.SHOOTING_VELOCITY_RPM); + Robot.motors.getLeftFlywheel().set(Constants.SHOOTER_LEFT_FLYWHEEL_SPEED); + Robot.motors.getRightFlywheel().set(Constants.SHOOTER_RIGHT_FLYWHEEL_SPEED); } public static void stopShooterMotors() { @@ -32,6 +32,7 @@ public static void backtrack() { } public static void autoShootIntoSpeaker() { + Robot.getTeleopActionRunner().removeActionsOfType(AutonShoot.class); Robot.getTeleopActionRunner().addActionToRun(new AutonShoot()); } } diff --git a/src/main/java/frc/robot/RobotConfigs.java b/src/main/java/frc/robot/RobotConfigs.java deleted file mode 100644 index 693cd2c..0000000 --- a/src/main/java/frc/robot/RobotConfigs.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot; - -import frc.robot.motor.MotorController; -import frc.robot.motor.MotorControllerFactory; - -public class RobotConfigs { - - static RobotConfig config = RobotConfig.COMPETITION; - - public static MotorController getLeftParent() { - return config == RobotConfig.COMPETITION - ? MotorControllerFactory.create(Constants.DRIVE_LEFT_PARENT_ID, MotorController.Type.SparkMax) - : MotorControllerFactory.create(20, MotorController.Type.Talon); - } - - public static MotorController getLeftChild() { - return config == RobotConfig.COMPETITION - ? MotorControllerFactory.create(Constants.DRIVE_LEFT_CHILD_ID, MotorController.Type.SparkMax) - : MotorControllerFactory.create(22, MotorController.Type.Talon); - } - - public static MotorController getRightParent() { - return config == RobotConfig.COMPETITION - ? MotorControllerFactory.create(Constants.DRIVE_RIGHT_PARENT_ID, MotorController.Type.SparkMax) - : MotorControllerFactory.create(21, MotorController.Type.Talon); - } - - public static MotorController getRightChild() { - return config == RobotConfig.COMPETITION - ? MotorControllerFactory.create(Constants.DRIVE_RIGHT_CHILD_ID, MotorController.Type.SparkMax) - : MotorControllerFactory.create(23, MotorController.Type.Talon); - } -} - -enum RobotConfig { - COMPETITION, - PRACTICING, -} diff --git a/src/main/java/frc/robot/auton/AutonMoveInches.java b/src/main/java/frc/robot/auton/AutonMoveInches.java index e0c3afd..1201459 100644 --- a/src/main/java/frc/robot/auton/AutonMoveInches.java +++ b/src/main/java/frc/robot/auton/AutonMoveInches.java @@ -48,7 +48,7 @@ private double getMaxTrajectoryVelocity() { * @param distance The distance in inches */ public AutonMoveInches(double distance) { - this.distance = distance; + this.distance = -distance; } @Override diff --git a/src/main/java/frc/robot/auton/AutonShoot.java b/src/main/java/frc/robot/auton/AutonShoot.java index 0af18ed..ec93568 100644 --- a/src/main/java/frc/robot/auton/AutonShoot.java +++ b/src/main/java/frc/robot/auton/AutonShoot.java @@ -12,7 +12,7 @@ public class AutonShoot extends AutonAction { public boolean isDone() { if (Timer.getFPGATimestamp() >= revFinishedTime) { Robot.motors.getFeeder().set(0.2); - Robot.motors.getIntake().set(.2); + //Robot.motors.getIntake().set(.2); } return Timer.getFPGATimestamp() >= revFinishedTime + 1.0; @@ -20,8 +20,8 @@ public boolean isDone() { @Override public void initiate() { - Robot.motors.getLeftFlywheel().set(1.0); - Robot.motors.getRightFlywheel().set(-1.0); + Robot.motors.getLeftFlywheel().set(Constants.SHOOTER_LEFT_FLYWHEEL_SPEED); + Robot.motors.getRightFlywheel().set(Constants.SHOOTER_RIGHT_FLYWHEEL_SPEED); revFinishedTime = Timer.getFPGATimestamp() + Constants.SHOOTER_FLYWHEEL_STARTUP_TIME; } diff --git a/src/main/java/frc/robot/auton/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java index 35327a0..4efa347 100644 --- a/src/main/java/frc/robot/auton/ParallelActionRunner.java +++ b/src/main/java/frc/robot/auton/ParallelActionRunner.java @@ -19,7 +19,7 @@ public void onEveryFrame() { AutonAction action = it.next(); if (action.isDone()) { action.shutdown(); - actions.remove(action); + it.remove(); } } } diff --git a/src/main/java/frc/robot/motor/MotorController.java b/src/main/java/frc/robot/motor/MotorController.java index 46bb1d5..e144e34 100644 --- a/src/main/java/frc/robot/motor/MotorController.java +++ b/src/main/java/frc/robot/motor/MotorController.java @@ -178,6 +178,8 @@ public static enum Type { /** Talon SRX motor controller */ Talon, /** Spark MAX motor controller */ - SparkMax, + SparkMaxBrushless, + + SparkMaxBrushed, } } diff --git a/src/main/java/frc/robot/motor/MotorControllerFactory.java b/src/main/java/frc/robot/motor/MotorControllerFactory.java index 046b075..3ad548b 100644 --- a/src/main/java/frc/robot/motor/MotorControllerFactory.java +++ b/src/main/java/frc/robot/motor/MotorControllerFactory.java @@ -1,13 +1,17 @@ package frc.robot.motor; +import com.revrobotics.CANSparkLowLevel.MotorType; + public class MotorControllerFactory { public static MotorController create(int id, MotorController.Type type) { switch (type) { case Talon: return new TalonMotorController(id); - case SparkMax: - return new SparkMotorController(id); + case SparkMaxBrushless: + return new SparkMotorController(id, MotorType.kBrushless); + case SparkMaxBrushed: + return new SparkMotorController(id, MotorType.kBrushed); default: return null; } diff --git a/src/main/java/frc/robot/motor/SparkMotorController.java b/src/main/java/frc/robot/motor/SparkMotorController.java index 5d2e497..84bb157 100644 --- a/src/main/java/frc/robot/motor/SparkMotorController.java +++ b/src/main/java/frc/robot/motor/SparkMotorController.java @@ -16,9 +16,13 @@ public class SparkMotorController implements MotorController { RelativeEncoder encoder; SparkPIDController pid; - SparkMotorController(int canID) { - controller = new CANSparkMax(canID, MotorType.kBrushless); - encoder = controller.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + SparkMotorController(int canID, MotorType type) { + controller = new CANSparkMax(canID, type); + if (type == MotorType.kBrushless) { + encoder = controller.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + } else { + encoder = controller.getEncoder(SparkRelativeEncoder.Type.kQuadrature, Constants.ENCODER_ROTATION); + } encoder.setPositionConversionFactor(Constants.DRIVE_GEARBOX_RATIO); encoder.setVelocityConversionFactor(Constants.DRIVE_GEARBOX_RATIO);