Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'full-robot-test' into Intake

Browse Source
This commit is contained in:
66945
2022-03-05 11:11:20 -08:00
committed by GitHub
parent 78f3b47846 ca9b247955
commit 9bfee4f194
65 changed files with 5023 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+131 -17
View File
@@ -4,6 +4,15 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
@@ -22,13 +31,16 @@ import frc4388.utility.LEDPatterns;
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 0.1;
public static final double ROTATION_SPEED = 4;
public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 22;
public static final double HEIGHT = 22;
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
public static final double MAX_SPEED_FEET_PER_SEC = 16;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
public static final double WIDTH = 15.25;
public static final double HEIGHT = 15.25;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
@@ -41,30 +53,70 @@ public final class Constants {
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
// ofsets are in degrees
//ofsets are in degrees
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
// public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
public static final int GYRO_ID = 14;
// offsets are in degrees
// NATHAN if you truncate or round or simplify these i will cry
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45 - 3.30;//
// 181.7578125;//180.0;//315.0 +45;//180.0;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.0625 +
// 0.18;// 360.-59.0625;//315.0;//224.296875 +
// public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.222;//
// 308.408203125;//225.0;//45.87890625;//360.0
// public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;//
// 180-2.021484375;//0.0;//134.384765625
public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.;
public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90) % 360.;
public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.;
public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180 - 90) % 360.;
// swerve PID constants
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final int SWERVE_TIMEOUT_MS = 30;
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SWERVE_GAINS = new Gains(1.5, 0.0, 0.0, 0.0, 0, 1.0);
// swerve auto constants
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI));
public static final boolean PATH_RECORD_VELOCITY = true;
public static final double PATH_MAX_VELOCITY = 5.0;
public static final double PATH_MAX_ACCELERATION = 5.0;
public static final double MIN_WAYPOINT_ANGLE = 20;
public static final double MIN_WAYPOINT_DISTANCE = 0.1;
public static final double MIN_WAYPOINT_VELOCITY = 0.1;
// swerve configuration
public static final double NEUTRAL_DEADBAND = 0.04;
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
public static final int REMOTE_0 = 0;
// conversions
// gear ratio: 5.14 rev motor = 1 rev wheel
// wheel diameter: official = 4 in, measured = 3.8 in
/* Ratio Calculation */
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 6.12;// 5.142857;
public static final double WHEEL_DIAMETER_INCHES = 4.0;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double INCHES_PER_METER = 39.370;
public static final double METERS_PER_INCH = 1 / INCHES_PER_METER;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 0.1;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
// TODO: put in real numbers for the hub
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
}
public static final class SerializerConstants {
@@ -98,5 +150,67 @@ public final class Constants {
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
}
public static final class ShooterConstants {
/* PID Constants Shooter */
public static final int CLOSED_LOOP_TIME_MS = 1;
public static final int SHOOTER_TIMEOUT_MS = 32;
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(
true, 60, 40, 0.5);
public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23;
public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24;
public static final int SHOOTER_ROTATE_ID = 31; // TODO: find this value
public static final double TURRET_SPEED_MULTIPLIER = 0.1d;
public static final int DEGREES_PER_ROT = 0;
public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
public static final double ENCODER_TICKS_PER_REV = 2048;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
public static final int TURRET_MOTOR_CAN_ID = 30;
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find
public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find
// deadzones
public static final double HARD_DEADZONE_LEFT = 0.0;
public static final double HARD_DEADZONE_RIGHT = 340.0;
public static final double DIG_DEADZONE_LEFT = 40.0;
public static final double DIG_DEADZONE_RIGHT = 60.0;
public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find
public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; // "//
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
/* Hood Constants */
public static final int SHOOTER_ANGLE_ADJUST_ID = 32;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find
public static final float HOOD_FORWARD_LIMIT = 0; // TODO: find
public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find
}
public static final class VisionConstants {
public static final double TURN_P_VALUE = 0.8;
public static final double X_ANGLE_ERROR = 0.5;
public static final double GRAV = 385.83;
public static final double TARGET_HEIGHT = 67.5;
public static final double FOV = 29.8; // Field of view limelight
public static final double LIME_ANGLE = 24.7;
}
}
src/main/java/frc4388/robot/Main.java
+12
View File
@@ -4,7 +4,15 @@
package frc4388.robot;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.diffplug.common.base.DurianPlugins;
import com.diffplug.common.base.Errors;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.utility.AnsiLogging;
/**
* Do NOT add any static variables to this class, or any initialization at all.
@@ -21,6 +29,10 @@ public final class Main {
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
AnsiLogging.systemInstall();
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));
RobotBase.startRobot(Robot::new);
}
}
// hi ryan -aarav
src/main/java/frc4388/robot/Robot.java
+113 -19
View File
@@ -4,9 +4,22 @@
package frc4388.robot;
import java.io.File;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.stream.IntStream;
import com.diffplug.common.base.Errors;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.utility.RobotTime;
/**
@@ -17,8 +30,9 @@ import frc4388.utility.RobotTime;
* project.
*/
public class Robot extends TimedRobot {
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
@@ -28,9 +42,70 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
LOGGER.log(Level.ALL, "Logging Test 1/8");
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
LOGGER.log(Level.WARNING, "Logging Test 3/8");
LOGGER.log(Level.INFO, "Logging Test 4/8");
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
Errors.log().run(() -> {
throw new Throwable("Exception Test");
});
// var path =
// PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
// Forward.path").toFile());
// LOGGER.finest(path::toString);
LOGGER.fine("robotInit()");
// LOGGER.fine("Sssssssssh.");
// DriverStation.silenceJoystickConnectionWarning(true);
// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
SmartDashboard.putData(CommandScheduler.getInstance());
SmartDashboard.putData("JVM Memory", new RunCommand(() -> {
}) {
@Override
public boolean runsWhenDisabled() {
return true;
}
@Override
public String getName() {
if (isScheduled()) {
Runtime runtime = Runtime.getRuntime();
long totalMemory = runtime.totalMemory() / 1_000_000;
long freeMemory = runtime.freeMemory() / 1_000_000;
long maxMemory = runtime.maxMemory() / 1_000_000;
return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
}
return "Not Running";
}
});
SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {
}) {
@Override
public boolean runsWhenDisabled() {
return true;
}
@Override
public String getName() {
if (isScheduled()) {
File deploy = Filesystem.getDeployDirectory();
long usedSpace = Errors.suppress().getWithDefault(
() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(),
0l) / 1_000_000;
long usableSpace = deploy.getUsableSpace() / 1_000_000;
return usedSpace + " MB / " + usableSpace + " MB";
}
return "Not Running";
}
});
}
/**
@@ -38,17 +113,27 @@ public class Robot extends TimedRobot {
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* <p>
* This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
// 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
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled
// commands, running already-scheduled commands, removing finished or
// interrupted commands,
// and running subsystem periodic() methods. This must be called from the
// robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
// print odometry data to smart dashboard for debugging (if causing timeout
// errors, you can comment it)
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
}
/**
@@ -58,7 +143,18 @@ public class Robot extends TimedRobot {
*/
@Override
public void disabledInit() {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
if (isTest()) {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
.resolve("recording." + System.currentTimeMillis() + ".path").toFile();
if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
m_robotContainer.createPath(null, null, false).write(outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
}
@Override
@@ -66,23 +162,14 @@ public class Robot extends TimedRobot {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
* This autonomous runs the autonomous command selected by your
* {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch (autoSelected) {
case "My Auto":
autonomousCommand = new MyAutoCommand();
break;
case "Default Auto":
default:
autonomousCommand = new ExampleCommand();
break;
}*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
@@ -99,6 +186,8 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
LOGGER.fine("teleopInit()");
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -107,6 +196,7 @@ public class Robot extends TimedRobot {
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
DriverStation.silenceJoystickConnectionWarning(true);
}
/**
@@ -114,7 +204,11 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
}
@Override
public void testInit() {
CommandScheduler.getInstance().cancelAll();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+345 -40
View File
@@ -4,17 +4,72 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.Joystick;
import java.io.File;
import java.io.IOException;
import java.io.StringWriter;
import java.nio.file.FileSystems;
import java.nio.file.Path;
import java.nio.file.StandardWatchEventKinds;
import java.nio.file.WatchEvent;
import java.nio.file.WatchKey;
import java.time.Clock;
import java.time.ZoneId;
import java.time.ZonedDateTime;
import java.time.format.DateTimeFormatter;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Matcher;
import java.util.regex.Pattern;
import java.util.stream.Collectors;
import com.diffplug.common.base.Errors;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot;
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.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.ListeningSendableChooser;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -24,41 +79,71 @@ import frc4388.utility.controller.XboxController;
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
m_robotMap.leftFrontEncoder,
m_robotMap.rightFrontEncoder,
m_robotMap.leftBackEncoder,
m_robotMap.rightBackEncoder
);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
private final Hood m_robotHood = new Hood();
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autonomous */
private PathPlannerTrajectory loadedPathTrajectory = null;
private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
private final List<Waypoint> pathPoints = new ArrayList<>();
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter
.ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'");
private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
// Function that removes the ".path" from the end of a string.
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED,
// m_robotLED));
// Turret default command
// m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret,
// m_robotSwerveDrive));
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// continually sends updates to the Blinkin LED controller to keep the lights on
/*
* m_robotLED
* .setDefaultCommand(new RunCommand(m_robotLED::updateLED,
* m_robotLED).withName("LED update defaultCommand"));
* autoInit();
* recordInit();
*/
}
/**
@@ -69,12 +154,43 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
// "XboxController.Button.kBack" was undefined yet, 7 works just fine
new JoystickButton(getDriverController(), 7)
.whenPressed(m_robotSwerveDrive::resetGyro);
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox,
// XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// new XboxControllerRawButton(m_driverXbox,
// XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
.whenPressed(() -> m_robotMap.leftFront.reset())
.whenPressed(() -> m_robotMap.rightFront.reset())
.whenPressed(() -> m_robotMap.leftBack.reset())
.whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
/*
* new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
* .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
* .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
*
* new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
* .whenPressed(new InstantCommand());
*
* // activates "BoomBoom"
* new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
* .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
* m_robotHood));
*/
}
/**
@@ -83,35 +199,224 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
if (loadedPathTrajectory != null) {
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
PathPlannerState initialState = loadedPathTrajectory.getInitialState();
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
return new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
} else {
LOGGER.severe("No auto selected.");
return new RunCommand(() -> {
}).withName("No Autonomous Path");
}
}
/**
* Add your docs here.
*/
public IHandController getDriverController() {
public XboxController getDriverController() {
return m_driverXbox;
}
/**
* Add your docs here.
* Get odometry.
*
* @return Odometry
*/
public IHandController getOperatorController() {
public Pose2d getOdometry() {
return m_robotSwerveDrive.getOdometry();
}
/**
* Set odometry to given pose.
*
* @param pose Pose to set odometry to.
*/
public void resetOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose);
}
public XboxController getOperatorController() {
return m_operatorXbox;
}
/**
* Add your docs here.
* Creates a WatchKey for the path planner directory and registers it with the
* WatchService.
* Then creates a NotifierCommand that will update the auto chooser with the
* latest path files.
* Finally, adds the existing path files to the auto chooser
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
private void autoInit() {
try {
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
StandardWatchEventKinds.ENTRY_DELETE);
// TODO: Store this and other commands as fields so they can be rescheduled.
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Path Watcher").schedule();
} catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
}
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
.filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
.forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
* Add your docs here.
* Creates a button on the SmartDashboard that will record the path of the
* robot.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
public void recordInit() {
SmartDashboard.putData("Recording",
new RunCommand(this::recordPeriodic) {
@Override
public void end(boolean interupted) {
new InstantCommand(RobotContainer.this::saveRecording) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Save Recording").schedule();
}
}.withName("Record Path (Cancel to Save)"));
}
/**
* Called when a file is created, modified, or deleted.
* Adds newly created .path files to the SendableChooser.
* Reloads the path if the currently selected file is modified.
*
* @param watchKey The WatchKey that is being observed.
*/
private void updateAutoChooser(WatchKey watchKey) {
List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
if (!watchEvents.isEmpty()) {
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
.filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
Path watchEventPath = (Path) pathWatchEvent.context();
File watchEventFile = watchEventPath.toFile();
String watchEventFileName = watchEventFile.getName();
if (watchEventFileName.endsWith(".path")) {
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
watchEventFileName);
autoChooser.addOption(watchEventFile.getName(), watchEventFile);
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
loadPath(watchEventFileName);
}
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
LOGGER.log(Level.SEVERE,
"PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
watchEventFileName);
}
}
}
}
if (!watchKey.reset())
LOGGER.severe("File watch key invalid.");
}
private void loadPath(String pathName) {
LOGGER.warning("Loading path " + pathName);
loadedPathTrajectory = null;
loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
LOGGER.info("Done loading");
}
private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = PATHPLANNER_DIRECTORY
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// TODO: Change to use measured maximum velocity and acceleration.
var path = createPath(null, null, false);
if (RobotBase.isReal())
path.write(outputFile);
StringWriter writer = new StringWriter();
path.write(writer);
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
autoChooser.setDefaultOption(outputFile.getName(), outputFile);
LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
pathPoints.add(waypoint);
}
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
// Remove points whose angles to neighboring points are less than 10 degrees
// apart.
int j = 0;
for (int i = 1; i < pathPoints.size() - 1; i++) {
var prev = pathPoints.get(j).anchorPoint.orElseThrow();
var current = pathPoints.get(i).anchorPoint.orElseThrow();
var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
var fromPrevious = current.minus(prev);
var toNext = next.minus(current);
var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
|| (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
&& pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
pathPoints.set(i, null);
else
j = i;
}
pathPoints.removeIf(Objects::isNull);
// Make control points
pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
for (int i = 1; i < pathPoints.size() - 1; i++) {
var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
}
pathPoints.get(pathPoints.size() - 1).prevControl = Optional
.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
// Create the path
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
path.maxVelocity = Optional.ofNullable(maxVelocity);
path.maxAcceleration = Optional.ofNullable(maxAcceleration);
path.isReversed = Optional.ofNullable(isReversed);
pathPoints.clear();
return path;
}
private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current,
Translation2d next) {
var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
return Pair.of(current.minus(line), current.plus(line));
}
}
src/main/java/frc4388/robot/RobotMap.java
+155 -37
View File
@@ -4,12 +4,23 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
/**
* Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -18,18 +29,20 @@ import frc4388.robot.Constants.SwerveDriveConstants;
public class RobotMap {
public RobotMap() {
configureLEDMotorControllers();
// configureLEDMotorControllers();
configureSwerveMotorControllers();
// configureShooterMotorControllers();
}
/* LED Subsystem */
public final Spark LEDController = new Spark(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);
@@ -42,12 +55,15 @@ public class RobotMap {
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_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
public SwerveModule leftFront;
public SwerveModule leftBack;
public SwerveModule rightFront;
public SwerveModule rightBack;
void configureSwerveMotorControllers() {
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
leftFrontSteerMotor.configFactoryDefault();
leftFrontWheelMotor.configFactoryDefault();
@@ -58,38 +74,140 @@ public class RobotMap {
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);
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.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);
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(mode);
leftFrontWheelMotor.setNeutralMode(mode);// Coast
rightFrontSteerMotor.setNeutralMode(mode);
rightFrontWheelMotor.setNeutralMode(mode);// Coast
leftBackSteerMotor.setNeutralMode(mode);
leftBackWheelMotor.setNeutralMode(mode);// Coast
rightBackSteerMotor.setNeutralMode(mode);
rightBackWheelMotor.setNeutralMode(mode);// Coast
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
// config cancoder as remote encoder for swerve steer motors
//leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
}
// Shooter Config
/* Boom Boom Subsystem */
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
// Create motor CANSparkMax
void configureShooterMotorControllers() {
// LEFT FALCON
shooterFalconLeft.configFactoryDefault();
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
shooterFalconLeft.setInverted(true);
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
ShooterConstants.SHOOTER_TIMEOUT_MS);
// RIGHT FALCON
shooterFalconRight.setInverted(false);
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
shooterFalconRight.configFactoryDefault();
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
// m_shooterFalconRight.configPeakOutputForward(0,
// ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
ShooterConstants.SHOOTER_TIMEOUT_MS);
// /* Turret Subsytem */
// shooterFalconRight.configStatorCurrentLimit(new
// StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers
// out of our ass anymore
// shooterFalconLeft.configSupplyCurrentLimit(new
// SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull
// numbers out of our ass anymore
}
}
src/main/java/frc4388/robot/commands/AimToCenter.java
+78
View File
@@ -0,0 +1,78 @@
// 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;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
public class AimToCenter extends CommandBase {
/** Creates a new AimWithOdometry. */
Turret m_turret;
SwerveDrive m_drive;
// use odometry to find x and y later
double x;
double y;
double m_targetAngle;
// public static Gains m_aimGains;
public AimToCenter(Turret turret, SwerveDrive drive) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_drive = drive;
addRequirements(m_turret, m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = 0;
y = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_turret.runshooterRotatePID(m_targetAngle);
}
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return angle;
}
/**
* Checks if in hardware deadzone (due to mechanical limitations).
* @param angle Angle to check.
* @return True if in hardware deadzone.
*/
public static boolean isHardwareDeadzone(double angle) {
return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
}
/**
* Checks if in digital deadzone (due to climber).
* @param angle Angle to check.
* @return True if in digital deadzone.
*/
public static boolean isDigitalDeadzone(double angle) {
return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
}
// 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;
}
}
src/main/java/frc4388/robot/commands/Shoot.java
+160
View File
@@ -0,0 +1,160 @@
// 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;
import edu.wpi.first.hal.simulation.SimulatorJNI;
import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.utility.DummySensor;
import frc4388.utility.Gains;
public class Shoot extends CommandBase {
// subsystems
public SwerveDrive m_swerve;
public BoomBoom m_boomBoom;
public Turret m_turret;
public Hood m_hood;
// given
public double m_gyroAngle;
public double m_odoX;
public double m_odoY;
public double m_distance;
// targets
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public double m_driveTargetAngle;
// pid
public double error;
public double prevError;
public Gains shootGains = ShooterConstants.SHOOT_GAINS;
public double kP, kI, kD;
public double proportional, integral, derivative;
public double time;
public double output;
public double tolerance = 5.0;
// testing
public DummySensor dummy = new DummySensor(0);
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
* TODO: Velocity Correction
* @param sDrive Drive Train
* @param sShooter Shooter Drum
* @param sTurret Shooter Turret
* @param sHood Shooter Hood
*/
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
// Use addRequirements() here to declare subsystem dependencies.
m_swerve = sDrive;
m_boomBoom = sShooter;
m_turret = sTurret;
m_hood = sHood;
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = shootGains.kP;
kI = shootGains.kI;
kD = shootGains.kD;
proportional = 0;
integral = 0;
derivative = 0;
time = 0.02;
DummySensor.resetAll();
}
/**
* Updates error for custom PID.
*/
public void updateError() {
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_odoX = m_swerve.getOdometry().getX();
m_odoY = m_swerve.getOdometry().getY();
m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
// target angle tests
m_gyroAngle = 0;
m_odoX = -1;
m_odoY = 1;
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
if (AimToCenter.isHardwareDeadzone(m_targetAngle)) {
m_targetAngle = m_targetAngle + 20;
}
if (AimToCenter.isDigitalDeadzone(m_targetAngle)) {
// this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work
m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true);
}
// initial error
updateError();
System.out.println("Error: " + error);
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
prevError = error;
updateError();
proportional = error;
integral = integral + error * time;
derivative = (error - prevError) / time;
output = kP * proportional + kI * integral + kD * derivative;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// custom pid
runPID();
// m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
// entire swerve drive or its the line below
m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
}
// 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() {
updateError();
return Math.abs(error) <= tolerance;
}
}
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+202
View File
@@ -0,0 +1,202 @@
// 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.subsystems;
import java.io.File;
import java.io.IOException;
import java.util.Comparator;
import java.util.Map;
import java.util.Optional;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Pattern;
import java.util.stream.IntStream;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
import frc4388.utility.Gains;
public class BoomBoom extends SubsystemBase {
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
public WPI_TalonFX m_shooterFalconLeft;
public WPI_TalonFX m_shooterFalconRight;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static BoomBoom m_boomBoom;
double velP;
double input;
public boolean m_isDrumReady = false;
public double m_fireVel;
public Hood m_hoodSubsystem;
public Turret m_turretSubsystem;
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
public static class ShooterTableEntry {
public Double distance, hoodExt, drumVelocity;
}
private ShooterTableEntry[] m_shooterTable;
/*
* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
*/
/** Creates a new BoomBoom. */
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
m_shooterFalconLeft = shooterFalconLeft;
m_shooterFalconRight = shooterFalconRight;
try {
// This is a helper class that allows us to read a CSV file into a Java array.
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
// This is a regular expression that removes all parentheses from the header of the CSV file.
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
/**
* Removes the parentheses from the CSV header
*
* @param header The header to be sanitized.
* @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
* The parentheses.matcher(header) is matching the header with the parentheses regular
* expression. The replaceAll method is replacing all of the parentheses with an empty
* string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
* the parent sanitizer.
*/
@Override
protected String headerSanitizer(final String header) {
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
}
};
// This is reading the CSV file into a Java array.
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
// This is a running a helper method that is logging the contents of the table to the console on a new thread.
new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
} catch (final IOException exception) {
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;
dummyEntry.hoodExt = 0.0;
dummyEntry.drumVelocity = 0.0;
m_shooterTable = new ShooterTableEntry[] { dummyEntry };
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
}
}
public Double getVelocity(final Double distance) {
// This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
// linear interpolation of the two values (drumVelocity) at the two closest points in the table
// (m_shooterTable) to the given value (distance).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
public Double getHood(final Double distance) {
// This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
// interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
// to the given value (distance).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
/**
* Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
* given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the
* interpolation (y) between the two values (y0 and y1) accquired by applying the target getter
* function to the lower and upper bounds entries.
*
* @param table An array of entries to search through.
* @param lookupValue The value to lookup in the table.
* @param lookupGetter A function that takes an entry from the table and returns .
* @param targetGetter A function that takes an E and returns a Number.
* @return The interpolated value.
*/
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
final E closestRecord = closestEntry.getValue();
final int closestRecordIndex = closestEntry.getKey();
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
}
/**
* If the value is in the table, return the entry. Otherwise, return the entry with the closest
* value
*
* @param table The array of values to search.
* @param value The value to search for.
* @param valueGetter A function that takes an element of the table and returns a Number to compare
* with the given value.
* @param exactMatch If true, the lookup will only return a match if the value is exactly equal to
* the value of the entry. If false, the lookup will return a match with a value closest to
* the given value.
* @return The entry with the closest value to the given value.
*/
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
}
/**
* Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a
* linear interpolation of the two values y0 and y1
*
* @param x The value to interpolate.
* @param x0 the x coordinate of the lower bound to interpolate to
* @param y0 The value at x0.
* @param x1 the x-coordinate of the upper bound to interpolate to
* @param y1 The value at x1.
* @return The interpolation between y0 and y1 at x.
*/
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
m_hoodSubsystem = subsystem0;
m_turretSubsystem = subsystem1;
}
/**
* Runs the Drum motor at a given speed
* @param speed percent output form -1.0 to 1.0
*/
public void runDrumShooter(double speed) {
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
}
public void setShooterGains() {
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
public void runDrumShooterVelocityPID(double targetVel) {
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
m_shooterFalconRight.follow(m_shooterFalconLeft);
// New BoomBoom controller stuff
// Controls a motor with the output of the BangBang controller
// Controls a motor with the output of the BangBang conroller and a feedforward
// Shrinks the feedforward slightly to avoid over speeding the shooter
// m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
// feedforward.calculate(targetVel));
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
}
src/main/java/frc4388/robot/subsystems/Hood.java
+47 -44
View File
@@ -16,19 +16,21 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
public class Hood extends SubsystemBase {
// public BoomBoom m_shooterSubsystem;
public BoomBoom m_shooterSubsystem;
public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
// public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public RelativeEncoder m_angleEncoder;
public CANSparkMax m_angleAdjustMotor;
public SparkMaxPIDController m_angleAdjusterPIDController;
public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
public boolean m_isHoodReady = false;
@@ -37,15 +39,17 @@ public double m_fireAngle;
/** Creates a new Hood. */
public Hood(CANSparkMax angleAdjustMotor) {
m_angleAdjustMotor = angleAdjustMotor;
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_angleEncoder= m_angleAdjustMotor.getEncoder();
m_angleAdjusterPIDController = m_angleAdjustMotor.getPIDController();
m_hoodUpLimitSwitch = m_angleAdjustMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodDownLimitSwitch = m_angleAdjustMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
public Hood() {
m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodUpLimitSwitch.enableLimitSwitch(true);
m_hoodDownLimitSwitch.enableLimitSwitch(true);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
setHoodSoftLimits(true);
}
@@ -53,44 +57,43 @@ public double m_fireAngle;
public void periodic() {
// This method will be called once per scheduler run
}
// public void runAngleAdjustPID(double targetAngle)
// {
// //Set PID Coefficients
// m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
// m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
// m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
// m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
// m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
// m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN / 5, m_angleAdjusterGains.m_kPeakOutput / 5);
// m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
// }
/**
* Runs The Hood
* @param input The Speed Times 0.6
/**
* Set status of hood motor soft limits.
* @param set Boolean to set soft limits to.
*/
public void runHood(double input) {
input *= .6;
m_angleAdjustMotor.set(input);
public void setHoodSoftLimits(boolean set) {
m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
public void runAngleAdjustPID(double targetAngle)
{
//Set PID Coefficients
m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP);
m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI);
m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD);
m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone);
m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF);
m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput);
m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
}
public void runHood(double input) {
m_angleAdjusterMotor.set(input);
}
/**
* Resets The Encoder
*/
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
/**
* Gets The Encoders Position
* @return The Encoders Position
*/
public double getAnglePositionPID() {
return m_angleEncoder.getPosition();
public double getAnglePosition(){
return 0.0;//m_angleEncoder.getPosition();
}
// public double getAnglePositionDegrees(){
// return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
// }
public double getAnglePositionDegrees(){
return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+5 -5
View File
@@ -4,10 +4,10 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.logging.Logger;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
@@ -27,12 +27,12 @@ public class LED extends SubsystemBase {
m_LEDController = LEDController;
setPattern(LEDConstants.DEFAULT_PATTERN);
updateLED();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
@Override
public void periodic(){
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
}
/**
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+253 -151
View File
@@ -4,183 +4,285 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase {
SwerveDriveKinematics m_kinematics;
private WPI_TalonFX m_leftFrontSteerMotor;
private WPI_TalonFX m_leftFrontWheelMotor;
private WPI_TalonFX m_rightFrontSteerMotor;
private WPI_TalonFX m_rightFrontWheelMotor;
private WPI_TalonFX m_leftBackSteerMotor;
private WPI_TalonFX m_leftBackWheelMotor;
private WPI_TalonFX m_rightBackSteerMotor;
private WPI_TalonFX m_rightBackWheelMotor;
private CANCoder m_leftFrontEncoder;
private CANCoder m_rightFrontEncoder;
private CANCoder m_leftBackEncoder;
private CANCoder m_rightBackEncoder;
private SwerveModule m_leftFront;
private SwerveModule m_leftBack;
private SwerveModule m_rightFront;
private SwerveModule m_rightBack;
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
Translation2d m_frontLeftLocation =
new Translation2d(
Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation =
new Translation2d(
Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation =
new Translation2d(
Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation =
new Translation2d(
Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
//setSwerveGains();
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public RobotGyro gyro; //TODO Add Gyro Lol
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
/*
* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
* The numbers used
* below are robot specific, and should be tuned.
*/
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
CANCoder rightFrontEncoder,
CANCoder leftBackEncoder,
CANCoder rightBackEncoder)
{
m_leftFrontSteerMotor = leftFrontSteerMotor;
m_leftFrontWheelMotor = leftFrontWheelMotor;
m_rightFrontSteerMotor = rightFrontSteerMotor;
m_rightFrontWheelMotor = rightFrontWheelMotor;
m_leftBackSteerMotor = leftBackSteerMotor;
m_leftBackWheelMotor = leftBackWheelMotor;
m_rightBackSteerMotor = rightBackSteerMotor;
m_rightBackWheelMotor = rightBackWheelMotor;
m_leftFrontEncoder = leftFrontEncoder;
m_rightFrontEncoder = rightFrontEncoder;
m_leftBackEncoder = leftBackEncoder;
m_rightBackEncoder = rightBackEncoder;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
modules = new SwerveModule[] {
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
};
//gyro.reset();
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
{
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
driveFromSpeeds(speeds);*/
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states =
kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state);
}
}
//Converts a ChassisSpeed to SwerveModuleStates (targets)
public void driveFromSpeeds(ChassisSpeeds speeds)
{
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
// Convert to module states
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
private final Field2d m_field = new Field2d();
// Front left module state
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
// Front right module state
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
// Back left module state
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
// Back right module state
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
//Set the motors
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
WPI_PigeonIMU gyro) {
m_leftFront = leftFront;
m_leftBack = leftBack;
m_rightFront = rightFront;
m_rightBack = rightBack;
m_gyro = gyro;
modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack };
m_poseEstimator = new SwerveDrivePoseEstimator(
m_gyro.getRotation2d(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
VecBuilder.fill(Units.degreesToRadians(1)),
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
//Sets steering motors to PID values
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
{
/*//Set the Wheel motor speeds
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
//PID
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
System.out.println("Target: " + leftFront.angle.getDegrees());*/
}
/*public void setSwerveGains(){
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}*/
// https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @param speeds[0] Speed of the robot in the x direction (forward).
* @param speeds[1] Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the
* field.
*/
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
if (speedX == 0 && speedY == 0 && rot == 0)
ignoreAngles = true;
else
ignoreAngles = false;
Translation2d speed = new Translation2d(-speedX, speedY);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(-leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
setModuleStates(states);
}
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
// {
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
// }
/**
* Set each module of the swerve drive to the corresponding desired state.
*
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state, false);
}
// modules[0].setDesiredState(desiredStates[0], false);
}
@Override
public void periodic() {
updateOdometry();
updateSmartDash();
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
private void updateSmartDash() {
// odometry
SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have
// accurate chassis speeds
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* Gets the distance between two given poses.
*
* @param p1 The first pose.
* @param p2 The second pose.
* @return Absolute distance between p1 and p2.
*/
public double distBtwPoses(Pose2d p1, Pose2d p2) {
return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
}
/**
* Returns a scalar from your distance to the hub to your target distance.
*
* @param target_dist The target distance.
* @return A scalar that multiplies your distance from the hub to get your
* target distance.
*/
public Pose2d poseGivenDist(double target_dist) {
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
double scalar = target_dist / distBtwPoses(p1, p2);
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
return new_pose;
}
/**
* Gets the current pose of the robot.
*
* @return Robot's current pose.
*/
public Pose2d getOdometry() {
// return m_odometry.getPoseMeters();
return m_poseEstimator.getEstimatedPosition();
}
/**
* Gets the current gyro using regression formula.
*
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
return new Rotation2d(regCur * Math.PI / 180);
}
/**
* Resets the odometry of the robot to the given pose.
*/
public void resetOdometry(Pose2d pose) {
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
/**
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
m_poseEstimator.update(getRegGyro(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
// Also apply vision measurements. We use 0.3 seconds in the past as an example
// -- on
// a real robot, this must be calculated based either on latency or timestamps.
// m_poseEstimator.addVisionMeasurement(
// m_poseEstimator.getEstimatedPosition(),
// Timer.getFPGATimestamp() - 0.1);
}
/**
* Resets pigeon.
*/
public void resetGyro() {
m_gyro.reset();
rotTarget = new Rotation2d(0);
}
/**
* Stop all four swerve modules.
*/
public void stopModules() {
modules[0].stop();
modules[1].stop();
modules[2].stop();
modules[3].stop();
}
/**
* Switches speed modes.
*
* @param shift True if fast mode, false if slow mode.
*/
public void highSpeed(boolean shift) {
if (shift) {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
} else {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+128 -26
View File
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
@@ -21,73 +22,174 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
public WPI_TalonFX angleMotor;
public WPI_TalonFX driveMotor;
private CANCoder canCoder;
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
private static double kEncoderTicksPerRotation = 4096;
private SwerveModuleState state;
private double canCoderFeedbackCoefficient;
public long m_currentTime;
public long m_lastTime;
public double m_deltaTime;
public double m_currentPos;
public double m_lastPos;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.canCoder = canCoder;
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP;
angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI;
angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD;
angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
// Use the CANCoder as the remote sensor for the primary TalonFX PID
angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleTalonFXConfiguration);
// angleMotor.setInverted(true);
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
// driveTalonFXConfiguration.slot0.kP = 0.05;
// driveTalonFXConfiguration.slot0.kI = 0.0;
// driveTalonFXConfiguration.slot0.kD = 0.0;
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
// FeedbackDevice.IntegratedSensor;
driveMotor.configFactoryDefault();
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
driveMotor.configNominalOutputForward(0, 30);
driveMotor.configNominalOutputReverse(0, 30);
driveMotor.configPeakOutputForward(1, 30);
driveMotor.configPeakOutputReverse(-1, 30);
driveMotor.configAllowableClosedloopError(0, 0, 30);
// driveMotor.setInverted(true);
driveMotor.config_kP(0, 0, 30);
driveMotor.config_kI(0, 0, 30);
driveMotor.config_kD(0, 0, 30);
// maybe try a feedforward value?
/*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
driveTalonFXConfiguration.slot0.kP = kDriveP;
driveTalonFXConfiguration.slot0.kI = kDriveI;
driveTalonFXConfiguration.slot0.kD = kDriveD;
driveTalonFXConfiguration.slot0.kF = kDriveF;
driveMotor.configAllSettings(driveTalonFXConfiguration);*/
// driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.magnetOffsetDegrees = offset;
canCoderConfiguration.sensorDirection = true;
canCoder.configAllSettings(canCoderConfiguration);
m_currentTime = System.currentTimeMillis();
m_lastTime = System.currentTimeMillis();
m_lastPos = driveMotor.getSelectedSensorPosition();
}
public Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback coefficient
// and the sesnor value reports degrees.
private Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback
// coefficient
// and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
/**
* Set the speed + rotation of the swerve module from a SwerveModuleState object
* @param desiredState - A SwerveModuleState representing the desired new state of the module
*
* @param desiredState - A SwerveModuleState representing the desired new state
* of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(),
// currentRotation.getDegrees());
state = SwerveModuleState.optimize(desiredState, currentRotation);
// Find the difference between our current rotational position + our new rotational position
// Find the difference between our current rotational position + our new
// rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// Find the new absolute position of the module based on the difference in rotation
// Find the new absolute position of the module based on the difference in
// rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
// Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
// Please work
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection +
// (Units.metersToInches(state.speedMetersPerSecond) *
// SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set(normFtPerSec);// - angleMotor.get());
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
// between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
// m_currentTime = System.currentTimeMillis();
// m_deltaTime = (double) (m_currentTime - m_lastTime);
// m_deltaTime = m_deltaTime / 10.0;
// m_currentPos = driveMotor.getSelectedSensorPosition();
// double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
// double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) %
// 2048;
// double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) -
// (m_deltaTime * driveMotor.getSelectedSensorVelocity());
// double m_actualDesiredPos = m_deltaTime *
// ((Units.metersToInches(state.speedMetersPerSecond) *
// SwerveDriveConstants.TICKS_PER_INCH) / 10);
// System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
// System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
// System.out.println("Last Pos: " + m_lastPos);
// driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/);
// m_lastTime = m_currentTime;
// m_lastPos = m_currentPos;
}
}
/**
* Get current module state.
*
* @return The current state of the module in m/s.
*/
public SwerveModuleState getState() {
// return state;
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
* SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
}
/**
* Stop the drive and steer motors of current module.
*/
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
@Override
public void periodic() {
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
}
public void reset() {
canCoder.setPositionToAbsolute();
// canCoder.configSensorInitializationStrategy(initializationStrategy)
}
}
src/main/java/frc4388/robot/subsystems/Turret.java
+115
View File
@@ -0,0 +1,115 @@
// 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.subsystems;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import java.util.concurrent.TimeoutException;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxLimitSwitch;
import com.revrobotics.SparkMaxPIDController;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.commands.Shoot;
import frc4388.utility.Gains;
public class Turret extends SubsystemBase {
/** Creates a new Turret. */
public BoomBoom m_boomBoomSubsystem;
public SwerveDrive m_sDriveSubsystem;
public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID,
// MotorType.kBrushless);
public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS;
SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit;
public Gyro m_turretGyro;
public double m_targetDistance = 0;
public boolean m_isAimReady = false;
SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
// Variables
public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument
m_boomBoomRotateMotor = boomBoomRotateMotor;
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit.enableLimitSwitch(true);
m_boomBoomLeftLimit.enableLimitSwitch(true);
SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT);
setTurretSoftLimits(true);
m_boomBoomRotateMotor.setInverted(false);
m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Set status of turret motor soft limits.
* @param set Boolean to set soft limits to.
*/
public void setTurretSoftLimits(boolean set) {
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) {
m_boomBoomSubsystem = subsystem0;
m_sDriveSubsystem = subsystem1;
}
public void runTurretWithInput(double input) {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
public void runshooterRotatePID(double targetAngle) {
targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
public void resetGyroShooterRotate() {
m_boomBoomRotateEncoder.setPosition(0);
}
public double getboomBoomRotatePosition() {
return m_boomBoomRotateEncoder.getPosition();
}
public double getBoomBoomAngleDegrees() {
return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
/ ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+131
View File
@@ -0,0 +1,131 @@
// 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.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.networktables.NetworkTableEntry;
import frc4388.robot.Constants.VisionConstants;
public class Vision extends SubsystemBase {
//setup
Turret m_turret;
BoomBoom m_boomBoom;
Hood m_hood;
NetworkTableEntry xEntry;
//Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
public double realDistance;
public static double fireVel;
public static double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
public double m_fireAngle;
public Vision(Turret aimSubsystem, BoomBoom boomBoom) {
m_turret = aimSubsystem;
m_boomBoom = boomBoom;
m_hood = m_boomBoom.m_hoodSubsystem;
//addRequirements(m_turret);
limeOff();
changePipeline(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3);
}
public void track(){
target = getV();
xAngle = getX();
yAngle = getY();
//find distance
distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
realDistance = (1.09 * distance) - 12.8;
// if (target == 1.0) { //checks if target is in view
// //aims left and right
// turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
// if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
// turnAmount = 0;
// }
// else if (turnAmount > 0 && turnAmount < 0.1){
// turnAmount = 0.1;
// }
// else if (turnAmount < 0 && turnAmount > -0.1){
// turnAmount = -0.1;
// }
// }
SmartDashboard.putNumber("Distance to Target", realDistance);
// //start CSV
// fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance);
// fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance);
// //fire angle unknown so far
// //end of CSV
// m_boomBoom.m_fireVel = fireVel;
// m_hood.m_fireAngle = fireAngle;
// m_turret.m_targetDistance = distance;
// checkFinished();
}
public void checkFinished(){
if (xAngle < 0.5 && xAngle > -0.5 && target == 1){
m_turret.m_isAimReady = true;
}
else{
m_turret.m_isAimReady = false;
}
}
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
public void changePipeline(int pipelineId)
{
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
}
public double getV()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
}
public double getX()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
}
public double getY()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
}
@Override
public void periodic(){
//called once per scheduler run
}
}