mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Small refactoring
This commit is contained in:
nathanrsxtn
@@ -345,4 +345,13 @@ public final class Constants {
|
||||
public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV;
|
||||
public static final double DEGREES_PER_PIXEL = 1 / PIXELS_PER_DEGREE;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double PATH_MAX_VEL = 3.0;
|
||||
public static final double PATH_MAX_ACCEL = 3.0;
|
||||
public static final double LOCK_ON_DURATION = 0.8;
|
||||
public static final double LOCK_ON_TIME_ALLOWANCE = 1.6;
|
||||
public static final double STORAGE_TIME_TWO_BALLS = 1.5;
|
||||
public static final double STORAGE_TIME_ONE_BALL = 0.8;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,51 +4,36 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Vector2D;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the TimedRobot
|
||||
* documentation. If you change the name of this class or the package after
|
||||
* creating this project, you must also update the build.gradle file in the
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
// private double current;
|
||||
|
||||
// private static DesmosServer desmosServer = new DesmosServer(8000);
|
||||
|
||||
public static Alliance alliance;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
* This function is run when the robot is first started up and should be used for any initialization
|
||||
* code.
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
LOGGER.fine("robotInit()");
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
@@ -57,138 +42,88 @@ public class Robot extends TimedRobot {
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
// var path =
|
||||
// PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
|
||||
// Forward.path").toFile());
|
||||
// LOGGER.finest(path::toString);
|
||||
LOGGER.fine("robotInit()");
|
||||
|
||||
|
||||
|
||||
// LOGGER.fine("Sssssssssh.");
|
||||
// DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings,
|
||||
// and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our autonomous
|
||||
// chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
|
||||
SmartDashboard.putData(CommandScheduler.getInstance());
|
||||
|
||||
// desmosServer.start();
|
||||
|
||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
||||
ExtenderIntakeGroup.setDirectionToOut();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* This function is called every robot packet, no matter the mode. Use this for items like
|
||||
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>
|
||||
* This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
* This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard
|
||||
* integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00);
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
|
||||
|
||||
// System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString());
|
||||
// System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString());
|
||||
// System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString());
|
||||
|
||||
// current =
|
||||
// m_robotContainer.m_robotBoomBoom.getCurrent() +
|
||||
// m_robotContainer.m_robotClimber.getCurrent(); //+
|
||||
// m_robotContainer.m_robotHood.getCurrent() +
|
||||
// m_robotContainer.m_robotIntake.getCurrent() +
|
||||
// m_robotContainer.m_robotExtender.getCurrent() +
|
||||
// m_robotContainer.m_robotSerializer.getCurrent() +
|
||||
// m_robotContainer.m_robotStorage.getCurrent() +
|
||||
// m_robotContainer.m_robotSwerveDrive.getCurrent();
|
||||
// m_robotContainer.m_robotTurret.getCurrent();
|
||||
// SmartDashboard.putNumber("Total Robot Current Draw", current);
|
||||
// SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
|
||||
// SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding
|
||||
// newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or
|
||||
// interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the
|
||||
// robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled commands,
|
||||
// running already-scheduled commands, removing finished or interrupted commands, and running
|
||||
// subsystem periodic() methods. This must be called from the robot's periodic block in order for
|
||||
// anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
|
||||
// print odometry data to smart dashboard for debugging (if causing timeout
|
||||
// errors, you can comment it)
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
* the robot is disabled.
|
||||
* This function is called once each time the robot enters Disabled mode. You can use it to reset
|
||||
* any subsystem information you want to clear when the robot is disabled.
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
LOGGER.fine("disabledInit()");
|
||||
m_robotTime.endMatchTime();
|
||||
|
||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {
|
||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
||||
}
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your
|
||||
* {@link RobotContainer} class.
|
||||
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
LOGGER.fine("autonomousInit()");
|
||||
|
||||
Robot.alliance = DriverStation.getAlliance();
|
||||
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
LOGGER.fine("teleopInit()");
|
||||
|
||||
Robot.alliance = DriverStation.getAlliance();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
// This makes sure that the autonomous stops running when teleop starts running.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
public void teleopPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
@@ -199,6 +134,5 @@ public class Robot extends TimedRobot {
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
@Override
|
||||
public void testPeriodic() {
|
||||
}
|
||||
public void testPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Objects;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
@@ -18,28 +17,26 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.Preferences;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.PathRecorder;
|
||||
import frc4388.robot.commands.RunCommandForTime;
|
||||
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender;
|
||||
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
||||
import frc4388.robot.commands.shooter.TimedWaitUntilCommand;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
import frc4388.robot.commands.shuffleboard.CommandSchedule;
|
||||
import frc4388.robot.commands.shuffleboard.ShooterTuner;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
@@ -55,579 +52,286 @@ import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.Vector2D;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
//TODO: Try using ConditionalCommand for subsystem default commands.
|
||||
//TODO: Replace Path Recorder with Auto Chooser.
|
||||
//TODO: Add POV button pad bindings as an example.
|
||||
//XXX: Re-enable extender in autonomous.
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
|
||||
* subsystems, commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
||||
|
||||
// RobotMap
|
||||
/* Robot Map */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40);
|
||||
public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
|
||||
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
||||
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.frontLeft, m_robotMap.frontRight, m_robotMap.backLeft, m_robotMap.backRight, m_robotMap.gyro);
|
||||
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt);
|
||||
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
||||
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
||||
|
||||
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
|
||||
public 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 Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
/* Autonomous */
|
||||
/* Dashboard Tools */
|
||||
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
||||
|
||||
private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
|
||||
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
|
||||
private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false);
|
||||
// Controllers
|
||||
private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
||||
|
||||
public static boolean softLimits = true;
|
||||
|
||||
private static boolean softLimits = true;
|
||||
|
||||
// control mode switching
|
||||
public static enum ControlMode { SHOOTER, CLIMBER };
|
||||
public static ControlMode currentControlMode = ControlMode.SHOOTER;
|
||||
private enum ControlMode {
|
||||
SHOOTER, CLIMBER
|
||||
}
|
||||
|
||||
// turret mode switching
|
||||
private enum TurretMode { MANUAL, AUTONOMOUS };
|
||||
private TurretMode currentTurretMode = TurretMode.MANUAL;
|
||||
|
||||
// climber mode switching
|
||||
private enum ClimberMode { MANUAL, AUTONOMOUS };
|
||||
private ClimberMode currentClimberMode = ClimberMode.MANUAL;
|
||||
private ControlMode currentControlMode = ControlMode.SHOOTER;
|
||||
|
||||
// drive on off mode switching
|
||||
private enum DriveMode { ON, OFF };
|
||||
private enum DriveMode {
|
||||
ON, OFF
|
||||
}
|
||||
|
||||
private DriveMode currentDriveMode = DriveMode.ON;
|
||||
|
||||
// private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
|
||||
public SendableChooser<Command> autoChooser = new SendableChooser<Command>();
|
||||
|
||||
/**
|
||||
* SmartDash
|
||||
* - Limelight cam X
|
||||
* - Limit switches X
|
||||
* - Shooter RPM X
|
||||
* - Distance to target x
|
||||
* - target locked
|
||||
* - claws boolean
|
||||
* - field
|
||||
*/
|
||||
|
||||
// private SequentialCommandGroup makeTheWeirdGroup() {
|
||||
// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new ParallelCommandGroup(
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
|
||||
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
// return weirdAutoShootingGroup;
|
||||
// }
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
autoChooser.addOption("DriveOffLineAndShoot", driveOffLineAndShoot);
|
||||
autoChooser.addOption("OneBallAuto", oneBallAuto);
|
||||
autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto);
|
||||
autoChooser.addOption("ThreeBallAuto", threeBallAuto);
|
||||
// autoChooser.addOption("instantThreeBallSingleShotAutoSequence", instantThreeBallSingleShotAutoSequence);
|
||||
autoChooser.addOption("instantThreeBallDoubleSingleShotAutoSequence", instantThreeBallDoubleSingleShotAutoSequence);
|
||||
autoChooser.addOption("threeBallPlus", threeBallPlus);
|
||||
|
||||
SmartDashboard.putData("AutoChooser", autoChooser);
|
||||
|
||||
Preferences.initString("Autonomous", "Three Ball");
|
||||
|
||||
configureButtonBindings();
|
||||
/* Default Commands */
|
||||
// Swerve Drive with Input
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(),
|
||||
getDriverController().getRight(), true);
|
||||
}
|
||||
if (currentDriveMode == DriveMode.OFF) {
|
||||
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
||||
}
|
||||
}, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true);
|
||||
}
|
||||
if (currentDriveMode == DriveMode.OFF) {
|
||||
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
||||
}
|
||||
}, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// Intake with Triggers
|
||||
m_robotIntake.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotIntake.runWithTriggers(
|
||||
getOperatorController().getLeftTriggerAxis(),
|
||||
getOperatorController().getRightTriggerAxis()),
|
||||
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
|
||||
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
|
||||
m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand"));
|
||||
|
||||
// Serializer Manual
|
||||
m_robotSerializer.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
|
||||
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
||||
// TODO: Comment
|
||||
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand"));
|
||||
|
||||
// Turret Manual
|
||||
m_robotTurret.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
|
||||
}, m_robotTurret));
|
||||
// Serializer Manual
|
||||
m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand"));
|
||||
|
||||
// Hood Manual
|
||||
m_robotHood.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getLeftY()); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
|
||||
}, m_robotHood));
|
||||
// Turret Manual
|
||||
m_robotTurret.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotTurret.runTurretWithInput(getOperatorController().getLeftX());
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotTurret.runTurretWithInput(0);
|
||||
}
|
||||
}, m_robotTurret));
|
||||
|
||||
// //Climber Manual
|
||||
m_robotClimber.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
|
||||
}, m_robotClimber));
|
||||
// m_robotClimber.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
|
||||
|
||||
m_robotBoomBoom.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
|
||||
);
|
||||
// Hood Manual
|
||||
m_robotHood.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotHood.runHood(getOperatorController().getLeftY());
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotHood.runHood(0);
|
||||
}
|
||||
}, m_robotHood));
|
||||
|
||||
// Climber Manual
|
||||
m_robotClimber.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotClimber.setMotors(0.0);
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotClimber.setMotors(-getOperatorController().getRightY());
|
||||
}
|
||||
}, m_robotClimber));
|
||||
|
||||
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
|
||||
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
|
||||
SmartDashboard.putData("Command Schedule", m_commandSchedule);
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
* Use this method to define your button->command mappings. Buttons can be created by instantiating
|
||||
* a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
|
||||
* {@link XboxController}), and then passing it to a
|
||||
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
//! Driver Buttons
|
||||
// ! Driver Buttons
|
||||
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value).whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value).whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
|
||||
// Left Bumper > Shift Down
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
// Left Bumper > Shift Down
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
.whenPressed(new InstantCommand(() -> switchControlMode()))
|
||||
.whenReleased(new InstantCommand(() -> switchControlMode()));
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value).whenPressed(new InstantCommand(() -> switchControlMode())).whenReleased(new InstantCommand(() -> switchControlMode()));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
|
||||
.whenPressed(new InstantCommand(() -> switchDriveMode()))
|
||||
.whenReleased(new InstantCommand(() -> switchDriveMode()));
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> switchDriveMode())).whenReleased(new InstantCommand(() -> switchDriveMode()));
|
||||
|
||||
//! Operator Buttons
|
||||
// ! Operator Buttons
|
||||
|
||||
// Right Bumper > Storage Out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
// Right Bumper > Storage Out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// Left Bumper > Storage In
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
// Left Bumper > Storage In
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// B > Toggle claws
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||
// B > Toggle claws
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||
|
||||
// X > Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
// X > Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value).whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
|
||||
// A > Spit Out Ball
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
|
||||
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
// A > Spit Out Ball
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value).whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)).whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
|
||||
// Y > Full aim command
|
||||
// Y > Full aim command
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
|
||||
|
||||
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood,
|
||||
// m_robotVisionOdometry));
|
||||
|
||||
//! Test Buttons
|
||||
// ! Test Buttons
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target);
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood,
|
||||
// m_robotVisionOdometry, false, false));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
// .whenPressed(new RunCommandForTime(new RunCommand(() ->
|
||||
// m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)),
|
||||
// m_robotTurret), 1.0));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value).whileHeld(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom)); // * aim with turret to target);
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
||||
|
||||
|
||||
|
||||
//! Button Box Buttons
|
||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
|
||||
// ! Button Box Buttons
|
||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood,
|
||||
// climber, and extender)
|
||||
|
||||
// SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
||||
// ));
|
||||
|
||||
|
||||
// SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood),
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender),
|
||||
// new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender),
|
||||
|
||||
// new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender),
|
||||
// new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake,
|
||||
// m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)
|
||||
// ));
|
||||
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
|
||||
// Middle Switch > Climber and Shooter mode switching
|
||||
// .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false),
|
||||
// m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true),
|
||||
// m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0),
|
||||
// m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake,
|
||||
// m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
|
||||
// Middle Switch > Climber and Shooter mode switching
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
||||
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
||||
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
||||
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
||||
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
||||
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
||||
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
||||
|
||||
// // Left Button > Extender In
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
// // Left Button > Extender In
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kX.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
|
||||
// Left Button > Extender Out
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
// Left Button > Extender Out
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (paths) or commands you want to run (in order)
|
||||
* @return array of commands
|
||||
*/
|
||||
public SequentialCommandGroup buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
||||
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
|
||||
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.m, m_robotSwerveDrive));
|
||||
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
// parse input
|
||||
for (int i=0; i<inputs.length; i++) {
|
||||
if (inputs[i] instanceof String) {
|
||||
|
||||
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
||||
PathPlannerState initState = traj.getInitialState();
|
||||
|
||||
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
|
||||
commands.add(new PPSwerveControllerCommand(
|
||||
traj,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive));
|
||||
}
|
||||
|
||||
if (inputs[i] instanceof Command) {
|
||||
commands.add((Command) inputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive));
|
||||
Command[] ret = new Command[commands.size()];
|
||||
ret = commands.toArray(ret);
|
||||
SequentialCommandGroup seqCG = new SequentialCommandGroup(ret);
|
||||
return seqCG;
|
||||
private boolean isLockedOn() {
|
||||
return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn();
|
||||
}
|
||||
|
||||
// ! ways to not coast
|
||||
// // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
||||
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
||||
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
|
||||
// * 3b. try to only set the drive motors to brake if in auto mode.
|
||||
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
|
||||
|
||||
// ? 1.0 input, 1 second: 134 inches
|
||||
// ? 0.75 input, 1 second: 48 inches
|
||||
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
|
||||
|
||||
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
||||
|
||||
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
||||
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
|
||||
// ! ball positions are "unit tested"
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
|
||||
private SequentialCommandGroup shoot(double storageRunTime) {
|
||||
return new SequentialCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelRaceGroup shoot(double storageRunTime, double timeout) {
|
||||
return new SequentialCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
|
||||
)
|
||||
).withTimeout(timeout + storageRunTime);
|
||||
}
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.3, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
Command resetGyro = new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive);
|
||||
Command brakeDrive = new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
|
||||
|
||||
private Command brakeStorage(double storageRunTime) {
|
||||
return new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotIntake), storageRunTime, true);
|
||||
}
|
||||
|
||||
SequentialCommandGroup extendThenAimTurret() {
|
||||
return new SequentialCommandGroup(
|
||||
new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time
|
||||
);
|
||||
}
|
||||
|
||||
ParallelDeadlineGroup idleDrumUntilShootingFirstBall() {
|
||||
return new ParallelDeadlineGroup(
|
||||
extendThenAimTurret(),
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom)
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelDeadlineGroup intakeWithPath1(double intakeRunTime) {
|
||||
return new ParallelDeadlineGroup(
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
buildAuto(3.0, 3.0, "JMove1") // TODO: make faster?
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelDeadlineGroup intakeWithPath2(double intakeRunTime) {
|
||||
return new ParallelDeadlineGroup(
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
buildAuto(3.0, 3.0, "JMove2") // TODO: make faster?
|
||||
);
|
||||
}
|
||||
|
||||
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1(3.0), weirdAutoShootingGroup2);
|
||||
// ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3);
|
||||
|
||||
ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup(
|
||||
intakeWithPath2(2.8),
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true)
|
||||
);
|
||||
|
||||
// ! DRIVE OFF LINE AND SHOOT (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
|
||||
SequentialCommandGroup driveOffLineAndShoot = new SequentialCommandGroup(
|
||||
resetGyro,
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond), // * go backwards five feet
|
||||
brakeDrive,
|
||||
shoot(1.0),
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! ONE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
|
||||
SequentialCommandGroup oneBallAuto = new SequentialCommandGroup(
|
||||
shoot(1.0),
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! TWO BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
|
||||
SequentialCommandGroup twoBallAuto = new SequentialCommandGroup(
|
||||
idleDrumUntilShootingFirstBall(),
|
||||
shoot(1.0, 4.0), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
|
||||
shoot(5.0), // TODO: optimize time
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
|
||||
SequentialCommandGroup threeBallAuto = new SequentialCommandGroup(
|
||||
idleDrumUntilShootingFirstBall(),
|
||||
shoot(0.8), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPathAndTrackTarget,
|
||||
// intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
|
||||
shoot(0.8), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPath2AndIdleShooterAndAimTurret,
|
||||
shoot(4.0), // TODO: optimize time
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// private final CommandGroupBase instantThreeBallSingleShotAutoSequence = CommandGroupBase.sequence(
|
||||
// // Preloaded Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstBallFeed"),
|
||||
// new WaitCommand(3.0).withName("FirstBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstBallStopFeed"),
|
||||
// // Second Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
// buildAuto(3.0, 3.0, "JMove1").withName("JMove1"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("SecondBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("SecondBallFeed"),
|
||||
// new WaitCommand(3.0).withName("SecondBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("SecondBallStopFeed"),
|
||||
// // Third Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
// buildAuto(3.0, 3.0, "JMove2").withName("JMove2"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"),
|
||||
// new WaitCommand(3.0).withName("ThirdBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
// );
|
||||
|
||||
private static final class InstAutoConst {
|
||||
private static final double PATH_MAX_VEL = 3.0;
|
||||
private static final double PATH_MAX_ACCEL = 3.0;
|
||||
private static final double TRACK_TIME_ALLOWANCE = 1.0;
|
||||
private static final double STORAGE_TIME_TWO_BALLS = 1.5;
|
||||
private static final double STORAGE_TIME_ONE_BALL = 0.8;
|
||||
}
|
||||
private final CommandGroupBase instantThreeBallDoubleSingleShotAutoSequence = CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
// Preload and Second Ball
|
||||
buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove1").withName("JMove1"),
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstSecondBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstSecondBallFeed"),
|
||||
new WaitCommand(InstAutoConst.STORAGE_TIME_TWO_BALLS).withName("FirstSecondBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstSecondBallStopFeed"),
|
||||
// Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove2").withName("JMove2"),
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"),
|
||||
new WaitCommand(InstAutoConst.STORAGE_TIME_ONE_BALL).withName("ThirdBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
);
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
@@ -637,27 +341,74 @@ public class RobotContainer {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
private final CommandGroupBase threeBallPlus = CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
// Get Second Ball
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove1").withName("JMove1"),
|
||||
// Shoot Preloaded and Second Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"),
|
||||
// Get Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove2").withName("JMove2"),
|
||||
// Shoot Third Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"),
|
||||
// Stop
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
);
|
||||
|
||||
private ParallelDeadlineGroup makeTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) {
|
||||
return CommandGroupBase.sequence(
|
||||
new TimedWaitUntilCommand(this::isLockedOn, lockOnDuration).withTimeout(lockOnTimeAllowance).withName(name + "LockOn"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage).withName(name + "Feed"),
|
||||
new WaitCommand(storageRunTime).withName(name + "ShootTimer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName(name + "StopFeed")).deadlineWith(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom).withName(name + "Track")
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (paths) or commands you want to run (in order)
|
||||
* @return array of commands
|
||||
*/
|
||||
private SequentialCommandGroup makePathingGroup(Double maxVel, Double maxAccel, String input) {
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
PathPlannerTrajectory pathTrajectory = PathPlanner.loadPath(input, Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY), Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION));
|
||||
PathPlannerState initialState = pathTrajectory.getInitialState();
|
||||
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition), m_robotSwerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules, m_robotSwerveDrive)
|
||||
);
|
||||
}
|
||||
|
||||
public void switchControlMode() {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
currentControlMode = ControlMode.CLIMBER;
|
||||
} else {
|
||||
currentControlMode = ControlMode.SHOOTER;
|
||||
}
|
||||
currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER;
|
||||
}
|
||||
|
||||
public void switchDriveMode() {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
currentDriveMode = DriveMode.OFF;
|
||||
} else {
|
||||
currentDriveMode = DriveMode.ON;
|
||||
}
|
||||
currentDriveMode = currentDriveMode == DriveMode.ON ? DriveMode.OFF : DriveMode.ON;
|
||||
}
|
||||
|
||||
public static DeadbandedXboxController getDriverController() {
|
||||
public DeadbandedXboxController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
public static DeadbandedXboxController getOperatorController() {
|
||||
public DeadbandedXboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
@@ -665,19 +416,6 @@ public class RobotContainer {
|
||||
return m_buttonBox;
|
||||
}
|
||||
|
||||
public static void setSoftLimits(boolean set) {
|
||||
softLimits = set;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
*
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
*
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
@@ -16,7 +15,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
@@ -30,293 +28,255 @@ import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.shuffleboard.SendableCANSparkMax;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit testing and
|
||||
* modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
// #region Swerve Subsystem
|
||||
public final WPI_TalonFX frontLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX frontLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX frontRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX frontRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX backLeftSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX backLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX backRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX backRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder frontLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder frontRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder backLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder backRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public RobotMap() {
|
||||
// configureLEDMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
configureShooterMotorControllers();
|
||||
configureIntakeMotors();
|
||||
configureExtenderMotors();
|
||||
configureSerializerMotors();
|
||||
configureStorageMotors();
|
||||
configureClimberMotors();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
// public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {}
|
||||
|
||||
|
||||
/* Swerve Subsystem */
|
||||
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public SwerveModule frontLeft;
|
||||
public SwerveModule frontRight;
|
||||
public SwerveModule backLeft;
|
||||
public SwerveModule backRight;
|
||||
|
||||
public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
|
||||
// #endregion Swerve Subsystem
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule rightBack;
|
||||
// #region Extender Subsystem
|
||||
public final CANSparkMax extenderMotor = new SendableCANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||
// #endregion Extender Subsystem
|
||||
|
||||
// #region Intake Subsystem
|
||||
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
|
||||
// #endregion Intake Subsystem
|
||||
|
||||
// #region Serializer Subsystem
|
||||
public final CANSparkMax serializerBelt = new SendableCANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
||||
// #endregion Serializer Subsystem
|
||||
|
||||
// #region Storage Subsystem
|
||||
public final CANSparkMax storageMotor = new SendableCANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
|
||||
// #endregion Storage Subsystem
|
||||
|
||||
// #region Shooter System
|
||||
// #region 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);
|
||||
// #endregion Boom Boom Subsystem
|
||||
|
||||
// #region Turret Subsystem
|
||||
public final CANSparkMax shooterTurret = new SendableCANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
// #endregion Turret Subsystem
|
||||
|
||||
// #region Hood Subsystem
|
||||
public final CANSparkMax angleAdjusterMotor = new SendableCANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
// #endregion Hood Subsystem
|
||||
// #endregion Shooter System
|
||||
|
||||
// #region Climber Subsystem
|
||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID);
|
||||
// #endregion Climber Subsystem
|
||||
|
||||
// #region Claws Subsystem
|
||||
public final Servo leftClaw = new Servo(1);
|
||||
public final Servo rightClaw = new Servo(2);
|
||||
// #endregion Claws Subsystem
|
||||
|
||||
// #region LED Subsystem
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
// #endregion LED Subsystem
|
||||
|
||||
public RobotMap() {
|
||||
configureSwerveMotorControllers();
|
||||
configureExtenderMotors();
|
||||
configureIntakeMotors();
|
||||
configureSerializerMotors();
|
||||
configureStorageMotors();
|
||||
configureShooterMotorControllers();
|
||||
configureClimberMotors();
|
||||
registerDevices();
|
||||
}
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
frontLeftSteerMotor.configFactoryDefault();
|
||||
frontLeftDriveMotor.configFactoryDefault();
|
||||
frontRightSteerMotor.configFactoryDefault();
|
||||
frontRightDriveMotor.configFactoryDefault();
|
||||
backLeftSteerMotor.configFactoryDefault();
|
||||
backLeftDriveMotor.configFactoryDefault();
|
||||
backRightSteerMotor.configFactoryDefault();
|
||||
backRightDriveMotor.configFactoryDefault();
|
||||
|
||||
frontLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightDriveMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
frontLeftSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
frontRightSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
backLeftSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
backRightSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
frontLeftDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
frontRightDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
backLeftDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
backRightDriveMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
SendableRegistry.setName(leftFrontSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(leftFrontWheelMotor, "SwerveDrive", "Left Front Wheel Motor");
|
||||
SendableRegistry.setName(rightFrontSteerMotor, "SwerveDrive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(rightFrontWheelMotor, "SwerveDrive", "Right Front Wheel Motor");
|
||||
SendableRegistry.setName(leftBackSteerMotor, "SwerveDrive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(leftBackWheelMotor, "SwerveDrive", "Left Back Wheel Motor");
|
||||
SendableRegistry.setName(rightBackSteerMotor, "SwerveDrive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(rightBackWheelMotor, "SwerveDrive", "Right Back Wheel Motor");
|
||||
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
rightFrontSteerMotor.configFactoryDefault();
|
||||
rightFrontWheelMotor.configFactoryDefault();
|
||||
leftBackSteerMotor.configFactoryDefault();
|
||||
leftBackWheelMotor.configFactoryDefault();
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
NeutralMode mode = NeutralMode.Coast;
|
||||
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftFrontWheelMotor.setNeutralMode(mode);// Coast
|
||||
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheelMotor.setNeutralMode(mode);// Coast
|
||||
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheelMotor.setNeutralMode(mode);// Coast
|
||||
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheelMotor.setNeutralMode(mode);// Coast
|
||||
|
||||
// current limits
|
||||
|
||||
leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
|
||||
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
|
||||
SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
|
||||
SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
|
||||
SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
frontLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
frontRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
frontLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
frontRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
frontLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
frontRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
backRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||
|
||||
frontLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
frontRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
backRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||
|
||||
frontLeft = new SwerveModule(frontLeftDriveMotor, frontLeftSteerMotor, frontLeftEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
frontRight = new SwerveModule(frontRightDriveMotor, frontRightSteerMotor, frontRightEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
backLeft = new SwerveModule(backLeftDriveMotor, backLeftSteerMotor, backLeftEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
backRight = new SwerveModule(backRightDriveMotor, backRightSteerMotor, backRightEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
frontLeftSteerMotor.configRemoteFeedbackFilter(frontLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backLeftSteerMotor.configRemoteFeedbackFilter(backLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
frontRightSteerMotor.configRemoteFeedbackFilter(frontRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
backRightSteerMotor.configRemoteFeedbackFilter(backRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Climb Subsystem */
|
||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
|
||||
void configureExtenderMotors() {
|
||||
extenderMotor.restoreFactoryDefaults();
|
||||
extenderMotor.setInverted(true);
|
||||
extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||
}
|
||||
|
||||
private void configureClimberMotors() {
|
||||
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
||||
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
||||
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
||||
elbow.configFactoryDefault();
|
||||
elbow.setNeutralMode(NeutralMode.Brake);
|
||||
}
|
||||
/* Hooks Subsystem */
|
||||
public final Servo leftClaw = new Servo(1);
|
||||
public final Servo rightClaw = new Servo(2);
|
||||
void configureIntakeMotors() {
|
||||
intakeMotor.configFactoryDefault();
|
||||
intakeMotor.setInverted(false);
|
||||
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
// 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);
|
||||
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
}
|
||||
|
||||
// turret subsystem
|
||||
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
void configureSerializerMotors() {
|
||||
serializerBelt.restoreFactoryDefaults();
|
||||
}
|
||||
|
||||
// hood subsystem
|
||||
public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
void configureStorageMotors() {
|
||||
storageMotor.restoreFactoryDefaults();
|
||||
storageMotor.setIdleMode(IdleMode.kCoast);
|
||||
}
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void configureShooterMotorControllers() {
|
||||
|
||||
// LEFT FALCON
|
||||
SendableRegistry.setName(shooterFalconLeft, "BoomBoom", "Left Motor");
|
||||
void configureShooterMotorControllers() {
|
||||
// Boom Boom Subsystem
|
||||
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
|
||||
SendableRegistry.setName(shooterFalconRight, "BoomBoom", "Right Motor");
|
||||
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);
|
||||
|
||||
shooterFalconRight.configFactoryDefault();
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconRight.setInverted(false);
|
||||
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.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);
|
||||
|
||||
// turret
|
||||
|
||||
// Turret Subsystem
|
||||
shooterTurret.restoreFactoryDefaults();
|
||||
shooterTurret.setIdleMode(IdleMode.kBrake);
|
||||
shooterTurret.setInverted(true);
|
||||
|
||||
// hood subsystem
|
||||
|
||||
// Hood Subsystem
|
||||
angleAdjusterMotor.restoreFactoryDefaults();
|
||||
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
|
||||
angleAdjusterMotor.setInverted(true);
|
||||
}
|
||||
|
||||
/* Serializer Subsystem */
|
||||
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
||||
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
|
||||
void configureClimberMotors() {
|
||||
elbow.configFactoryDefault();
|
||||
elbow.setNeutralMode(NeutralMode.Brake);
|
||||
}
|
||||
|
||||
/* Intake Subsystem */
|
||||
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
|
||||
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||
private void registerDevices() {
|
||||
SendableRegistry.setName(frontLeftSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(frontLeftDriveMotor, "SwerveDrive", "Left Front Drive Motor");
|
||||
SendableRegistry.setName(frontRightSteerMotor, "SwerveDrive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(frontRightDriveMotor, "SwerveDrive", "Right Front Drive Motor");
|
||||
SendableRegistry.setName(backLeftSteerMotor, "SwerveDrive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(backLeftDriveMotor, "SwerveDrive", "Left Back Drive Motor");
|
||||
SendableRegistry.setName(backRightSteerMotor, "SwerveDrive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(backRightDriveMotor, "SwerveDrive", "Right Back Drive Motor");
|
||||
|
||||
void configureIntakeMotors() {
|
||||
SendableRegistry.setName(intakeMotor, "Intake", "Intake Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) extenderMotor, "Intake", "Extender Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) serializerBelt, "Intake", "Serializer Belt");
|
||||
SendableRegistry.setName((SendableCANSparkMax) storageMotor, "Intake", "Storage Motor");
|
||||
|
||||
intakeMotor.configFactoryDefault();
|
||||
intakeMotor.setInverted(false);
|
||||
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||
}
|
||||
SendableRegistry.setName(shooterFalconLeft, "Shooter", "BoomBoom Left Motor");
|
||||
SendableRegistry.setName(shooterFalconRight, "Shooter", "BoomBoom Right Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) shooterTurret, "Shooter", "Turret");
|
||||
SendableRegistry.setName((SendableCANSparkMax) angleAdjusterMotor, "Shooter", "Hood");
|
||||
|
||||
void configureExtenderMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(extenderMotor), "Intake", "Extender Motor");
|
||||
|
||||
extenderMotor.restoreFactoryDefaults();
|
||||
extenderMotor.setInverted(true);
|
||||
extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||
}
|
||||
|
||||
void configureSerializerMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(serializerBelt), "Intake", "Serializer Belt");
|
||||
SendableRegistry.setName(serializerBeam, "Intake", "Serializer Beam");
|
||||
|
||||
serializerBelt.restoreFactoryDefaults();
|
||||
}
|
||||
|
||||
/* Storage Subsystem */
|
||||
public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
void configureStorageMotors() {
|
||||
SendableRegistry.add(new SendableCANSparkMax(storageMotor), "Intake", "Storage Motor");
|
||||
storageMotor.restoreFactoryDefaults();
|
||||
storageMotor.setIdleMode(IdleMode.kCoast);
|
||||
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
||||
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
||||
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,118 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
|
||||
public class AimToCenter extends CommandBase {
|
||||
/** Creates a new AimWithOdometry. */
|
||||
Turret m_turret;
|
||||
VisionOdometry m_visionOdometry;
|
||||
|
||||
Supplier<Pose2d> supplier;
|
||||
Pose2d odo;
|
||||
|
||||
// use odometry to find x and y later
|
||||
double x;
|
||||
double y;
|
||||
double m_targetAngle;
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
|
||||
public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier<Pose2d> supplier) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_turret = turret;
|
||||
m_visionOdometry = visionOdometry;
|
||||
|
||||
this.supplier = supplier;
|
||||
|
||||
addRequirements(m_turret, m_visionOdometry);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
odo = this.supplier.get();
|
||||
// ! Yes I realize this stupid, yes it works I promise, coordinate system is funky
|
||||
x = -odo.getY();
|
||||
y = -odo.getX();
|
||||
|
||||
SmartDashboard.putNumber("trans x", x);
|
||||
SmartDashboard.putNumber("trans y", y);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
|
||||
odo = this.supplier.get(); // * update odometry using really cool supplier -aarav
|
||||
|
||||
m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360;
|
||||
SmartDashboard.putNumber("Target Angle", m_targetAngle);
|
||||
m_turret.runShooterRotatePID(m_targetAngle);
|
||||
|
||||
// Check if limelight is within range (comment out to disable vision odo)
|
||||
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
|
||||
// m_visionOdometry.updateOdometryWithVision();
|
||||
// m_visionOdometry.setLEDs(true);
|
||||
}
|
||||
else{
|
||||
// m_visionOdometry.setLEDs(false);
|
||||
}
|
||||
}
|
||||
|
||||
public static double angleToCenter(double x, double y, double gyro) {
|
||||
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
|
||||
// double angle = Math.toDegrees(Math.atan2(y, -x) - gyro);
|
||||
return (angle - 360);
|
||||
}
|
||||
|
||||
public static double aaravAngleToCenter(double x, double y, double gyro) {
|
||||
|
||||
double actualGyro = -gyro + 90;
|
||||
|
||||
double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro;
|
||||
if (-x > 0) { return (180 + exp); }
|
||||
if (-x < 0) { return (360 + exp); }
|
||||
|
||||
if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated
|
||||
if (x == 0 && y < 0) { return (90 - actualGyro); }
|
||||
|
||||
System.err.println("Invalid case.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if in deadzone.
|
||||
* @param angle Angle to check.
|
||||
* @return True if in deadzone.
|
||||
*/
|
||||
public static boolean isDeadzone(double angle) {
|
||||
if (angle == Double.NaN) {
|
||||
return false;
|
||||
}
|
||||
return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT));
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -1,41 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
|
||||
public class Seek extends SequentialCommandGroup {
|
||||
|
||||
/** Seeks.
|
||||
* Seeking -> Sought
|
||||
* @author Aarav Shah
|
||||
* @blame Aarav Shah (thomas did this)
|
||||
*/
|
||||
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds) {
|
||||
// Add your commands in the addCommands() call, e.g.
|
||||
// addCommands(new FooCommand(), new BarCommand());
|
||||
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
|
||||
}
|
||||
|
||||
/** Seeks.
|
||||
* Seeking -> Sought
|
||||
* @author Aarav Shah
|
||||
* @blame Aarav Shah (thomas did this)
|
||||
*/
|
||||
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, LED leds, double[] fakeOdo) {
|
||||
// Add your commands in the addCommands() call, e.g.
|
||||
// addCommands(new FooCommand(), new BarCommand());
|
||||
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry, leds));
|
||||
}
|
||||
}
|
||||
@@ -1,232 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants;
|
||||
import frc4388.robot.Robot;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.DummySensor;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class Shoot extends CommandBase {
|
||||
|
||||
// subsystems
|
||||
private SwerveDrive swerve;
|
||||
private Turret turret;
|
||||
private BoomBoom drum;
|
||||
private Hood hood;
|
||||
private VisionOdometry visionOdometry;
|
||||
|
||||
private boolean toShoot;
|
||||
|
||||
// given
|
||||
private double odoX, odoY;
|
||||
private double distance;
|
||||
private double gyroAngle;
|
||||
|
||||
// targets
|
||||
private double targetAngle, targetVel, targetHood;
|
||||
|
||||
// pid
|
||||
private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS;
|
||||
private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS;
|
||||
private double error;
|
||||
private double prevError;
|
||||
private double kP, kI, kD;
|
||||
private double proportional, integral, derivative;
|
||||
private double output, normOutput;
|
||||
private double tolerance;
|
||||
|
||||
boolean timerStarted;
|
||||
long startTime;
|
||||
private double timeTolerance;
|
||||
|
||||
private boolean isAimedInTolerance;
|
||||
private int inverted;
|
||||
private double initialSwerveRotation;
|
||||
|
||||
private boolean endsWithLimelight;
|
||||
|
||||
private double[] fakeOdo;
|
||||
|
||||
/**
|
||||
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
|
||||
*
|
||||
* @param swerve Drive Train
|
||||
* @param drum Shooter Drum
|
||||
* @param turret Shooter Turret
|
||||
* @param hood Shooter Hood
|
||||
*
|
||||
* @author Aarav Shah
|
||||
*/
|
||||
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
this.swerve = swerve;
|
||||
this.drum = drum;
|
||||
this.turret = turret;
|
||||
this.hood = hood;
|
||||
this.visionOdometry = visionOdometry;
|
||||
|
||||
this.toShoot = toShoot;
|
||||
this.endsWithLimelight = endsWithLimelight;
|
||||
|
||||
kP = turretGains.kP;
|
||||
kI = turretGains.kI;
|
||||
kD = turretGains.kD;
|
||||
|
||||
proportional = 0;
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
|
||||
tolerance = 10.0;
|
||||
timeTolerance = 500;
|
||||
isAimedInTolerance = false;
|
||||
|
||||
this.inverted = 1;
|
||||
|
||||
this.fakeOdo = null;
|
||||
|
||||
addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry);
|
||||
}
|
||||
|
||||
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo, boolean toShoot, boolean endsWithLime) {
|
||||
this(swerve, drum, turret, hood, visionOdometry, toShoot, endsWithLime);
|
||||
if (fakeOdo.length != 2) {
|
||||
throw new IllegalArgumentException();
|
||||
}
|
||||
this.fakeOdo = fakeOdo;
|
||||
}
|
||||
|
||||
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) {
|
||||
this(swerve, drum, turret, hood, visionOdometry, false, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates error for custom PID.
|
||||
*/
|
||||
public void updateError() {
|
||||
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
|
||||
|
||||
error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
|
||||
|
||||
// if (error > 180) {
|
||||
// error = error - 360;
|
||||
// this.inverted = -1;
|
||||
// } else {
|
||||
// this.inverted = 1;
|
||||
// }
|
||||
if (error > 180) { error = error - 360; }
|
||||
|
||||
isAimedInTolerance = (Math.abs(error) <= tolerance);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
timerStarted = false;
|
||||
startTime = 0;
|
||||
|
||||
this.odoX = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[0]), -this.swerve.getOdometry().getY());
|
||||
this.odoY = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[1]), -this.swerve.getOdometry().getX());
|
||||
|
||||
this.distance = Math.hypot(odoX, odoY);
|
||||
|
||||
this.gyroAngle = this.swerve.getRegGyro().getDegrees();
|
||||
this.initialSwerveRotation = gyroAngle;
|
||||
|
||||
// get targets (shooter tables)
|
||||
this.targetVel = drum.getVelocity(distance);
|
||||
this.targetHood = drum.getHood(distance);
|
||||
|
||||
this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
|
||||
|
||||
// initial error
|
||||
updateError();
|
||||
prevError = error;
|
||||
}
|
||||
|
||||
/**
|
||||
* Run custom PID.
|
||||
*/
|
||||
public void runPID() {
|
||||
prevError = error;
|
||||
updateError();
|
||||
|
||||
this.proportional = error;
|
||||
this.integral = integral + (error * Constants.LOOP_TIME);
|
||||
this.derivative = (error - prevError) / Constants.LOOP_TIME;
|
||||
this.output = kP * proportional + kI * integral + kD * derivative;
|
||||
this.normOutput = (output / 360); // inverted;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
|
||||
runPID();
|
||||
|
||||
SmartDashboard.putNumber("Error", this.error);
|
||||
SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
|
||||
SmartDashboard.putNumber("Normalized Output", this.normOutput);
|
||||
|
||||
this.turret.runTurretWithCustomPID(normOutput);
|
||||
this.swerve.driveWithInput(RobotContainer.getDriverController().getLeft(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative
|
||||
|
||||
if (this.toShoot) {
|
||||
this.hood.runAngleAdjustPID(this.targetHood);
|
||||
this.drum.runDrumShooterVelocityPID(this.targetVel);
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
|
||||
System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase());
|
||||
// ? return to initial swerve rotation
|
||||
// swerve.driveWithInput(0, 0, initialSwerveRotation, true);
|
||||
|
||||
// this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
|
||||
// this.turret.m_boomBoomRotateMotor.set(0.0);
|
||||
|
||||
// ? should stop the turret and the swerve
|
||||
////this.swerve.stopModules();
|
||||
this.turret.runTurretWithInput(0.0);
|
||||
|
||||
if (this.toShoot) {
|
||||
this.hood.runHood(0.0);
|
||||
this.drum.runDrumShooter(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (!endsWithLimelight) {
|
||||
if (isAimedInTolerance && !timerStarted) {
|
||||
timerStarted = true;
|
||||
startTime = System.currentTimeMillis();
|
||||
}
|
||||
SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
|
||||
return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
|
||||
} else {
|
||||
return this.visionOdometry.m_camera.getLatestResult().hasTargets();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,234 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.ShooterCommands;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.stream.Collector;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.Vector2D;
|
||||
import frc4388.utility.desmos.DesmosServer;
|
||||
|
||||
public class TrackTarget extends CommandBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName());
|
||||
/** Creates a new TrackTarget. */
|
||||
SwerveDrive m_swerve;
|
||||
Turret m_turret;
|
||||
VisionOdometry m_visionOdometry;
|
||||
BoomBoom m_boomBoom;
|
||||
Hood m_hood;
|
||||
LED m_leds;
|
||||
|
||||
boolean isAuto;
|
||||
|
||||
static double velocity;
|
||||
static double hoodPosition;
|
||||
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
|
||||
private static boolean targetLocked = false;
|
||||
public static boolean isTargetNotLocked() {
|
||||
return !targetLocked;
|
||||
}
|
||||
private double velocityTolerance = 300.0;
|
||||
private double hoodTolerance = 5.0;
|
||||
|
||||
boolean isExecuted = false;
|
||||
|
||||
// timing
|
||||
boolean isAimed;
|
||||
|
||||
boolean timerStarted;
|
||||
long startTime;
|
||||
private double timeTolerance;
|
||||
|
||||
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds, boolean isAuto) {
|
||||
m_turret = turret;
|
||||
m_boomBoom = boomBoom;
|
||||
m_hood = hood;
|
||||
m_visionOdometry = visionOdometry;
|
||||
m_leds = leds;
|
||||
|
||||
this.isAuto = isAuto;
|
||||
this.timeTolerance = 1000;
|
||||
|
||||
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
|
||||
SmartDashboard.putNumber("Distance Adjust", -35);
|
||||
SmartDashboard.putBoolean("Target Locked", false);
|
||||
}
|
||||
|
||||
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, LED leds) {
|
||||
this(turret, boomBoom, hood, visionOdometry, leds, false);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
timerStarted = false;
|
||||
startTime = 0;
|
||||
|
||||
velocity = 0;
|
||||
hoodPosition = 0;
|
||||
m_visionOdometry.setDriverMode(false);
|
||||
m_visionOdometry.setLEDs(true);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
points = m_visionOdometry.getTargetPoints();
|
||||
if (points.isEmpty()) {
|
||||
LOGGER.severe("Vision Obscured");
|
||||
return;
|
||||
}
|
||||
// points = getFakePoints();
|
||||
//// points = filterPoints(points);
|
||||
|
||||
Point average = VisionOdometry.averagePoint(points);
|
||||
// double position = m_turret.m_boomBoomRotateEncoder.getPosition();
|
||||
|
||||
// if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
|
||||
// Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
|
||||
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
|
||||
// else
|
||||
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
|
||||
// RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
|
||||
// true);
|
||||
|
||||
double regressedDistance = getDistance(average.y);
|
||||
|
||||
// ! offset trig solution
|
||||
double trigDesiredOffset = 10; // * inches
|
||||
double trigAngleOffset = Math.toDegrees(Math.atan(trigDesiredOffset / regressedDistance)); // * degrees
|
||||
double trigPixelOffset = trigAngleOffset * VisionConstants.PIXELS_PER_DEGREE; // * pixels
|
||||
|
||||
// ! offset csv solution
|
||||
double csvAngleOffset = m_boomBoom.getOffset(regressedDistance); // * degrees
|
||||
double csvPixelOffset = csvAngleOffset * VisionConstants.PIXELS_PER_DEGREE; // * pixels
|
||||
|
||||
double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
|
||||
output *= 2.1;
|
||||
|
||||
// m_turret.runTurretWithInput(output - trigPixelOffset);
|
||||
m_turret.runTurretWithInput(output - csvPixelOffset);
|
||||
|
||||
// ! no longer a +30 lol -aarav
|
||||
double distAdj = SmartDashboard.getNumber("Distance Adjust", -35);
|
||||
velocity = m_boomBoom.getVelocity(regressedDistance + distAdj);
|
||||
hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj);
|
||||
|
||||
m_boomBoom.runDrumShooterVelocityPID(velocity);
|
||||
m_hood.runAngleAdjustPID(hoodPosition);
|
||||
|
||||
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
|
||||
double currentHood = this.m_hood.getEncoderPosition();
|
||||
|
||||
targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2);
|
||||
|
||||
|
||||
SmartDashboard.putNumber("Distance", regressedDistance - 35);
|
||||
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
|
||||
SmartDashboard.putNumber("Vel Target Track", velocity);
|
||||
SmartDashboard.putBoolean("Target Locked", targetLocked);
|
||||
|
||||
if (targetLocked){
|
||||
m_leds.setPattern(LEDConstants.SHOOTING_PATTERN);
|
||||
}
|
||||
else{
|
||||
m_leds.setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
}
|
||||
}
|
||||
|
||||
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
|
||||
Point average = VisionOdometry.averagePoint(points);
|
||||
|
||||
HashMap<Point, Double> pointDistances = new HashMap<>();
|
||||
double distanceSum = 0;
|
||||
|
||||
for(Point point : points) {
|
||||
Vector2D difference = new Vector2D(point);
|
||||
difference.subtract(new Vector2D(average));
|
||||
|
||||
double mag = difference.magnitude();
|
||||
distanceSum += mag;
|
||||
|
||||
pointDistances.put(point, mag);
|
||||
}
|
||||
|
||||
final double averageDist = distanceSum / points.size();
|
||||
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 1.3 * averageDist).collect(Collectors.toList());
|
||||
}
|
||||
|
||||
public final ArrayList<Point> getFakePoints() {
|
||||
ArrayList<Point> fakePoints = new ArrayList<>();
|
||||
|
||||
for(int i = 0; i < 10; i++) {
|
||||
Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2));
|
||||
fakePoints.add(p);
|
||||
}
|
||||
|
||||
return fakePoints;
|
||||
}
|
||||
|
||||
public final double getDistance(double averageY) {
|
||||
double y_rot = averageY / VisionConstants.LIME_VIXELS;
|
||||
y_rot *= Math.toRadians(VisionConstants.V_FOV);
|
||||
y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
|
||||
y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
|
||||
|
||||
double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
|
||||
|
||||
double regressedDistance = distanceRegression(distance);
|
||||
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
|
||||
SmartDashboard.putNumber("Distance from Lime 123", distance);
|
||||
SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance);
|
||||
return regressedDistance;
|
||||
}
|
||||
|
||||
public final double distanceRegression(double distance) {
|
||||
return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_visionOdometry.setLEDs(false);
|
||||
SmartDashboard.putBoolean("Target Locked", false);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (this.isAuto) {
|
||||
if (targetLocked && !timerStarted) {
|
||||
timerStarted = true;
|
||||
startTime = System.currentTimeMillis();
|
||||
}
|
||||
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
package frc4388.robot.commands.shooter;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
|
||||
public class TimedWaitUntilCommand extends CommandBase {
|
||||
private final BooleanSupplier m_condition;
|
||||
private final double m_duration;
|
||||
|
||||
protected Timer m_timer = new Timer();
|
||||
|
||||
public TimedWaitUntilCommand(BooleanSupplier condition, double seconds, Subsystem... requirements) {
|
||||
m_condition = requireNonNullParam(condition, "condition", "TimedWaitUntilCommand");
|
||||
m_duration = seconds;
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (m_condition.getAsBoolean())
|
||||
return m_timer.hasElapsed(m_duration);
|
||||
m_timer.reset();
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_timer.stop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
package frc4388.robot.commands.shooter;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
|
||||
// TODO: Try putting swerve drive rotation to be perpendicular to the center point to prevent being rolled while shooting.
|
||||
// FIXME: Regression and distance offset happen separately, and the turret offset uses the regressed distance without the distance offset.
|
||||
public class TrackTarget extends CommandBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName());
|
||||
private static final double TURRET_OFFSET_DEGREES = 10;
|
||||
|
||||
private final VisionOdometry m_visionOdometry;
|
||||
private final Turret m_turret;
|
||||
private final Hood m_hood;
|
||||
private final BoomBoom m_boomBoom;
|
||||
|
||||
public TrackTarget(VisionOdometry visionOdometry, Turret turret, Hood hood, BoomBoom boomBoom) {
|
||||
this.m_visionOdometry = visionOdometry;
|
||||
this.m_turret = turret;
|
||||
this.m_hood = hood;
|
||||
this.m_boomBoom = boomBoom;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_visionOdometry.setDriverMode(false);
|
||||
m_visionOdometry.setLEDs(true);
|
||||
SmartDashboard.putBoolean("TrackTarget/Running", true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
ArrayList<Point> points = m_visionOdometry.getTargetPoints();
|
||||
if (!points.isEmpty()) {
|
||||
Point pointAverage = VisionOdometry.averagePoint(points);
|
||||
|
||||
double distanceTarget = getDistance(pointAverage.y);
|
||||
double turretOffset = Math.toDegrees(Math.atan(TURRET_OFFSET_DEGREES / distanceTarget)) * VisionConstants.PIXELS_PER_DEGREE;
|
||||
distanceTarget -= SmartDashboard.getNumber("TrackTarget/Target Distance Offset", -35);
|
||||
SmartDashboard.putNumber("TrackTarget/Target Distance (Regressed and Offset)", distanceTarget);
|
||||
|
||||
double turretOutput = 2.1 * ((pointAverage.x + 40) - VisionConstants.LIME_HIXELS / 2.0) / VisionConstants.LIME_HIXELS;
|
||||
double hoodTarget = m_boomBoom.getHood(distanceTarget);
|
||||
double velocityTarget = m_boomBoom.getVelocity(distanceTarget);
|
||||
SmartDashboard.putNumber("TrackTarget/Target Hood", hoodTarget);
|
||||
SmartDashboard.putNumber("TrackTarget/Target Velocity", velocityTarget);
|
||||
|
||||
m_turret.runTurretWithInput(turretOutput - turretOffset);
|
||||
m_hood.runAngleAdjustPID(hoodTarget);
|
||||
m_boomBoom.runDrumShooterVelocityPID(velocityTarget);
|
||||
} else {
|
||||
LOGGER.severe("No vision target points.");
|
||||
SmartDashboard.putString("TrackTarget/Target Distance (Regressed and Offset)", "obscured");
|
||||
SmartDashboard.putString("TrackTarget/Target Hood", "obscured");
|
||||
SmartDashboard.putString("TrackTarget/Target Velocity", "obscured");
|
||||
}
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
SmartDashboard.putBoolean("TrackTarget/Running", false);
|
||||
}
|
||||
private static double getDistance(double averageY) {
|
||||
double yRot = averageY / VisionConstants.LIME_VIXELS;
|
||||
yRot *= Math.toRadians(VisionConstants.V_FOV);
|
||||
yRot -= Math.toRadians(VisionConstants.V_FOV) / 2;
|
||||
yRot += Math.toRadians(VisionConstants.LIME_ANGLE);
|
||||
double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(yRot);
|
||||
|
||||
double regressedDistance = regression(distance, 79.6078, 1.01343, -56.6671);
|
||||
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
|
||||
return regressedDistance;
|
||||
}
|
||||
private static double regression(double x, double a, double b, double c) {
|
||||
return a * Math.pow(b, x) + c;
|
||||
}
|
||||
}
|
||||
@@ -13,12 +13,16 @@ import java.util.logging.Logger;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
@@ -52,7 +56,15 @@ public class ShooterTuner extends CommandBase {
|
||||
var tab = Shuffleboard.getTab("Shooter Tuner");
|
||||
if (tab.getComponents().isEmpty()) {
|
||||
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5);
|
||||
manual.add("Manual Shooter Data", m_shotEditor);
|
||||
//XXX:
|
||||
var sbi = new SendableBuilderImpl();
|
||||
sbi.setTable(NetworkTableInstance.getDefault().getTable("ShooterTuner/Manual Shooter Data"));
|
||||
sbi.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d);
|
||||
sbi.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d);
|
||||
sbi.addDoubleProperty("Turret Offset", () -> tableOverrideEntry.turretOffset, d -> tableOverrideEntry.turretOffset = d);
|
||||
sbi.addDoubleProperty("Distance", () -> tableOverrideEntry.distance, d -> tableOverrideEntry.distance = d);
|
||||
sbi.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b);
|
||||
// manual.add("Manual Shooter Data", m_shotEditor);
|
||||
manual.add("Manual Data Appender", m_shotCsvAppender);
|
||||
var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5);
|
||||
csv.add("Shooter Table", m_tableEditor);
|
||||
|
||||
@@ -24,6 +24,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.diffplug.common.base.Errors;
|
||||
import com.diffplug.common.base.StringPrinter;
|
||||
|
||||
import edu.wpi.first.math.system.plant.DCMotor;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
@@ -37,8 +38,7 @@ import frc4388.utility.NumericData;
|
||||
public class BoomBoom extends SubsystemBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
|
||||
public WPI_TalonFX m_shooterFalconLeft;
|
||||
public WPI_TalonFX m_shooterFalconRight;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static BoomBoom m_boomBoom;
|
||||
double speed2;
|
||||
|
||||
@@ -217,4 +217,11 @@ public class BoomBoom extends SubsystemBase {
|
||||
public double getCurrent(){
|
||||
return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent();
|
||||
}
|
||||
|
||||
public boolean isLockedOn() {
|
||||
double currentDrum = Math.min(m_shooterFalconLeft.getSelectedSensorVelocity(), m_shooterFalconRight.getSelectedSensorVelocity());
|
||||
double targetDrum = Math.max(m_shooterFalconLeft.get(), m_shooterFalconRight.get());
|
||||
return Math.abs(currentDrum - targetDrum) > VELOCITY_TOLERANCE;
|
||||
}
|
||||
private static final double VELOCITY_TOLERANCE = 300.0;
|
||||
}
|
||||
@@ -143,4 +143,11 @@ public class Hood extends SubsystemBase {
|
||||
public double getCurrent(){
|
||||
return m_angleAdjusterMotor.getOutputCurrent();
|
||||
}
|
||||
|
||||
public boolean isLockedOn() {
|
||||
double currentHood = getEncoderPosition();
|
||||
double targetHood = m_angleAdjusterMotor.get();
|
||||
return Math.abs(currentHood - targetHood) > HOOD_TOLERANCE;
|
||||
}
|
||||
private static final double HOOD_TOLERANCE = 5.0;
|
||||
}
|
||||
|
||||
@@ -1,22 +1,16 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SerializerConstants;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
|
||||
public class Serializer extends SubsystemBase{
|
||||
private CANSparkMax m_serializerBelt;
|
||||
// private CANSparkMax m_serializerShooterBelt;
|
||||
private DigitalInput m_serializerBeam;
|
||||
private boolean serializerState;
|
||||
|
||||
public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) {
|
||||
public Serializer(CANSparkMax serializerBelt) {
|
||||
m_serializerBelt = serializerBelt;
|
||||
m_serializerBeam = serializerBeam;
|
||||
|
||||
m_serializerBelt.set(0);
|
||||
// m_serializerShooterBelt.set(0);
|
||||
|
||||
}
|
||||
/**
|
||||
@@ -26,23 +20,6 @@ public class Serializer extends SubsystemBase{
|
||||
public void setSerializer(double input){
|
||||
m_serializerBelt.set(input);
|
||||
}
|
||||
/**
|
||||
* Gets The State Of The Beam
|
||||
* @return The State Of The Beam
|
||||
*/
|
||||
public boolean getBeam() {
|
||||
return m_serializerBeam.get();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets The Serializer State With The Beam
|
||||
* @param state Your State Of The Button
|
||||
* @param beambroken The State of the Beam Senser
|
||||
*/
|
||||
public void setSerializerStateWithBeam() {
|
||||
if (m_serializerBeam.get()) setSerializer(0.0);
|
||||
else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the Serializer State
|
||||
@@ -51,6 +28,7 @@ public class Serializer extends SubsystemBase{
|
||||
public boolean getSerializerState() {
|
||||
return serializerState;
|
||||
}
|
||||
|
||||
public double getCurrent(){
|
||||
return m_serializerBelt.getOutputCurrent();
|
||||
}
|
||||
|
||||
@@ -25,10 +25,10 @@ import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
private SwerveModule m_leftFront;
|
||||
private SwerveModule m_leftBack;
|
||||
private SwerveModule m_rightFront;
|
||||
private SwerveModule m_rightBack;
|
||||
private SwerveModule m_frontLeft;
|
||||
private SwerveModule m_frontRight;
|
||||
private SwerveModule m_backLeft;
|
||||
private SwerveModule m_backRight;
|
||||
|
||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
|
||||
@@ -56,16 +56,18 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
public final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
|
||||
WPI_Pigeon2 gyro) {
|
||||
public SwerveDrive(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft, SwerveModule backRight, WPI_Pigeon2 gyro) {
|
||||
//XXX:
|
||||
setName("Swerve Drive");
|
||||
addChild("Gyro", m_gyro);
|
||||
|
||||
m_leftFront = leftFront;
|
||||
m_leftBack = leftBack;
|
||||
m_rightFront = rightFront;
|
||||
m_rightBack = rightBack;
|
||||
m_frontLeft = frontLeft;
|
||||
m_frontRight = frontRight;
|
||||
m_backLeft = backLeft;
|
||||
m_backRight = backRight;
|
||||
m_gyro = gyro;
|
||||
|
||||
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
|
||||
modules = new SwerveModule[] {m_frontLeft, m_frontRight, m_backLeft, m_backRight};
|
||||
|
||||
// m_poseEstimator = new SwerveDrivePoseEstimator(
|
||||
// getRegGyro(),//m_gyro.getRotation2d(),
|
||||
@@ -295,10 +297,10 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
|
||||
public double getCurrent(){
|
||||
return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
|
||||
return m_frontLeft.getCurrent() + m_frontRight.getCurrent() + m_backRight.getCurrent() + m_backLeft.getCurrent();
|
||||
}
|
||||
|
||||
public double getVoltage(){
|
||||
return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
|
||||
return m_frontLeft.getVoltage() + m_frontRight.getVoltage() + m_backRight.getVoltage() + m_backLeft.getVoltage();
|
||||
}
|
||||
}
|
||||
@@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.commands.ShooterCommands.Shoot;
|
||||
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
@@ -253,5 +252,10 @@ public class Turret extends SubsystemBase {
|
||||
public double getCurrent(){
|
||||
return m_boomBoomRotateMotor.getOutputCurrent();
|
||||
}
|
||||
|
||||
public boolean isLockedOn() {
|
||||
double currentTurret = getBoomBoomAngleDegrees();
|
||||
double targetTurret = m_boomBoomRotateMotor.get();
|
||||
return Math.abs(currentTurret - targetTurret) > TURRET_TOLERANCE;
|
||||
}
|
||||
private static final double TURRET_TOLERANCE = 0.2;
|
||||
}
|
||||
@@ -20,6 +20,7 @@ import org.photonvision.targeting.TargetCorner;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@@ -67,6 +68,12 @@ public class VisionOdometry extends SubsystemBase {
|
||||
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
|
||||
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
//XXX: REMOVE
|
||||
if (RobotBase.isSimulation()) {
|
||||
points = new ArrayList<>();
|
||||
points.add(new Point(320, 240));
|
||||
return points;
|
||||
}
|
||||
if(!result.hasTargets())
|
||||
return points;
|
||||
|
||||
@@ -84,7 +91,6 @@ public class VisionOdometry extends SubsystemBase {
|
||||
points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y));
|
||||
}
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user