diff --git a/PathWeaver/Paths/GSC_ARED b/PathWeaver/Paths/GSC_ARED new file mode 100644 index 0000000..610bd7c --- /dev/null +++ b/PathWeaver/Paths/GSC_ARED @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,10.0,0.0,true, +10.0,-10.0,0.0,-10.0,true, diff --git a/PathWeaver/Paths/GSC_BRED b/PathWeaver/Paths/GSC_BRED new file mode 100644 index 0000000..610bd7c --- /dev/null +++ b/PathWeaver/Paths/GSC_BRED @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,10.0,0.0,true, +10.0,-10.0,0.0,-10.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index ec4f6d5..0a3c36a 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -4,6 +4,6 @@ "maxVelocity": 2.6, "maxAcceleration": 2.7, "wheelBase": 0.648, - "gameName": "Bounce Path", + "gameName": "Galactic Search A", "outputDir": ".." } \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index e33cc7b..7f63722 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,7 +1,16 @@ Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) -70,20,7000,2.5, -127,27,8467,2, -223,31.25,10398,2.75, -272,32.4,11776,2.5, -342,33,13733,2, -458,30.5,17000,0, \ No newline at end of file +0,16,12000,1, +65.9,16,12000,1, +103,19,12000,1, +126.6,20.28,12000,1.5, +156.6,28,12000,1.5, +174,28,12000,1.5, +178,28,12000,1.3 +180,28.5,12000,1.3, +185.85,28.5,12000,1.3, +187,28.5,12000,1.3 +200,28.4,12000,1.3 +231,28.4,12000,1.8, +245,28.8,12000,1.8, +262,28.8,12000,1.8, +999,28.8,12000,1.8, \ No newline at end of file diff --git a/src/main/deploy/Robot Data - DistancesOLD.csv b/src/main/deploy/Robot Data - DistancesOLD.csv index a15874a..61cc162 100644 --- a/src/main/deploy/Robot Data - DistancesOLD.csv +++ b/src/main/deploy/Robot Data - DistancesOLD.csv @@ -1,8 +1,63 @@ Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) -1,10,7769,0 -70,21,7769,0, check -100,24,8580,0 -145,28,9390,0 -230,31,11010,0,Add a 270 -397,33,14250,0 -415,33,14452,0 \ No newline at end of file +LOBING +0,16,6000,1, +65.9,16,6000,1, +126.6,20.25,7000,0.95, +180,21,7300,1, +185.85,23.5,7500,1.625, +245,26,9000,1, +262,28,9000,1.25, +999,28,9000,1.25, + +LASER +0,16,6000,1, +65.9,16,6000,1, +126.6,20.25,7000,0.95, +185.85,30,13000,1, +245,31,18000,1, +250,31,18000,1, +999,31,18000,1, + +IN BETWEEN +0,16,6000,1, +65.9,16,6000,1, +126.6,20.25,7000,0.95, +180,24,8000,1, +185.85,26.5, 9000,1.625, +245,27,11000,1, +262,27,11000,1.25, +999,27,11000,1.25, + +HOLD +0,16,6000,1, +65.9,16,6000,1, +103,19,6600,1, +126.6,20.25,7000,1.5, +156.6,22,7500,1.5, +174,23,8000,1.5, +180,23,8000,1.3, +185.85,26.5,9000,1.3, +231,28.8,9500,1.8, +245,28.8,9500,1.8, +262,28.8,9500,1.8, +999,28.8,9500,1.8, + + + + + + +Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) +0,16,6000,1, +65.9,16,6000,1, +103,19,6600,1, +126.6,20.25,7000,1.5, +156.6,23,7500,1.5, +174,28.5,13000,1.5, +180,28.5,13000,1.3, +185.85,28.5,13000,1.3, +190,28.5,13000,1.9 +231,28.5,13000,1.8, +245,28.8,13000,1.8, +262,28.8,13000,1.8, +999,28.8,13000,1.8, \ No newline at end of file diff --git a/src/main/driverStation/GOAT DRIVERSTATION.json b/src/main/driverStation/GOAT DRIVERSTATION.json index e61f688..486a0fe 100644 --- a/src/main/driverStation/GOAT DRIVERSTATION.json +++ b/src/main/driverStation/GOAT DRIVERSTATION.json @@ -624,6 +624,12 @@ "_title": "DigitalInput[12]/Value", "Colors/Color when true": "#7CFC00FF", "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [15]", + "_title": "Talon FX [15]", + "Visuals/Orientation": "HORIZONTAL" } ] } @@ -1003,6 +1009,138 @@ } } } + }, + { + "title": "Tab 5", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "3,0": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://limelight", + "_title": "limelight", + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": 0, + "imageHeight": 0 + } + }, + "7,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Fire Angle CSV", + "_title": "/SmartDashboard/Fire Angle CSV" + } + }, + "7,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drum Velocity CSV", + "_title": "/SmartDashboard/Drum Velocity CSV" + } + }, + "6,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance to Target", + "_title": "/SmartDashboard/Distance to Target" + } + }, + "3,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance to Target", + "_title": "/SmartDashboard/Distance to Target" + } + }, + "0,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drum Velocity", + "_title": "/SmartDashboard/Drum Velocity", + "Graph/Visible time": 30.0, + "Y-axis/Automatic bounds": true, + "Y-axis/Upper bound": 1.0, + "Y-axis/Lower bound": -1.0, + "Visible data/SmartDashboard/Drum Velocity": true + } + }, + "6,2": { + "size": [ + 4, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drum Velocity", + "_title": "/SmartDashboard/Drum Velocity", + "Graph/Visible time": 30.0, + "Y-axis/Automatic bounds": true, + "Y-axis/Upper bound": 1.0, + "Y-axis/Lower bound": -1.0, + "Visible data/SmartDashboard/Drum Velocity": true + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Turret Angle Raw", + "_title": "/SmartDashboard/Turret Angle Raw" + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Drum Ready", + "_title": "/SmartDashboard/Drum Ready", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + } + } + } } ], "windowGeometry": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c8a6e36..0964c31 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -112,7 +112,8 @@ public final class Constants { public static final class ShooterConstants { /* Motor IDs */ - public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_FALCON_BALLER_ID = 8; + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15; public static final int SHOOTER_ANGLE_ADJUST_ID = 10; public static final int SHOOTER_ROTATE_ID = 9; @@ -120,7 +121,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); //Ff was 0.055 //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); @@ -156,7 +157,7 @@ public final class Constants { } public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 15; + public static final int LEVELER_CAN_ID = 30; } public static final class IntakeConstants {; @@ -171,7 +172,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 0.5; + public static final double STORAGE_SPEED = 0.75; public static final double STORAGE_TIMEOUT = 3000; /* Storage Characteristics */ @@ -211,10 +212,10 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 71.5; + public static final double TARGET_HEIGHT = 67.5; public static final double LIME_ANGLE = 24.7; public static final double TURN_P_VALUE = 0.8; - public static final double X_ANGLE_ERROR = 1.3; + public static final double X_ANGLE_ERROR = 0.5; public static final double MOTOR_DEAD_ZONE = 0.2; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a39f05b..e83560f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -188,7 +188,7 @@ public class RobotContainer { // runs the hood with joystick m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(12000), m_robotShooter)); // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller @@ -209,10 +209,16 @@ public class RobotContainer { private void configureButtonBindings() { /* Test Buttons */ // A driver test button + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) + .whenReleased(new InterruptSubystem(m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotShooterHood)); + // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); + // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive)); @@ -253,17 +259,19 @@ public class RobotContainer { //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) .whenReleased(new InterruptSubystem(m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) @@ -274,10 +282,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) - //.whenPressed(new StoragePrep(m_robotStorage)) - //.whenReleased(new InterruptSubystem(m_robotStorage)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); - //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); + + //.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); + //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); @@ -290,6 +298,7 @@ public class RobotContainer { .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + //Run drum new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false) .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index b2aba9c..b39d6b9 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase { } */ - m_drive.driveWithInput(-moveOutput, steerOutput); + m_drive.driveWithInput(moveOutput, steerOutput * .8); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 5e28114..14e19ce 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -61,7 +61,7 @@ public class CalibrateShooter extends CommandBase { @Override public boolean isFinished() { if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() && - m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) { + m_shooterHood.m_hoodDownLimit.get()) { return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java index b750c66..100e077 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java @@ -35,7 +35,7 @@ public class ShooterGoalPosition extends CommandBase { @Override public void execute() { m_shooter.runDrumShooterVelocityPID(5000); - m_hood.runAngleAdjustPID(3); + m_hood.runAngleAdjustPID(4); m_aim.runshooterRotatePID(-26.5); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java index ba452b7..793f245 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java @@ -34,9 +34,9 @@ public class ShooterTrenchPosition extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(5000); - m_hood.runAngleAdjustPID(3); - m_aim.runshooterRotatePID(-26.5); + m_shooter.runDrumShooterVelocityPID(5500); + m_hood.runAngleAdjustPID(11); + //m_aim.runshooterRotatePID(-28); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index b3d3ac1..11ccd56 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -35,7 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase { @Override public void execute() { //Tells whether the target velocity has been reached - m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition(); + m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition(); m_targetVel = m_shooter.addFireVel(); double error = m_actualVel - m_targetVel; if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){ diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 3f46e9f..598c317 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -69,24 +69,26 @@ public class TrackTarget extends CommandBase { yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); // Finding Distance - distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); realDistance = (1.09 * distance) - 12.8; if (target == 1.0) { // If target in view // Aiming Left/Right xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance); - turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; + turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { turnAmount = 0; } // Angle Error Zone // Deadzones - else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = VisionConstants.MOTOR_DEAD_ZONE; - } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; + else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) { + turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE; + } else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) { + turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE; } + m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); + //m_shooterAim.runshooterRotatePID(targetAngle); SmartDashboard.putNumber("Distance to Target", realDistance); SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance)); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index e89ca83..cba7de3 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -313,7 +313,7 @@ public class Drive extends SubsystemBase { public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) { m_pneumaticsSubsystem = subsystem; m_shooter = shooter; - m_orchestra.addInstrument(m_shooter.m_shooterFalcon); + m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); } public void updateTime() { diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 890e0f7..a45894e 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,8 +21,8 @@ import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { - public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - + public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); + public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static Shooter m_shooter; public static IHandController m_controller; @@ -50,24 +50,42 @@ public class Shooter extends SubsystemBase { //SmartDashboard.putNumber("Velocity Target", 10000); //SmartDashboard.putNumber("Angle Target", 3); - m_shooterFalcon.configFactoryDefault(); - m_shooterFalcon.setNeutralMode(NeutralMode.Coast); - m_shooterFalcon.setInverted(true); - m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + //LEFT FALCON + m_shooterFalconLeft.configFactoryDefault(); + m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + m_shooterFalconLeft.setInverted(true); + m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + + //RIGHT FALCON + m_shooterFalconRight.configFactoryDefault(); + m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); + m_shooterFalconRight.setInverted(false); + m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); - m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - - m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - int closedLoopTimeMs = 1; - m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + //LEFT FALCON + m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + + //RIGHT FALCON + //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterTable = new ShooterTables(); - - m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); @@ -83,13 +101,13 @@ public class Shooter extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run try{ - SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); + SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); - SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); } @@ -118,18 +136,20 @@ public class Shooter extends SubsystemBase { * @param speed Speed to set the motor at */ public void runDrumShooter(double speed) { - m_shooterFalcon.set(speed); + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + m_shooterFalconRight.follow(m_shooterFalconLeft); + //m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed); } /** * Configures gains for shooter PID. */ public void setShooterGains() { - m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } /** @@ -138,6 +158,7 @@ public class Shooter extends SubsystemBase { */ public void runDrumShooterVelocityPID(double targetVel) { //System.out.println("Target Velocity" + targetVel); - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID + m_shooterFalconRight.follow(m_shooterFalconLeft); } } \ No newline at end of file