diff --git a/simgui.json b/simgui.json index 4e5f439..935c3ae 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", @@ -46,6 +47,8 @@ "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [30]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [31]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", @@ -73,5 +76,8 @@ }, "open": true } + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7725175..8ae655f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,7 +105,6 @@ public final class Constants { public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double INCHES_PER_METER = 39.370; public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 51ed393..e4fd2ea 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -129,17 +129,17 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); - SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); + // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); + // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - m_robotContainer.m_robotBoomBoom.getCurrent() + + // m_robotContainer.m_robotBoomBoom.getCurrent() + m_robotContainer.m_robotClimber.getCurrent() + - m_robotContainer.m_robotHood.getCurrent() + + // m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + - m_robotContainer.m_robotSwerveDrive.getCurrent() + - m_robotContainer.m_robotTurret.getCurrent(); + m_robotContainer.m_robotSwerveDrive.getCurrent(); + // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb53f70..3da5b6a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -100,11 +100,10 @@ public class RobotContainer { public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); @@ -133,7 +132,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - testShoulderMotor.setNeutralMode(NeutralMode.Brake); + // testShoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -152,9 +151,9 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) - ); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) + // ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -167,8 +166,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -201,11 +200,11 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + // new JoystickButton(getDriverController(), XboxController.Button.kA.value) + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); - new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + // new JoystickButton(getDriverController(), XboxController.Button.kB.value) + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); // .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2))) // .whenReleased(new RunCommand(() -> testElbowMotor.set(0))); @@ -256,6 +255,13 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(1.0), m_robotIntake)) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-1.0), m_robotIntake)) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) @@ -265,13 +271,14 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 58fa705..3aed04d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -36,7 +36,7 @@ public class RobotMap { public RobotMap() { // configureLEDMotorControllers(); configureSwerveMotorControllers(); - configureShooterMotorControllers(); + // configureShooterMotorControllers(); } /* LED Subsystem */ @@ -162,73 +162,72 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); } - // Shooter Config - /* Boom Boom Subsystem */ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); +// // Shooter Config +// /* Boom Boom Subsystem */ +// public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); +// public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // turret subsystem +// // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); - // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); +// // hood subsystem +// public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - // Create motor CANSparkMax - void configureShooterMotorControllers() { +// // Create motor CANSparkMax +// void configureShooterMotorControllers() { - // LEFT FALCON - shooterFalconLeft.configFactoryDefault(); - shooterFalconLeft.setNeutralMode(NeutralMode.Coast); - shooterFalconLeft.setInverted(true); - shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); +// // LEFT FALCON +// shooterFalconLeft.configFactoryDefault(); +// shooterFalconLeft.setNeutralMode(NeutralMode.Coast); +// shooterFalconLeft.setInverted(true); +// shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); - // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); - shooterFalconRight.configFactoryDefault(); - shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - // m_shooterFalconRight.configPeakOutputForward(0, - // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); +// // RIGHT FALCON +// shooterFalconRight.setInverted(false); +// shooterFalconRight.setNeutralMode(NeutralMode.Coast); +// shooterFalconRight.configFactoryDefault(); +// shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); +// // m_shooterFalconRight.configPeakOutputForward(0, +// // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) +// shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.follow(shooterFalconLeft); +// shooterFalconRight.follow(shooterFalconLeft); - // /* Turret Subsytem */ - // shooterFalconRight.configStatorCurrentLimit(new - // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers - // out of our ass anymore - // shooterFalconLeft.configSupplyCurrentLimit(new - // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull - // numbers out of our ass anymore +// // /* Turret Subsytem */ +// // shooterFalconRight.configStatorCurrentLimit(new +// // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers +// // out of our ass anymore +// // shooterFalconLeft.configSupplyCurrentLimit(new +// // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull +// // numbers out of our ass anymore - // hood subsystem - angleAdjusterMotor.restoreFactoryDefaults(); - angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - } +// // hood subsystem +// angleAdjusterMotor.restoreFactoryDefaults(); +// angleAdjusterMotor.setIdleMode(IdleMode.kBrake); +// } /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); -// public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); /* Intake Subsytem */ diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 0a4b39c..1da9228 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -79,7 +79,10 @@ public double m_fireAngle; m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } - + /** + * Runs the hood with the given input + * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) + */ public void runHood(double input) { m_angleAdjusterMotor.set(input); } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 719891e..2754c6d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -47,9 +47,9 @@ public class Intake extends SubsystemBase { // This method will be called once per scheduler run } /** - * Runs The Intake With Triggers. - * @param leftTrigger Left Trigger to Run - - * @param rightTrigger Right Trigger to Run + + * Runs The Intake With Triggers as input + * @param leftTrigger Left Trigger to Run Inward + * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); @@ -65,7 +65,10 @@ public class Intake extends SubsystemBase { double extenderMotorSpeed = extended ? 0.25d : -0.25d; m_extenderMotor.set(extenderMotorSpeed); } - + /** + * Moves the extender motor to pull the intake in or out + * @param input A value from -1.0 to 1.0, positive is in + */ public void runExtender(double input) { if (!m_serializer.getBeam() && input < 0.) return; m_extenderMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index 54df449..99702fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -19,7 +19,10 @@ public class Serializer extends SubsystemBase{ // m_serializerShooterBelt.set(0); } - + /** + * + * @param input from -1.0 to 1.0, positive is inward + */ public void setSerializer(double input){ m_serializerBelt.set(input); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 0a0dc22..8f78712 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -36,7 +36,7 @@ public class Storage extends SubsystemBase { /** * Runs The Storage at a Specifyed Speed - * @param input The Specifyed Speed + * @param input The value frm -1.0 to 1.0, positive is inwards (towards the shooter) */ public void runStorage(double input) { m_storageMotor.set(input);