mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Small refactoring
This commit is contained in:
nathanrsxtn
@@ -0,0 +1,393 @@
|
|||||||
|
{
|
||||||
|
"tabPane": [
|
||||||
|
{
|
||||||
|
"title": "SmartDashboard",
|
||||||
|
"autoPopulate": true,
|
||||||
|
"autoPopulatePrefix": "SmartDashboard/",
|
||||||
|
"widgetPane": {
|
||||||
|
"gridSize": 128.0,
|
||||||
|
"showGrid": true,
|
||||||
|
"hgap": 16.0,
|
||||||
|
"vgap": 16.0,
|
||||||
|
"titleType": 0,
|
||||||
|
"tiles": {}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "LiveWindow",
|
||||||
|
"autoPopulate": true,
|
||||||
|
"autoPopulatePrefix": "LiveWindow/",
|
||||||
|
"widgetPane": {
|
||||||
|
"gridSize": 128.0,
|
||||||
|
"showGrid": true,
|
||||||
|
"hgap": 16.0,
|
||||||
|
"vgap": 16.0,
|
||||||
|
"titleType": 0,
|
||||||
|
"tiles": {}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "Robot",
|
||||||
|
"autoPopulate": false,
|
||||||
|
"autoPopulatePrefix": "",
|
||||||
|
"widgetPane": {
|
||||||
|
"gridSize": 60.0,
|
||||||
|
"showGrid": true,
|
||||||
|
"hgap": 10.0,
|
||||||
|
"vgap": 10.0,
|
||||||
|
"titleType": 1,
|
||||||
|
"tiles": {
|
||||||
|
"0,0": {
|
||||||
|
"size": [
|
||||||
|
6,
|
||||||
|
8
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Match",
|
||||||
|
"Layout/Number of columns": 1,
|
||||||
|
"Layout/Number of rows": 3,
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": {
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Basic FMS Info",
|
||||||
|
"_title": "Basic FMS Info"
|
||||||
|
},
|
||||||
|
"0,1": {
|
||||||
|
"_type": "Field",
|
||||||
|
"_title": "Field",
|
||||||
|
"Game/Game": "A2021_Infinite_Recharge",
|
||||||
|
"Visuals/Robot Icon Size": 50.0,
|
||||||
|
"Visuals/Show Outside Circles": false
|
||||||
|
},
|
||||||
|
"0,2": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Match Time",
|
||||||
|
"Layout/Number of columns": 3,
|
||||||
|
"Layout/Number of rows": 1,
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": {
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Autonomous",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 15.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 4,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
"1,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Teleoperated",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 90.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
"2,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Climb",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 30.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"0,8": {
|
||||||
|
"size": [
|
||||||
|
6,
|
||||||
|
5
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "List Layout",
|
||||||
|
"_title": "Power",
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": [
|
||||||
|
{
|
||||||
|
"_type": "Graph",
|
||||||
|
"_title": "Graph",
|
||||||
|
"Graph/Visible time": 30.0,
|
||||||
|
"Graph/X-axis auto scrolling": true,
|
||||||
|
"Y-axis/Automatic bounds": true,
|
||||||
|
"Y-axis/Upper bound": 1.0,
|
||||||
|
"Y-axis/Lower bound": -1.0,
|
||||||
|
"Y-axis/Unit": "ul"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"_type": "PDP",
|
||||||
|
"_title": "PDP",
|
||||||
|
"Visuals/Show voltage and current values": true
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"6,0": {
|
||||||
|
"size": [
|
||||||
|
3,
|
||||||
|
4
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Robot Preferences",
|
||||||
|
"_title": "Robot Preferences",
|
||||||
|
"Miscellaneous/Show search box": false
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"9,0": {
|
||||||
|
"size": [
|
||||||
|
7,
|
||||||
|
8
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Subsystem Layout",
|
||||||
|
"_title": "Drive",
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": [
|
||||||
|
{
|
||||||
|
"_type": "Swerve Drivebase",
|
||||||
|
"_title": "",
|
||||||
|
"Visuals/Show velocity vectors": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Layout",
|
||||||
|
"Layout/Number of columns": 2,
|
||||||
|
"Layout/Number of rows": 1,
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": {
|
||||||
|
"1,0": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Layout",
|
||||||
|
"Layout/Number of columns": 1,
|
||||||
|
"Layout/Number of rows": 2,
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": {
|
||||||
|
"0,1": {
|
||||||
|
"_type": "Split Button Chooser",
|
||||||
|
"_title": "Speed"
|
||||||
|
},
|
||||||
|
"0,0": {
|
||||||
|
"_type": "3-Axis Accelerometer",
|
||||||
|
"_title": "Accelerometer",
|
||||||
|
"Accelerometer/Range": "k16G",
|
||||||
|
"Visuals/Show value": true,
|
||||||
|
"Visuals/Precision": 2,
|
||||||
|
"Visuals/Show tick marks": true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Gyro",
|
||||||
|
"_title": "Gyro",
|
||||||
|
"Visuals/Major tick spacing": 45.0,
|
||||||
|
"Visuals/Starting angle": 180.0,
|
||||||
|
"Visuals/Show tick mark ring": true,
|
||||||
|
"Visuals/Counter clockwise": false
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"16,0": {
|
||||||
|
"size": [
|
||||||
|
7,
|
||||||
|
8
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Camera Stream",
|
||||||
|
"_title": "Camera",
|
||||||
|
"Crosshair/Show crosshair": false,
|
||||||
|
"Crosshair/Crosshair color": "#FFFFFFFF",
|
||||||
|
"Controls/Show controls": false,
|
||||||
|
"Controls/Rotation": "NONE",
|
||||||
|
"compression": -1.0,
|
||||||
|
"fps": -1,
|
||||||
|
"imageWidth": -1,
|
||||||
|
"imageHeight": -1
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"10,8": {
|
||||||
|
"size": [
|
||||||
|
6,
|
||||||
|
5
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Turret",
|
||||||
|
"Layout/Number of columns": 2,
|
||||||
|
"Layout/Number of rows": 1,
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": {
|
||||||
|
"1,0": {
|
||||||
|
"_type": "Boolean Box",
|
||||||
|
"_title": "Shooter Safety",
|
||||||
|
"Colors/Color when true": "#7CFC00FF",
|
||||||
|
"Colors/Color when false": "#8B0000FF"
|
||||||
|
},
|
||||||
|
"0,0": {
|
||||||
|
"_type": "List Layout",
|
||||||
|
"_title": "Layout",
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": [
|
||||||
|
{
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Speed",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 100.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 0,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Angle",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 100.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 0,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Hood",
|
||||||
|
"Range/Min": 0.0,
|
||||||
|
"Range/Max": 100.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 0,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"6,8": {
|
||||||
|
"size": [
|
||||||
|
4,
|
||||||
|
5
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Cargo",
|
||||||
|
"Layout/Number of columns": 1,
|
||||||
|
"Layout/Number of rows": 2,
|
||||||
|
"Layout/Label position": "HIDDEN",
|
||||||
|
"_children": {
|
||||||
|
"0,1": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Layout",
|
||||||
|
"Layout/Number of columns": 3,
|
||||||
|
"Layout/Number of rows": 1,
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": {
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Intake",
|
||||||
|
"Range/Min": -1.0,
|
||||||
|
"Range/Max": 1.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
"1,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Serializer",
|
||||||
|
"Range/Min": -1.0,
|
||||||
|
"Range/Max": 1.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
"2,0": {
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Storage",
|
||||||
|
"Range/Min": -1.0,
|
||||||
|
"Range/Max": 1.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Grid Layout",
|
||||||
|
"_title": "Layout",
|
||||||
|
"Layout/Number of columns": 1,
|
||||||
|
"Layout/Number of rows": 1,
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": {
|
||||||
|
"0,0": {
|
||||||
|
"_type": "Split Button Chooser",
|
||||||
|
"_title": "Extender"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"6,4": {
|
||||||
|
"size": [
|
||||||
|
3,
|
||||||
|
4
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "List Layout",
|
||||||
|
"_title": "Climber",
|
||||||
|
"Layout/Label position": "BOTTOM",
|
||||||
|
"_children": [
|
||||||
|
{
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Claws",
|
||||||
|
"Range/Min": -1.0,
|
||||||
|
"Range/Max": 1.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"_type": "Number Bar",
|
||||||
|
"_title": "Arm",
|
||||||
|
"Range/Min": -1.0,
|
||||||
|
"Range/Max": 1.0,
|
||||||
|
"Range/Center": 0.0,
|
||||||
|
"Visuals/Num tick marks": 5,
|
||||||
|
"Visuals/Show text": true
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"16,8": {
|
||||||
|
"size": [
|
||||||
|
7,
|
||||||
|
5
|
||||||
|
],
|
||||||
|
"content": {
|
||||||
|
"_type": "Camera Stream",
|
||||||
|
"_title": "Limelight",
|
||||||
|
"Crosshair/Show crosshair": true,
|
||||||
|
"Crosshair/Crosshair color": "#FFFFFFFF",
|
||||||
|
"Controls/Show controls": false,
|
||||||
|
"Controls/Rotation": "NONE",
|
||||||
|
"compression": -1.0,
|
||||||
|
"fps": -1,
|
||||||
|
"imageWidth": -1,
|
||||||
|
"imageHeight": -1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"windowGeometry": {
|
||||||
|
"x": 0.0,
|
||||||
|
"y": 25.0,
|
||||||
|
"width": 1680.0,
|
||||||
|
"height": 1025.0
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -345,4 +345,13 @@ public final class Constants {
|
|||||||
public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV;
|
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 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;
|
package frc4388.robot;
|
||||||
|
|
||||||
import java.io.File;
|
|
||||||
import java.util.logging.Level;
|
import java.util.logging.Level;
|
||||||
import java.util.logging.Logger;
|
import java.util.logging.Logger;
|
||||||
|
|
||||||
import com.diffplug.common.base.Errors;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
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.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.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
||||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
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
|
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||||
* functions corresponding to each mode, as described in the TimedRobot
|
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||||
* documentation. If you change the name of this class or the package after
|
* the package after creating this project, you must also update the build.gradle file in the
|
||||||
* creating this project, you must also update the build.gradle file in the
|
|
||||||
* project.
|
* project.
|
||||||
*/
|
*/
|
||||||
public class Robot extends TimedRobot {
|
public class Robot extends TimedRobot {
|
||||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
|
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
|
||||||
Command m_autonomousCommand;
|
Command m_autonomousCommand;
|
||||||
|
|
||||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
|
||||||
private RobotContainer m_robotContainer;
|
private RobotContainer m_robotContainer;
|
||||||
|
|
||||||
// private double current;
|
|
||||||
|
|
||||||
// private static DesmosServer desmosServer = new DesmosServer(8000);
|
// 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
|
* This function is run when the robot is first started up and should be used for any initialization
|
||||||
* used for any initialization code.
|
* code.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void robotInit() {
|
public void robotInit() {
|
||||||
|
LOGGER.fine("robotInit()");
|
||||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||||
LOGGER.log(Level.WARNING, "Logging Test 3/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.FINE, "Logging Test 6/8");
|
||||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||||
LOGGER.log(Level.FINEST, "Logging Test 8/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()");
|
|
||||||
|
|
||||||
|
// Instantiate our RobotContainer. This will perform all our button bindings, and put our autonomous
|
||||||
|
// chooser on the dashboard.
|
||||||
// LOGGER.fine("Sssssssssh.");
|
|
||||||
// DriverStation.silenceJoystickConnectionWarning(true);
|
|
||||||
// Instantiate our RobotContainer. This will perform all our button bindings,
|
|
||||||
// and put our
|
|
||||||
// autonomous chooser on the dashboard.
|
|
||||||
m_robotContainer = new RobotContainer();
|
m_robotContainer = new RobotContainer();
|
||||||
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
|
|
||||||
SmartDashboard.putData(CommandScheduler.getInstance());
|
|
||||||
|
|
||||||
// desmosServer.start();
|
// desmosServer.start();
|
||||||
|
|
||||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
||||||
ExtenderIntakeGroup.setDirectionToOut();
|
ExtenderIntakeGroup.setDirectionToOut();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This function is called every robot packet, no matter the mode. Use
|
* This function is called every robot packet, no matter the mode. Use this for items like
|
||||||
* this for items like diagnostics that you want ran during disabled,
|
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
|
||||||
* autonomous, teleoperated and test.
|
|
||||||
*
|
*
|
||||||
* <p>
|
* <p>
|
||||||
* This runs after the mode specific periodic functions, but before
|
* This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard
|
||||||
* LiveWindow and SmartDashboard integrated updating.
|
* integrated updating.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void robotPeriodic() {
|
public void robotPeriodic() {
|
||||||
m_robotTime.updateTimes();
|
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled commands,
|
||||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00);
|
// running already-scheduled commands, removing finished or interrupted commands, and running
|
||||||
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.
|
// subsystem periodic() methods. This must be called from the robot's periodic block in order for
|
||||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
|
// anything in the Command-based framework to work.
|
||||||
|
|
||||||
// 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.
|
|
||||||
CommandScheduler.getInstance().run();
|
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.
|
* This function is called once each time the robot enters Disabled mode. You can use it to reset
|
||||||
* You can use it to reset any subsystem information you want to clear when
|
* any subsystem information you want to clear when the robot is disabled.
|
||||||
* the robot is disabled.
|
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void disabledInit() {
|
public void disabledInit() {
|
||||||
LOGGER.fine("disabledInit()");
|
LOGGER.fine("disabledInit()");
|
||||||
m_robotTime.endMatchTime();
|
|
||||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void disabledPeriodic() {
|
public void disabledPeriodic() {}
|
||||||
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This autonomous runs the autonomous command selected by your
|
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
|
||||||
* {@link RobotContainer} class.
|
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void autonomousInit() {
|
public void autonomousInit() {
|
||||||
LOGGER.fine("autonomousInit()");
|
LOGGER.fine("autonomousInit()");
|
||||||
|
|
||||||
Robot.alliance = DriverStation.getAlliance();
|
|
||||||
|
|
||||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||||
|
|
||||||
// schedule the autonomous command (example)
|
// schedule the autonomous command (example)
|
||||||
if (m_autonomousCommand != null) {
|
if (m_autonomousCommand != null) {
|
||||||
m_autonomousCommand.schedule();
|
m_autonomousCommand.schedule();
|
||||||
}
|
}
|
||||||
m_robotTime.startMatchTime();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This function is called periodically during autonomous.
|
* This function is called periodically during autonomous.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void autonomousPeriodic() {}
|
public void autonomousPeriodic() {
|
||||||
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void teleopInit() {
|
public void teleopInit() {
|
||||||
LOGGER.fine("teleopInit()");
|
LOGGER.fine("teleopInit()");
|
||||||
|
|
||||||
Robot.alliance = DriverStation.getAlliance();
|
DriverStation.silenceJoystickConnectionWarning(true);
|
||||||
|
|
||||||
// This makes sure that the autonomous stops running when
|
// This makes sure that the autonomous stops running when teleop starts running.
|
||||||
// teleop starts running. If you want the autonomous to
|
|
||||||
// continue until interrupted by another command, remove
|
|
||||||
// this line or comment it out.
|
|
||||||
if (m_autonomousCommand != null) {
|
if (m_autonomousCommand != null) {
|
||||||
m_autonomousCommand.cancel();
|
m_autonomousCommand.cancel();
|
||||||
}
|
}
|
||||||
m_robotTime.startMatchTime();
|
|
||||||
DriverStation.silenceJoystickConnectionWarning(true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This function is called periodically during operator control.
|
* This function is called periodically during operator control.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void teleopPeriodic() {}
|
public void teleopPeriodic() {
|
||||||
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void testInit() {
|
public void testInit() {
|
||||||
@@ -199,6 +134,5 @@ public class Robot extends TimedRobot {
|
|||||||
* This function is called periodically during test mode.
|
* This function is called periodically during test mode.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void testPeriodic() {
|
public void testPeriodic() {}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -4,7 +4,6 @@
|
|||||||
|
|
||||||
package frc4388.robot;
|
package frc4388.robot;
|
||||||
|
|
||||||
import java.util.ArrayList;
|
|
||||||
import java.util.Objects;
|
import java.util.Objects;
|
||||||
import java.util.logging.Logger;
|
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.Pose2d;
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.wpilibj.GenericHID;
|
import edu.wpi.first.wpilibj.GenericHID;
|
||||||
|
import edu.wpi.first.wpilibj.Preferences;
|
||||||
import edu.wpi.first.wpilibj.XboxController;
|
import edu.wpi.first.wpilibj.XboxController;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
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.ParallelDeadlineGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
|
||||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||||
|
import frc4388.robot.Constants.AutoConstants;
|
||||||
import frc4388.robot.Constants.OIConstants;
|
import frc4388.robot.Constants.OIConstants;
|
||||||
import frc4388.robot.Constants.StorageConstants;
|
import frc4388.robot.Constants.StorageConstants;
|
||||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||||
import frc4388.robot.commands.PathRecorder;
|
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.ExtenderIntakeGroup;
|
||||||
import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender;
|
import frc4388.robot.commands.shooter.TimedWaitUntilCommand;
|
||||||
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
import frc4388.robot.commands.shooter.TrackTarget;
|
||||||
import frc4388.robot.commands.shuffleboard.CommandSchedule;
|
import frc4388.robot.commands.shuffleboard.CommandSchedule;
|
||||||
import frc4388.robot.commands.shuffleboard.ShooterTuner;
|
import frc4388.robot.commands.shuffleboard.ShooterTuner;
|
||||||
import frc4388.robot.subsystems.BoomBoom;
|
import frc4388.robot.subsystems.BoomBoom;
|
||||||
@@ -55,579 +52,286 @@ import frc4388.robot.subsystems.Storage;
|
|||||||
import frc4388.robot.subsystems.SwerveDrive;
|
import frc4388.robot.subsystems.SwerveDrive;
|
||||||
import frc4388.robot.subsystems.Turret;
|
import frc4388.robot.subsystems.Turret;
|
||||||
import frc4388.robot.subsystems.VisionOdometry;
|
import frc4388.robot.subsystems.VisionOdometry;
|
||||||
import frc4388.utility.Vector2D;
|
|
||||||
import frc4388.utility.controller.ButtonBox;
|
import frc4388.utility.controller.ButtonBox;
|
||||||
import frc4388.utility.controller.DeadbandedXboxController;
|
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
|
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
|
||||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
* subsystems, commands, and button mappings) should be declared here.
|
||||||
* commands, and button mappings) should be declared here.
|
|
||||||
*/
|
*/
|
||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
||||||
|
|
||||||
// RobotMap
|
/* Robot Map */
|
||||||
public final RobotMap m_robotMap = new RobotMap();
|
public final RobotMap m_robotMap = new RobotMap();
|
||||||
|
|
||||||
/* Subsystems */
|
/* Subsystems */
|
||||||
public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40);
|
public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40);
|
||||||
public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
|
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 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 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, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt);
|
||||||
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
||||||
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
||||||
|
|
||||||
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
|
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 BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||||
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
||||||
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
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 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 ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
|
||||||
private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false);
|
private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false);
|
||||||
// Controllers
|
|
||||||
private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
/* Controllers */
|
||||||
private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
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);
|
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
||||||
|
|
||||||
public static boolean softLimits = true;
|
private static boolean softLimits = true;
|
||||||
|
|
||||||
// control mode switching
|
// control mode switching
|
||||||
public static enum ControlMode { SHOOTER, CLIMBER };
|
private enum ControlMode {
|
||||||
public static ControlMode currentControlMode = ControlMode.SHOOTER;
|
SHOOTER, CLIMBER
|
||||||
|
}
|
||||||
|
|
||||||
// turret mode switching
|
private ControlMode currentControlMode = ControlMode.SHOOTER;
|
||||||
private enum TurretMode { MANUAL, AUTONOMOUS };
|
|
||||||
private TurretMode currentTurretMode = TurretMode.MANUAL;
|
|
||||||
|
|
||||||
// climber mode switching
|
|
||||||
private enum ClimberMode { MANUAL, AUTONOMOUS };
|
|
||||||
private ClimberMode currentClimberMode = ClimberMode.MANUAL;
|
|
||||||
|
|
||||||
// drive on off mode switching
|
// drive on off mode switching
|
||||||
private enum DriveMode { ON, OFF };
|
private enum DriveMode {
|
||||||
|
ON, OFF
|
||||||
|
}
|
||||||
|
|
||||||
private DriveMode currentDriveMode = DriveMode.ON;
|
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.
|
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||||
*/
|
*/
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
autoChooser.addOption("DriveOffLineAndShoot", driveOffLineAndShoot);
|
autoChooser.addOption("threeBallPlus", threeBallPlus);
|
||||||
autoChooser.addOption("OneBallAuto", oneBallAuto);
|
|
||||||
autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto);
|
|
||||||
autoChooser.addOption("ThreeBallAuto", threeBallAuto);
|
|
||||||
// autoChooser.addOption("instantThreeBallSingleShotAutoSequence", instantThreeBallSingleShotAutoSequence);
|
|
||||||
autoChooser.addOption("instantThreeBallDoubleSingleShotAutoSequence", instantThreeBallDoubleSingleShotAutoSequence);
|
|
||||||
|
|
||||||
SmartDashboard.putData("AutoChooser", autoChooser);
|
SmartDashboard.putData("AutoChooser", autoChooser);
|
||||||
|
|
||||||
|
Preferences.initString("Autonomous", "Three Ball");
|
||||||
|
|
||||||
configureButtonBindings();
|
configureButtonBindings();
|
||||||
/* Default Commands */
|
/* Default Commands */
|
||||||
// Swerve Drive with Input
|
// Swerve Drive with Input
|
||||||
m_robotSwerveDrive.setDefaultCommand(
|
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||||
new RunCommand(() -> {
|
if (currentDriveMode == DriveMode.ON) {
|
||||||
if (currentDriveMode == DriveMode.ON) {
|
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true);
|
||||||
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(),
|
}
|
||||||
getDriverController().getRight(), true);
|
if (currentDriveMode == DriveMode.OFF) {
|
||||||
}
|
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
||||||
if (currentDriveMode == DriveMode.OFF) {
|
}
|
||||||
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
}, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
|
||||||
}
|
|
||||||
}, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
|
||||||
|
|
||||||
// Intake with Triggers
|
// Intake with Triggers
|
||||||
m_robotIntake.setDefaultCommand(
|
m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand"));
|
||||||
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));
|
|
||||||
|
|
||||||
// Serializer Manual
|
// TODO: Comment
|
||||||
m_robotSerializer.setDefaultCommand(
|
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand"));
|
||||||
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
|
|
||||||
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
|
||||||
|
|
||||||
// Turret Manual
|
// Serializer Manual
|
||||||
m_robotTurret.setDefaultCommand(
|
m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand"));
|
||||||
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));
|
|
||||||
|
|
||||||
// Hood Manual
|
// Turret Manual
|
||||||
m_robotHood.setDefaultCommand(
|
m_robotTurret.setDefaultCommand(new RunCommand(() -> {
|
||||||
new RunCommand(() -> {
|
if (currentControlMode == ControlMode.SHOOTER) {
|
||||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getLeftY()); }
|
m_robotTurret.runTurretWithInput(getOperatorController().getLeftX());
|
||||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
|
}
|
||||||
}, m_robotHood));
|
if (currentControlMode == ControlMode.CLIMBER) {
|
||||||
|
m_robotTurret.runTurretWithInput(0);
|
||||||
|
}
|
||||||
|
}, m_robotTurret));
|
||||||
|
|
||||||
// //Climber Manual
|
// Hood Manual
|
||||||
m_robotClimber.setDefaultCommand(
|
m_robotHood.setDefaultCommand(new RunCommand(() -> {
|
||||||
new RunCommand(() -> {
|
if (currentControlMode == ControlMode.SHOOTER) {
|
||||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
|
m_robotHood.runHood(getOperatorController().getLeftY());
|
||||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
|
}
|
||||||
}, m_robotClimber));
|
if (currentControlMode == ControlMode.CLIMBER) {
|
||||||
// m_robotClimber.setDefaultCommand(
|
m_robotHood.runHood(0);
|
||||||
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
|
}
|
||||||
|
}, m_robotHood));
|
||||||
m_robotBoomBoom.setDefaultCommand(
|
|
||||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
// 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("Shooter Tuner", m_shooterTuner);
|
||||||
SmartDashboard.putData("Command Schedule", m_commandSchedule);
|
SmartDashboard.putData("Command Schedule", m_commandSchedule);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Use this method to define your button->command mappings. Buttons can be
|
* Use this method to define your button->command mappings. Buttons can be created by instantiating
|
||||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
* a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
|
||||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
* {@link XboxController}), and then passing it to a
|
||||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||||
*/
|
*/
|
||||||
private void configureButtonBindings() {
|
private void configureButtonBindings() {
|
||||||
//! Driver Buttons
|
// ! Driver Buttons
|
||||||
|
|
||||||
// Start > Calibrate Odometry
|
// Start > Calibrate Odometry
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kBack.value).whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
|
||||||
|
|
||||||
// Start > Calibrate Odometry
|
// Start > Calibrate Odometry
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kStart.value).whenPressed(m_robotSwerveDrive::resetGyro);
|
||||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
|
||||||
|
|
||||||
// Left Bumper > Shift Down
|
// Left Bumper > Shift Down
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
|
||||||
|
|
||||||
// Right Bumper > Shift Up
|
// Right Bumper > Shift Up
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
|
||||||
|
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kA.value).whenPressed(new InstantCommand(() -> switchControlMode())).whenReleased(new InstantCommand(() -> switchControlMode()));
|
||||||
.whenPressed(new InstantCommand(() -> switchControlMode()))
|
|
||||||
.whenReleased(new InstantCommand(() -> switchControlMode()));
|
|
||||||
|
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
|
new JoystickButton(getDriverController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> switchDriveMode())).whenReleased(new InstantCommand(() -> switchDriveMode()));
|
||||||
.whenPressed(new InstantCommand(() -> switchDriveMode()))
|
|
||||||
.whenReleased(new InstantCommand(() -> switchDriveMode()));
|
|
||||||
|
|
||||||
//! Operator Buttons
|
// ! Operator Buttons
|
||||||
|
|
||||||
// Right Bumper > Storage Out
|
// Right Bumper > Storage Out
|
||||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
|
||||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
|
||||||
|
|
||||||
// Left Bumper > Storage In
|
// Left Bumper > Storage In
|
||||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
|
||||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
|
||||||
|
|
||||||
// B > Toggle claws
|
// B > Toggle claws
|
||||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
new JoystickButton(getOperatorController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||||
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
|
||||||
|
|
||||||
// X > Toggles extender in and out
|
// X > Toggles extender in and out
|
||||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
new JoystickButton(getOperatorController(), XboxController.Button.kX.value).whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||||
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
|
||||||
|
|
||||||
// A > Spit Out Ball
|
// A > Spit Out Ball
|
||||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
new JoystickButton(getOperatorController(), XboxController.Button.kA.value).whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)).whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||||
.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)
|
// 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)
|
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
// .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);
|
|
||||||
|
|
||||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
// 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)
|
// 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)
|
// 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)
|
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
// .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,
|
||||||
//! Button Box Buttons
|
// climber, and extender)
|
||||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
|
|
||||||
|
|
||||||
// SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup(
|
// SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup(
|
||||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
||||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
||||||
|
|
||||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
||||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
||||||
|
|
||||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
||||||
// ));
|
// ));
|
||||||
|
|
||||||
// SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup(
|
// SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup(
|
||||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
||||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, 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_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_robotExtender.setExtenderSoftLimits(true), m_robotExtender),
|
||||||
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
|
||||||
// new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender),
|
// new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret),
|
||||||
// new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender),
|
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
||||||
// new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)
|
// 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)
|
// new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
||||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, 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))
|
|
||||||
|
|
||||||
// .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))
|
// .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false),
|
||||||
// .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
// m_robotExtender))
|
||||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
|
||||||
// .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
// .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||||
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
// .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||||
|
|
||||||
// Middle Switch > Climber and Shooter mode switching
|
// .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)
|
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
||||||
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||||
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||||
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||||
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
||||||
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||||
|
|
||||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||||
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
||||||
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
||||||
|
|
||||||
// // Left Button > Extender In
|
// // Left Button > Extender In
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
|
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));
|
||||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
|
||||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
|
||||||
|
|
||||||
// Left Button > Extender Out
|
// Left Button > Extender Out
|
||||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
|
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));
|
||||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
|
||||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
private boolean isLockedOn() {
|
||||||
* Generate autonomous
|
return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn();
|
||||||
* @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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ! 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.
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||||
*
|
*
|
||||||
@@ -637,27 +341,74 @@ public class RobotContainer {
|
|||||||
return autoChooser.getSelected();
|
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() {
|
public void switchControlMode() {
|
||||||
if (currentControlMode == ControlMode.SHOOTER) {
|
currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER;
|
||||||
currentControlMode = ControlMode.CLIMBER;
|
|
||||||
} else {
|
|
||||||
currentControlMode = ControlMode.SHOOTER;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void switchDriveMode() {
|
public void switchDriveMode() {
|
||||||
if (currentDriveMode == DriveMode.ON) {
|
currentDriveMode = currentDriveMode == DriveMode.ON ? DriveMode.OFF : DriveMode.ON;
|
||||||
currentDriveMode = DriveMode.OFF;
|
|
||||||
} else {
|
|
||||||
currentDriveMode = DriveMode.ON;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public static DeadbandedXboxController getDriverController() {
|
public DeadbandedXboxController getDriverController() {
|
||||||
return m_driverXbox;
|
return m_driverXbox;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static DeadbandedXboxController getOperatorController() {
|
public DeadbandedXboxController getOperatorController() {
|
||||||
return m_operatorXbox;
|
return m_operatorXbox;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -665,19 +416,6 @@ public class RobotContainer {
|
|||||||
return m_buttonBox;
|
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.
|
* Set odometry to given pose.
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -4,7 +4,6 @@
|
|||||||
|
|
||||||
package frc4388.robot;
|
package frc4388.robot;
|
||||||
|
|
||||||
|
|
||||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
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.util.sendable.SendableRegistry;
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
import edu.wpi.first.wpilibj.DigitalInput;
|
||||||
import edu.wpi.first.wpilibj.PWM;
|
|
||||||
import edu.wpi.first.wpilibj.Servo;
|
import edu.wpi.first.wpilibj.Servo;
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||||
import frc4388.robot.Constants.ClimberConstants;
|
import frc4388.robot.Constants.ClimberConstants;
|
||||||
@@ -30,293 +28,255 @@ import frc4388.robot.subsystems.SwerveModule;
|
|||||||
import frc4388.utility.shuffleboard.SendableCANSparkMax;
|
import frc4388.utility.shuffleboard.SendableCANSparkMax;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
* Defines and holds all I/O objects on the Roborio. This is useful for unit testing and
|
||||||
* testing and modularization.
|
* modularization.
|
||||||
*/
|
*/
|
||||||
public class RobotMap {
|
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() {
|
public SwerveModule frontLeft;
|
||||||
// configureLEDMotorControllers();
|
public SwerveModule frontRight;
|
||||||
configureSwerveMotorControllers();
|
public SwerveModule backLeft;
|
||||||
configureShooterMotorControllers();
|
public SwerveModule backRight;
|
||||||
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 final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
|
public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
|
||||||
|
// #endregion Swerve Subsystem
|
||||||
|
|
||||||
public SwerveModule leftFront;
|
// #region Extender Subsystem
|
||||||
public SwerveModule leftBack;
|
public final CANSparkMax extenderMotor = new SendableCANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
||||||
public SwerveModule rightFront;
|
// #endregion Extender Subsystem
|
||||||
public SwerveModule rightBack;
|
|
||||||
|
// #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() {
|
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
|
// current limits
|
||||||
|
frontLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
frontRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
backLeftSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
backRightSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
|
|
||||||
|
frontLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
frontRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
backLeftDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
backRightDriveMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
|
|
||||||
|
frontLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
frontRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
backLeftSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
backRightSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
||||||
rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
|
|
||||||
|
frontLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
frontRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
backLeftDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
backRightDriveMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
||||||
rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
|
|
||||||
|
frontLeft = new SwerveModule(frontLeftDriveMotor, frontLeftSteerMotor, frontLeftEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||||
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
|
frontRight = new SwerveModule(frontRightDriveMotor, frontRightSteerMotor, frontRightEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||||
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
backLeft = new SwerveModule(backLeftDriveMotor, backLeftSteerMotor, backLeftEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||||
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
|
backRight = new SwerveModule(backRightDriveMotor, backRightSteerMotor, backRightEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||||
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);
|
|
||||||
|
|
||||||
// config cancoder as remote encoder for swerve steer motors
|
// config cancoder as remote encoder for swerve steer motors
|
||||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
frontLeftSteerMotor.configRemoteFeedbackFilter(frontLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
backLeftSteerMotor.configRemoteFeedbackFilter(backLeftEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
frontRightSteerMotor.configRemoteFeedbackFilter(frontRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||||
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
|
backRightSteerMotor.configRemoteFeedbackFilter(backRightEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Climb Subsystem */
|
void configureExtenderMotors() {
|
||||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
|
extenderMotor.restoreFactoryDefaults();
|
||||||
|
extenderMotor.setInverted(true);
|
||||||
|
extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||||
|
}
|
||||||
|
|
||||||
private void configureClimberMotors() {
|
void configureIntakeMotors() {
|
||||||
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
intakeMotor.configFactoryDefault();
|
||||||
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
intakeMotor.setInverted(false);
|
||||||
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
||||||
elbow.configFactoryDefault();
|
|
||||||
elbow.setNeutralMode(NeutralMode.Brake);
|
|
||||||
}
|
|
||||||
/* Hooks Subsystem */
|
|
||||||
public final Servo leftClaw = new Servo(1);
|
|
||||||
public final Servo rightClaw = new Servo(2);
|
|
||||||
|
|
||||||
// Shooter Config
|
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||||
/* Boom Boom Subsystem */
|
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
||||||
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
}
|
||||||
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
|
||||||
|
|
||||||
// turret subsystem
|
void configureSerializerMotors() {
|
||||||
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
serializerBelt.restoreFactoryDefaults();
|
||||||
|
}
|
||||||
|
|
||||||
// hood subsystem
|
void configureStorageMotors() {
|
||||||
public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
storageMotor.restoreFactoryDefaults();
|
||||||
|
storageMotor.setIdleMode(IdleMode.kCoast);
|
||||||
|
}
|
||||||
|
|
||||||
// Create motor CANSparkMax
|
void configureShooterMotorControllers() {
|
||||||
void configureShooterMotorControllers() {
|
// Boom Boom Subsystem
|
||||||
|
|
||||||
// LEFT FALCON
|
|
||||||
SendableRegistry.setName(shooterFalconLeft, "BoomBoom", "Left Motor");
|
|
||||||
shooterFalconLeft.configFactoryDefault();
|
shooterFalconLeft.configFactoryDefault();
|
||||||
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||||
shooterFalconLeft.setInverted(true);
|
shooterFalconLeft.setInverted(true);
|
||||||
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
|
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, 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");
|
|
||||||
shooterFalconRight.configFactoryDefault();
|
shooterFalconRight.configFactoryDefault();
|
||||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||||
shooterFalconRight.setInverted(false);
|
shooterFalconRight.setInverted(false);
|
||||||
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
// m_shooterFalconRight.configPeakOutputForward(0,
|
// shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); //(comment it in if necessary)
|
||||||
// ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
|
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
|
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
|
||||||
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
|
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
|
||||||
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
|
|
||||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
|
||||||
|
|
||||||
shooterFalconRight.follow(shooterFalconLeft);
|
shooterFalconRight.follow(shooterFalconLeft);
|
||||||
|
|
||||||
// turret
|
// Turret Subsystem
|
||||||
shooterTurret.restoreFactoryDefaults();
|
shooterTurret.restoreFactoryDefaults();
|
||||||
shooterTurret.setIdleMode(IdleMode.kBrake);
|
shooterTurret.setIdleMode(IdleMode.kBrake);
|
||||||
shooterTurret.setInverted(true);
|
shooterTurret.setInverted(true);
|
||||||
|
|
||||||
// hood subsystem
|
// Hood Subsystem
|
||||||
angleAdjusterMotor.restoreFactoryDefaults();
|
angleAdjusterMotor.restoreFactoryDefaults();
|
||||||
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
|
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
|
||||||
angleAdjusterMotor.setInverted(true);
|
angleAdjusterMotor.setInverted(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Serializer Subsystem */
|
void configureClimberMotors() {
|
||||||
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
|
elbow.configFactoryDefault();
|
||||||
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
|
elbow.setNeutralMode(NeutralMode.Brake);
|
||||||
|
}
|
||||||
|
|
||||||
/* Intake Subsystem */
|
private void registerDevices() {
|
||||||
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
|
SendableRegistry.setName(frontLeftSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||||
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
|
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(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();
|
SendableRegistry.setName(shooterFalconLeft, "Shooter", "BoomBoom Left Motor");
|
||||||
intakeMotor.setInverted(false);
|
SendableRegistry.setName(shooterFalconRight, "Shooter", "BoomBoom Right Motor");
|
||||||
intakeMotor.setNeutralMode(NeutralMode.Coast);
|
SendableRegistry.setName((SendableCANSparkMax) shooterTurret, "Shooter", "Turret");
|
||||||
|
SendableRegistry.setName((SendableCANSparkMax) angleAdjusterMotor, "Shooter", "Hood");
|
||||||
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
|
|
||||||
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
|
|
||||||
}
|
|
||||||
|
|
||||||
void configureExtenderMotors() {
|
SendableRegistry.setName(elbow, "Climber", "Elbow");
|
||||||
SendableRegistry.add(new SendableCANSparkMax(extenderMotor), "Intake", "Extender Motor");
|
SendableRegistry.setName(leftClaw, "Climber", "Left Claw");
|
||||||
|
SendableRegistry.setName(rightClaw, "Climber", "Right Claw");
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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 com.diffplug.common.base.Errors;
|
||||||
|
|
||||||
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||||
import edu.wpi.first.util.sendable.Sendable;
|
import edu.wpi.first.util.sendable.Sendable;
|
||||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||||
import edu.wpi.first.wpilibj.Filesystem;
|
import edu.wpi.first.wpilibj.Filesystem;
|
||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
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.shuffleboard.Shuffleboard;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||||
@@ -52,7 +56,15 @@ public class ShooterTuner extends CommandBase {
|
|||||||
var tab = Shuffleboard.getTab("Shooter Tuner");
|
var tab = Shuffleboard.getTab("Shooter Tuner");
|
||||||
if (tab.getComponents().isEmpty()) {
|
if (tab.getComponents().isEmpty()) {
|
||||||
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5);
|
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);
|
manual.add("Manual Data Appender", m_shotCsvAppender);
|
||||||
var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5);
|
var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5);
|
||||||
csv.add("Shooter Table", m_tableEditor);
|
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.Errors;
|
||||||
import com.diffplug.common.base.StringPrinter;
|
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.util.sendable.Sendable;
|
||||||
import edu.wpi.first.wpilibj.Filesystem;
|
import edu.wpi.first.wpilibj.Filesystem;
|
||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
@@ -37,8 +38,7 @@ import frc4388.utility.NumericData;
|
|||||||
public class BoomBoom extends SubsystemBase {
|
public class BoomBoom extends SubsystemBase {
|
||||||
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
|
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
|
||||||
public WPI_TalonFX m_shooterFalconLeft;
|
public WPI_TalonFX m_shooterFalconLeft;
|
||||||
public WPI_TalonFX m_shooterFalconRight;
|
public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
|
||||||
public static BoomBoom m_boomBoom;
|
public static BoomBoom m_boomBoom;
|
||||||
double speed2;
|
double speed2;
|
||||||
|
|
||||||
@@ -217,4 +217,11 @@ public class BoomBoom extends SubsystemBase {
|
|||||||
public double getCurrent(){
|
public double getCurrent(){
|
||||||
return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent();
|
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(){
|
public double getCurrent(){
|
||||||
return m_angleAdjusterMotor.getOutputCurrent();
|
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;
|
package frc4388.robot.subsystems;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
import frc4388.robot.Constants.SerializerConstants;
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import com.revrobotics.CANSparkMax;
|
import com.revrobotics.CANSparkMax;
|
||||||
|
|
||||||
public class Serializer extends SubsystemBase{
|
public class Serializer extends SubsystemBase{
|
||||||
private CANSparkMax m_serializerBelt;
|
private CANSparkMax m_serializerBelt;
|
||||||
// private CANSparkMax m_serializerShooterBelt;
|
|
||||||
private DigitalInput m_serializerBeam;
|
|
||||||
private boolean serializerState;
|
private boolean serializerState;
|
||||||
|
|
||||||
public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) {
|
public Serializer(CANSparkMax serializerBelt) {
|
||||||
m_serializerBelt = serializerBelt;
|
m_serializerBelt = serializerBelt;
|
||||||
m_serializerBeam = serializerBeam;
|
|
||||||
|
|
||||||
m_serializerBelt.set(0);
|
m_serializerBelt.set(0);
|
||||||
// m_serializerShooterBelt.set(0);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
@@ -26,23 +20,6 @@ public class Serializer extends SubsystemBase{
|
|||||||
public void setSerializer(double input){
|
public void setSerializer(double input){
|
||||||
m_serializerBelt.set(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
|
* Gets the Serializer State
|
||||||
@@ -51,6 +28,7 @@ public class Serializer extends SubsystemBase{
|
|||||||
public boolean getSerializerState() {
|
public boolean getSerializerState() {
|
||||||
return serializerState;
|
return serializerState;
|
||||||
}
|
}
|
||||||
|
|
||||||
public double getCurrent(){
|
public double getCurrent(){
|
||||||
return m_serializerBelt.getOutputCurrent();
|
return m_serializerBelt.getOutputCurrent();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -25,10 +25,10 @@ import frc4388.utility.Gains;
|
|||||||
|
|
||||||
public class SwerveDrive extends SubsystemBase {
|
public class SwerveDrive extends SubsystemBase {
|
||||||
|
|
||||||
private SwerveModule m_leftFront;
|
private SwerveModule m_frontLeft;
|
||||||
private SwerveModule m_leftBack;
|
private SwerveModule m_frontRight;
|
||||||
private SwerveModule m_rightFront;
|
private SwerveModule m_backLeft;
|
||||||
private SwerveModule m_rightBack;
|
private SwerveModule m_backRight;
|
||||||
|
|
||||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||||
double halfHeight = SwerveDriveConstants.HEIGHT / 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 final Field2d m_field = new Field2d();
|
||||||
|
|
||||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
|
public SwerveDrive(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft, SwerveModule backRight, WPI_Pigeon2 gyro) {
|
||||||
WPI_Pigeon2 gyro) {
|
//XXX:
|
||||||
|
setName("Swerve Drive");
|
||||||
|
addChild("Gyro", m_gyro);
|
||||||
|
|
||||||
m_leftFront = leftFront;
|
m_frontLeft = frontLeft;
|
||||||
m_leftBack = leftBack;
|
m_frontRight = frontRight;
|
||||||
m_rightFront = rightFront;
|
m_backLeft = backLeft;
|
||||||
m_rightBack = rightBack;
|
m_backRight = backRight;
|
||||||
m_gyro = gyro;
|
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(
|
// m_poseEstimator = new SwerveDrivePoseEstimator(
|
||||||
// getRegGyro(),//m_gyro.getRotation2d(),
|
// getRegGyro(),//m_gyro.getRotation2d(),
|
||||||
@@ -295,10 +297,10 @@ public class SwerveDrive extends SubsystemBase {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public double getCurrent(){
|
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(){
|
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 edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
import frc4388.robot.Constants.ShooterConstants;
|
import frc4388.robot.Constants.ShooterConstants;
|
||||||
import frc4388.robot.commands.ShooterCommands.Shoot;
|
|
||||||
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
|
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
|
||||||
import frc4388.utility.Gains;
|
import frc4388.utility.Gains;
|
||||||
|
|
||||||
@@ -253,5 +252,10 @@ public class Turret extends SubsystemBase {
|
|||||||
public double getCurrent(){
|
public double getCurrent(){
|
||||||
return m_boomBoomRotateMotor.getOutputCurrent();
|
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.Pose2d;
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
import edu.wpi.first.wpilibj.Timer;
|
import edu.wpi.first.wpilibj.Timer;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
@@ -67,6 +68,12 @@ public class VisionOdometry extends SubsystemBase {
|
|||||||
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
|
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
|
||||||
|
|
||||||
ArrayList<Point> points = new ArrayList<>();
|
ArrayList<Point> points = new ArrayList<>();
|
||||||
|
//XXX: REMOVE
|
||||||
|
if (RobotBase.isSimulation()) {
|
||||||
|
points = new ArrayList<>();
|
||||||
|
points.add(new Point(320, 240));
|
||||||
|
return points;
|
||||||
|
}
|
||||||
if(!result.hasTargets())
|
if(!result.hasTargets())
|
||||||
return points;
|
return points;
|
||||||
|
|
||||||
@@ -84,7 +91,6 @@ public class VisionOdometry extends SubsystemBase {
|
|||||||
points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y));
|
points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return points;
|
return points;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -8,7 +8,6 @@ import java.io.OutputStream;
|
|||||||
import java.io.PrintStream;
|
import java.io.PrintStream;
|
||||||
import java.io.PrintWriter;
|
import java.io.PrintWriter;
|
||||||
import java.io.StringWriter;
|
import java.io.StringWriter;
|
||||||
import java.nio.charset.StandardCharsets;
|
|
||||||
import java.time.ZoneId;
|
import java.time.ZoneId;
|
||||||
import java.time.ZonedDateTime;
|
import java.time.ZonedDateTime;
|
||||||
import java.util.Map;
|
import java.util.Map;
|
||||||
@@ -27,12 +26,11 @@ import com.diffplug.common.base.Errors;
|
|||||||
|
|
||||||
import org.fusesource.jansi.Ansi.Attribute;
|
import org.fusesource.jansi.Ansi.Attribute;
|
||||||
import org.fusesource.jansi.Ansi.Color;
|
import org.fusesource.jansi.Ansi.Color;
|
||||||
|
|
||||||
import edu.wpi.first.hal.HAL;
|
|
||||||
|
|
||||||
import org.fusesource.jansi.AnsiConsole;
|
import org.fusesource.jansi.AnsiConsole;
|
||||||
import org.fusesource.jansi.AnsiPrintStream;
|
import org.fusesource.jansi.AnsiPrintStream;
|
||||||
|
|
||||||
|
import edu.wpi.first.hal.HAL;
|
||||||
|
|
||||||
public class AnsiLogging {
|
public class AnsiLogging {
|
||||||
public static final Level LEVEL = Level.ALL;
|
public static final Level LEVEL = Level.ALL;
|
||||||
private static final AnsiPrintStream ANSI_CONSOLE_STREAM = AnsiConsole.err();
|
private static final AnsiPrintStream ANSI_CONSOLE_STREAM = AnsiConsole.err();
|
||||||
@@ -62,6 +60,7 @@ public class AnsiLogging {
|
|||||||
DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage));
|
DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage));
|
||||||
// Store the handler for HAL to use when sending errors to DriverStation.
|
// Store the handler for HAL to use when sending errors to DriverStation.
|
||||||
halLoggerHandler = new LoggingAnsiConsoleHandler(new HalOutputStream());
|
halLoggerHandler = new LoggingAnsiConsoleHandler(new HalOutputStream());
|
||||||
|
// Logger.getLogger(name)
|
||||||
} catch (IOException exception) {
|
} catch (IOException exception) {
|
||||||
exception.printStackTrace(AnsiConsole.sysErr());
|
exception.printStackTrace(AnsiConsole.sysErr());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,76 +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.utility;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* <p>Keeps track of Robot times like time passed, delta time, etc
|
|
||||||
* <p>All times are in milliseconds
|
|
||||||
*/
|
|
||||||
public class RobotTime {
|
|
||||||
private long m_currTime = System.currentTimeMillis();
|
|
||||||
public long m_deltaTime = 0;
|
|
||||||
|
|
||||||
private long m_startRobotTime = m_currTime;
|
|
||||||
public long m_robotTime = 0;
|
|
||||||
public long m_lastRobotTime = 0;
|
|
||||||
|
|
||||||
private long m_startMatchTime = 0;
|
|
||||||
public long m_matchTime = 0;
|
|
||||||
public long m_lastMatchTime = 0;
|
|
||||||
|
|
||||||
public long m_frameNumber = 0;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Private constructor prevents other classes from instantiating
|
|
||||||
*/
|
|
||||||
private RobotTime(){}
|
|
||||||
|
|
||||||
private static RobotTime instance = null;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Gets the instance of Robot Time. If there is no instance running one will be created.
|
|
||||||
* @return instance of Robot Time
|
|
||||||
*/
|
|
||||||
public static RobotTime getInstance() {
|
|
||||||
if (instance == null) {
|
|
||||||
instance = new RobotTime();
|
|
||||||
}
|
|
||||||
return instance;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Call this once per periodic loop.
|
|
||||||
*/
|
|
||||||
public void updateTimes() {
|
|
||||||
m_lastRobotTime = m_robotTime;
|
|
||||||
m_lastMatchTime = m_matchTime;
|
|
||||||
|
|
||||||
m_currTime = System.currentTimeMillis();
|
|
||||||
m_robotTime = m_currTime - m_startRobotTime;
|
|
||||||
m_deltaTime = m_robotTime - m_lastRobotTime;
|
|
||||||
m_frameNumber++;
|
|
||||||
|
|
||||||
if (m_startMatchTime != 0) {
|
|
||||||
m_matchTime = m_currTime - m_startMatchTime;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Call this in both the auto and periodic inits
|
|
||||||
*/
|
|
||||||
public void startMatchTime() {
|
|
||||||
if (m_startMatchTime == 0) {
|
|
||||||
m_startMatchTime = m_currTime;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Call this in disabled init
|
|
||||||
*/
|
|
||||||
public void endMatchTime() {
|
|
||||||
m_startMatchTime = 0;
|
|
||||||
m_matchTime = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -5,19 +5,17 @@ import com.revrobotics.CANSparkMax;
|
|||||||
import edu.wpi.first.util.sendable.Sendable;
|
import edu.wpi.first.util.sendable.Sendable;
|
||||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||||
|
|
||||||
public class SendableCANSparkMax implements Sendable {
|
public class SendableCANSparkMax extends CANSparkMax implements Sendable {
|
||||||
private final CANSparkMax m_canSparkMax;
|
public SendableCANSparkMax(int deviceId, MotorType type) {
|
||||||
|
super(deviceId, type);
|
||||||
public SendableCANSparkMax(CANSparkMax canSparkMax) {
|
|
||||||
m_canSparkMax = canSparkMax;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Motor Controller");
|
builder.setSmartDashboardType("Motor Controller");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(m_canSparkMax::stopMotor);
|
builder.setSafeState(this::stopMotor);
|
||||||
builder.addDoubleProperty("Value", m_canSparkMax::get, m_canSparkMax::set);
|
builder.addDoubleProperty("Value", this::get, this::set);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,118 +0,0 @@
|
|||||||
package frc4388.commands;
|
|
||||||
|
|
||||||
import org.junit.Test;
|
|
||||||
|
|
||||||
import frc4388.robot.commands.ShooterCommands.AimToCenter;
|
|
||||||
|
|
||||||
import org.junit.Assert;
|
|
||||||
|
|
||||||
public class AimToCenterTest {
|
|
||||||
|
|
||||||
private static final double DELTA = 1e-15;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Unit tests the isDeadzone function in AimToCenter.java
|
|
||||||
* @author Ryan Manley
|
|
||||||
* @link www.hoohle.com
|
|
||||||
*/
|
|
||||||
public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
|
|
||||||
boolean output;
|
|
||||||
|
|
||||||
//20 deg
|
|
||||||
output = AimToCenter.isDeadzone(20.);
|
|
||||||
Assert.assertFalse(output);
|
|
||||||
|
|
||||||
//-10 deg
|
|
||||||
output = AimToCenter.isDeadzone(-10.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//-1 deg
|
|
||||||
output = AimToCenter.isDeadzone(-1.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//341 deg
|
|
||||||
output = AimToCenter.isDeadzone(341.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//340 deg
|
|
||||||
output = AimToCenter.isDeadzone(340.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//0 deg
|
|
||||||
output = AimToCenter.isDeadzone(0.);
|
|
||||||
Assert.assertFalse(output);
|
|
||||||
|
|
||||||
//200 deg
|
|
||||||
output = AimToCenter.isDeadzone(200.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//2000000 deg
|
|
||||||
output = AimToCenter.isDeadzone(2000000.);
|
|
||||||
Assert.assertTrue(output);
|
|
||||||
|
|
||||||
//NaN deg
|
|
||||||
// output = AimToCenter.isDeadzone(Double.NaN);
|
|
||||||
// Assert.assertFalse(output);
|
|
||||||
}
|
|
||||||
|
|
||||||
//@Test
|
|
||||||
public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
|
|
||||||
double actual;
|
|
||||||
double expected;
|
|
||||||
|
|
||||||
//(5,5) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(5., 5., 0.);
|
|
||||||
expected = 180. + 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(-5,5) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
|
|
||||||
expected = 180. + 90. + 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(-5,-5) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
|
|
||||||
expected = 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(5,-5) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(5., -5., 0.);
|
|
||||||
expected = 90. + 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(0,0) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(0., 0., 0.);
|
|
||||||
Assert.assertNotNull(actual);
|
|
||||||
|
|
||||||
//(5,5) Gyro = 180 deg
|
|
||||||
actual = AimToCenter.angleToCenter(5., 5., 180.);
|
|
||||||
expected = 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(100,100) Gyro = 90 deg
|
|
||||||
actual = AimToCenter.angleToCenter(100., 100., 90.);
|
|
||||||
expected = 90. + 45.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(null,5) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
|
|
||||||
expected = Double.NaN;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(null,null) Gyro = 0 deg
|
|
||||||
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
|
|
||||||
expected = Double.NaN;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(null,null) Gyro = null deg
|
|
||||||
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
|
|
||||||
expected = Double.NaN;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
//(5,5) Gyro = -20 deg
|
|
||||||
actual = AimToCenter.angleToCenter(5., -5., -45.);
|
|
||||||
expected = 180.;
|
|
||||||
Assert.assertEquals(expected, actual, DELTA);
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,101 +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.utility;
|
|
||||||
|
|
||||||
import static org.junit.Assert.*;
|
|
||||||
|
|
||||||
import org.junit.*;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Add your docs here.
|
|
||||||
*/
|
|
||||||
public class RobotTimeUtilityTest {
|
|
||||||
RobotTime robotTime = RobotTime.getInstance();
|
|
||||||
|
|
||||||
@Test
|
|
||||||
public void testUpdateTimes() {
|
|
||||||
// Arrange
|
|
||||||
long lastTime;
|
|
||||||
robotTime.m_deltaTime = 0;
|
|
||||||
robotTime.m_robotTime = 0;
|
|
||||||
robotTime.m_lastRobotTime = 0;
|
|
||||||
robotTime.m_frameNumber = 0;
|
|
||||||
robotTime.endMatchTime();
|
|
||||||
robotTime.m_lastMatchTime = 0;
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(0, robotTime.m_deltaTime);
|
|
||||||
assertEquals(0, robotTime.m_robotTime);
|
|
||||||
assertEquals(0, robotTime.m_lastRobotTime);
|
|
||||||
assertEquals(0, robotTime.m_frameNumber);
|
|
||||||
lastTime = robotTime.m_robotTime;
|
|
||||||
|
|
||||||
// Act
|
|
||||||
wait(1);
|
|
||||||
robotTime.updateTimes();
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
|
||||||
assertEquals(true, robotTime.m_robotTime > 0);
|
|
||||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
|
||||||
assertEquals(1, robotTime.m_frameNumber);
|
|
||||||
lastTime = robotTime.m_robotTime;
|
|
||||||
|
|
||||||
// Act
|
|
||||||
wait(1);
|
|
||||||
robotTime.updateTimes();
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
|
||||||
assertEquals(true, robotTime.m_robotTime > 0);
|
|
||||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
|
||||||
assertEquals(2, robotTime.m_frameNumber);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Test
|
|
||||||
public void testMatchTime() {
|
|
||||||
// Arrange
|
|
||||||
long lastTime;
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(0, robotTime.m_matchTime);
|
|
||||||
assertEquals(0, robotTime.m_lastMatchTime);
|
|
||||||
lastTime = robotTime.m_matchTime;
|
|
||||||
|
|
||||||
// Act
|
|
||||||
robotTime.startMatchTime();
|
|
||||||
wait(1);
|
|
||||||
robotTime.updateTimes();
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(true, robotTime.m_matchTime > 0);
|
|
||||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
|
||||||
lastTime = robotTime.m_matchTime;
|
|
||||||
|
|
||||||
// Act
|
|
||||||
wait(1);
|
|
||||||
robotTime.updateTimes();
|
|
||||||
robotTime.endMatchTime();
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(0, robotTime.m_matchTime);
|
|
||||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
|
||||||
lastTime = robotTime.m_matchTime;
|
|
||||||
|
|
||||||
// Act
|
|
||||||
wait(1);
|
|
||||||
robotTime.updateTimes();
|
|
||||||
|
|
||||||
// Assert
|
|
||||||
assertEquals(0, robotTime.m_matchTime);
|
|
||||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
|
||||||
}
|
|
||||||
|
|
||||||
private void wait(int millis) {
|
|
||||||
try {
|
|
||||||
Thread.sleep(millis);
|
|
||||||
} catch (Exception e) {}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user