diff --git a/simgui-ds.json b/simgui-ds.json index b16ea5c..1cc0e4e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,6 +96,9 @@ { "guid": "78696e70757401000000000000000000", "useGamepad": true + }, + { + "guid": "03000000c01600008704000000000000" } ] } diff --git a/simgui.json b/simgui.json index fb4fc2f..f3617f0 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,8 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Climber": "Subsystem", + "/LiveWindow/Extender": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", @@ -40,9 +42,14 @@ "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/LiveWindow/Ungrouped/Spark[0]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [14]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [15]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [21]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [22]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [30]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [31]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", @@ -54,8 +61,24 @@ "/LiveWindow/VisionOdometry": "Subsystem", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/JVM Memory": "Command", + "/SmartDashboard/Odometry Chooser": "String Chooser", "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/Usable Deploy Space": "Command" } + }, + "NetworkTables": { + "SmartDashboard": { + "Odometry Chooser": { + "open": true + }, + "SendableChooser[0]": { + "open": true + }, + "open": true + } + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 573b060..ccef197 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,3 +1,12 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -0 ,16 ,0 -999 ,28.8 ,1200 \ No newline at end of file +0 ,-29.5 ,8000 +78.5 ,-29.5 ,8000 +99 ,-39.62 ,8500 +127.25 ,-49.12 ,9500 +150 ,-66.22 ,10000 +164.5 ,-75.52 ,10000 +186 ,-76.24 ,10000 +207 ,-104.07 ,11000 +227 ,-105.32 ,11500 +255.5 ,-105.8 ,13500 +999 ,-105.8 ,13500 \ No newline at end of file diff --git a/src/main/deploy/VelocityCorrectionData.csv b/src/main/deploy/VelocityCorrectionData.csv new file mode 100644 index 0000000..21d13a2 --- /dev/null +++ b/src/main/deploy/VelocityCorrectionData.csv @@ -0,0 +1,12 @@ +Distance (in) ,Duration (s) +0 ,0 +78.5 ,0 +99 ,0 +127.25 ,0 +150 ,0 +164.5 ,0 +186 ,0 +207 ,0 +227 ,0 +255.5 ,0 +999 ,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2999836..7930670 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,9 @@ package frc4388.robot; +import java.security.PublicKey; + +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.math.controller.PIDController; @@ -12,7 +15,7 @@ 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 edu.wpi.first.math.util.Units; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -33,28 +36,27 @@ public final class Constants { public static final double TICKS_PER_ROTATION_FX = 2048; public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4; - public static final double WHEEL_SPEED = 0.1; - public static final double WIDTH = 15.25; - public static final double HEIGHT = 15.25; + public static final double ROTATION_SPEED = 4.0; + public static final double WIDTH = 23.5; + public static final double HEIGHT = 23.5; 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? + public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant? + public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: 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; - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - public static final int LEFT_BACK_STEER_CAN_ID = 6; - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - public static final int RIGHT_BACK_STEER_CAN_ID = 8; - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - 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; + 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; // + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // + public static final int LEFT_BACK_STEER_CAN_ID = 6; // + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // + public static final int RIGHT_BACK_STEER_CAN_ID = 8; // + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // + 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; // public static final int GYRO_ID = 14; // offsets are in degrees @@ -68,10 +70,10 @@ public final class Constants { // 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.; + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 ) % 360.; + public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 ) % 360.; + public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50) % 360.; + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180) % 360.; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -107,7 +109,6 @@ public final class Constants { public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double INCHES_PER_METER = 39.370; public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; @@ -115,9 +116,20 @@ public final class Constants { public static final double TICK_TIME_TO_SECONDS = 0.1; public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + // current limits + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_STEER = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_STEER = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); + + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_WHEEL = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); + + // 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)); } @@ -125,17 +137,28 @@ public final class Constants { public static final double SERIALIZER_BELT_SPEED = 0.1d; // CAN IDs - public static final int SERIALIZER_BELT = 16; + public static final int SERIALIZER_BELT = 17; public static final int SERIALIZER_BELT_BEAM = 27; // TODO } public static final class IntakeConstants { // CAN IDs - public static final int INTAKE_MOTOR = 14; - public static final int EXTENDER_MOTOR = 15; + public static final int INTAKE_MOTOR = 15; + public static final int EXTENDER_MOTOR = 16; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); //Find + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); + public static final double INTAKE_SPEED_MULTIPLIER = 0.4; } + + public static final class ExtenderConstants { + public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0; + public static final double EXTENDER_REVERSE_LIMIT = 0.0; + } + public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 17; + public static final int STORAGE_CAN_ID = 18; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO public static final int BEAM_SENSOR_INTAKE = 29; //TODO public static final double STORAGE_SPEED = 0.3; @@ -217,8 +240,10 @@ 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 int BUTTON_BOX_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; + public static final boolean SKEW_STICKS = true; } public static final class ShooterConstants { @@ -228,47 +253,48 @@ public final class Constants { 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 SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = new SupplyCurrentLimitConfiguration( + true, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_SHOOTER = new StatorCurrentLimitConfiguration( + true, 27, 0, 0); + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; + public static final double TURRET_SPEED_MULTIPLIER = 0.8; + public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; + public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; 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); + public static final Gains SHOOT_GAINS = new Gains(5.0, 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 + public static final int TURRET_MOTOR_CAN_ID = 19; + //Gains for turret + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/); + public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER; + //Gains for hood + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - // deadzones - public static final double HARD_DEADZONE_LEFT = 0.0; - public static final double HARD_DEADZONE_RIGHT = 340.0; + public static final double TURRET_FORWARD_HARD_LIMIT = 18.429; + public static final double TURRET_REVERSE_HARD_LIMIT = -106.454; - public static final double DIG_DEADZONE_LEFT = 40.0; - public static final double DIG_DEADZONE_RIGHT = 60.0; + public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5; + public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; - 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 + public static final double TURRET_SOFT_LIMIT_TOLERANCE = 20.0; + //Shooter gains for actual Drum + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); /* Hood Constants */ - public static final int SHOOTER_ANGLE_ADJUST_ID = 32; + public static final int SHOOTER_ANGLE_ADJUST_ID = 20; 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 double HOOD_FORWARD_SOFT_LIMIT = 0.0; // TODO: find + public static final double HOOD_REVERSE_SOFT_LIMIT = -150; // TODO: find + public static final double HOOD_SOFT_LIMIT_TOLERANCE = 20.0; } @@ -279,21 +305,25 @@ public final class Constants { // 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; + public static final double LIME_ANGLE = 56.4; public static final String NAME = "photonCamera"; - public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric - public static final double TARGET_RADIUS = 4*12; // Convert to metric + public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?) + public static final double LIME_HEIGHT = 26; + public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; - public static final double V_FOV = 49.7; - public static final double LIME_VIXELS = 960; - public static final double LIME_HIXELS = 720; + public static final double V_FOV = 45.7; + public static final double LIME_HIXELS = 920; + public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; + public static final double POINTS_THRESHOLD = 400; + public static final double RANGE = 10; - public static final double LIMELIGHT_RADIUS = 1.d; + public static final double EDGE_TO_CENTER = 20; + public static final double LIMELIGHT_RADIUS = 8; public static final double SHOOTER_CORRECTION = 1.d; } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index b83b7c8..759e2c9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,20 +7,27 @@ package frc4388.robot; import java.io.File; import java.nio.file.Files; import java.nio.file.Path; +import java.util.HashMap; +import java.util.Map; 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.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; +import frc4388.utility.VelocityCorrection; +import frc4388.utility.desmos.DesmosServer; /** * The VM is configured to automatically run this class, and to call the @@ -36,6 +43,15 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + private SendableChooser odoChooser = new SendableChooser(); + private HashMap odoChoices = new HashMap<>(); + private Pose2d selectedOdo; + private double current; + + private static DesmosServer desmosServer = new DesmosServer(8000); + + public static Alliance alliance; + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -106,6 +122,9 @@ public class Robot extends TimedRobot { return "Not Running"; } }); + + desmosServer.start(); + // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } /** @@ -120,6 +139,21 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); + // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); + current = + m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotClimber.getCurrent() + + m_robotContainer.m_robotHood.getCurrent() + + m_robotContainer.m_robotIntake.getCurrent() + + m_robotContainer.m_robotExtender.getCurrent() + + m_robotContainer.m_robotSerializer.getCurrent() + + m_robotContainer.m_robotStorage.getCurrent() + + m_robotContainer.m_robotSwerveDrive.getCurrent(); + m_robotContainer.m_robotTurret.getCurrent(); + SmartDashboard.putNumber("Total Robot Current Draw", current); + SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); + SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or @@ -134,6 +168,25 @@ public class Robot extends TimedRobot { // 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()); + // odo chooser stuff + addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), + new Pose2d(1, 2, new Rotation2d(Math.PI/3)), + new Pose2d(1, 3, new Rotation2d(Math.PI/4))); + updateOdoChooser(); + SmartDashboard.putData("Odometry Chooser", odoChooser); + } + + public void updateOdoChooser() { + for (Map.Entry entry : odoChoices.entrySet()) { + odoChooser.addOption(entry.getKey(), entry.getValue()); + } + } + + public void addOdoChoices(Pose2d... points) { + for (Pose2d point : points) { + String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + "°)"; + odoChoices.put(key, point); + } } /** @@ -155,6 +208,8 @@ public class Robot extends TimedRobot { } else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } + + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override @@ -168,6 +223,15 @@ public class Robot extends TimedRobot { @Override public void autonomousInit() { LOGGER.fine("autonomousInit()"); + + Robot.alliance = DriverStation.getAlliance(); + + selectedOdo = odoChooser.getSelected(); + if (selectedOdo == null) { + selectedOdo = m_robotContainer.getOdometry(); + } + m_robotContainer.resetOdometry(selectedOdo); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -187,7 +251,19 @@ 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()); + + + Robot.alliance = DriverStation.getAlliance(); + + m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); + + selectedOdo = odoChooser.getSelected(); + if (selectedOdo == null) { + selectedOdo = m_robotContainer.getOdometry(); + } + m_robotContainer.resetOdometry(selectedOdo); + + // 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 @@ -203,8 +279,7 @@ public class Robot extends TimedRobot { * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4ae3217..791fcbc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,12 +29,16 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; 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 org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -51,6 +55,7 @@ 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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -62,7 +67,20 @@ import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.StorageConstants; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; +import frc4388.robot.commands.ShooterCommands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.commands.ShooterCommands.TrackTarget; +import frc4388.robot.commands.StorageCommands.ManageStorage; import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -78,6 +96,7 @@ import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; import frc4388.utility.Vector2D; import frc4388.utility.PathPlannerUtil.Path.Waypoint; +import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -89,32 +108,38 @@ import frc4388.utility.controller.DeadbandedXboxController; */ public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + + // RobotMap + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); - // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, - // m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + // Subsystems + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); + public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); + + public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); + public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - // private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); - // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); - // //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(m_robotMap.angleAdjusterMotor); - // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - - - /* Controllers */ + private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); + + // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); - /* Autonomous */ + // Autonomous private PathPlannerTrajectory loadedPathTrajectory = null; // private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); @@ -128,6 +153,8 @@ public class RobotContainer { // Function that removes the ".path" from the end of a string. private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); + + public static boolean softLimits = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -147,39 +174,52 @@ public class RobotContainer { // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - // Turret default command% - //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); - // m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); - - // //Swerve Drive - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - // getDriverController().getLeftX(), - // getDriverController().getLeftY(), - // getDriverController().getRightX(), - // getDriverController().getRightY(), - // true), - // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // //Intake with Triggers - // m_robotIntake.setDefaultCommand( - // new RunCommand(() -> m_robotIntake.runWithTriggers( - // getOperatorController().getLeftTriggerAxis(), - // getOperatorController().getRightTriggerAxis()), - // m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - // //Storage Management + // Swerve Drive with Input + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + getDriverController().getLeftX(), + getDriverController().getLeftY(), + //getDriverController().getRightX(), + getDriverController().getRightX(), + getDriverController().getRightY(), + true), + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // Intake with Triggers + m_robotIntake.setDefaultCommand( + new RunCommand(() -> m_robotIntake.runWithTriggers( + getOperatorController().getLeftTriggerAxis(), + getOperatorController().getRightTriggerAxis()), + m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + // m_robotStorage.setDefaultCommand( - // new RunCommand(() -> m_robotStorage.manageStorage(), - // m_robotStorage).withName("Storage manageStorage defaultCommand")); - // //Serializer Management - // m_robotSerializer.setDefaultCommand( - // new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), - // m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // new ManageStorage(m_robotStorage, + // m_robotBoomBoom, + // m_robotTurret).withName("Storage ManageStorage defaultCommand")); + + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) + // ); + // Storage Management + /*m_robotStorage.setDefaultCommand( + new RunCommand(() -> m_robotStorage.manageStorage(), + m_robotStorage).withName("Storage manageStorage defaultCommand"));*/ + // Serializer Management + m_robotSerializer.setDefaultCommand( + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // Turret Manual + m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + m_robotHood.setDefaultCommand( + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); + // m_robotTurret.setDefaultCommand( + // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // 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")); + // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand")); + // autoInit(); // recordInit(); } @@ -193,88 +233,112 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - // "XboxController.Button.kBack" was undefined yet, 7 works just fine - // new JoystickButton(getDriverController(), XboxController.Button.kBack.value) - // .whenPressed(m_robotSwerveDrive::resetGyro); - // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - // // new XboxControllerRawButton(m_driverXbox, - // // XboxControllerRaw.LEFT_BUMPER_BUTTON) - // .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kBack.value) + .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kStart.value) + .whenPressed(m_robotSwerveDrive::resetGyro); + // Left Bumper > Shift Down + new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // Right Bumper > Shift Up + new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + - // 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) //Temp - // .whenPressed(() -> m_robotMap.leftFront.reset()) - // .whenPressed(() -> m_robotMap.rightFront.reset()) - // .whenPressed(() -> m_robotMap.leftBack.reset()) - // .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ - // run claws - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2))) - // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2))) - // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2))) - // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) - // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + + + + + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + + //Toggles extender in and out + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) + .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); + + // Right Bumper > Storage In + // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // // Left Bumper > Storage Out (note: neccessary?) + // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // A > Shoot with Odo + /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ + + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); + + //B > Shoot with Lime + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + // .whileHeld% + + + /* Button Box Buttons */ + + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); + + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new TurretManual(m_robotTurret)); + + // control turret manual mode + // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) + // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + + + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } - /* - * // activates "BoomBoom" - * new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, - * m_robotHood)); - */ - - //Extender - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whenPressed(() -> m_robotIntake.runExtender(true)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(() -> m_robotIntake.runExtender(false)); - - - - // //Storage - // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - // .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) - // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - - - // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - // .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) - // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - - // //Shooter - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); - // } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -308,6 +372,18 @@ public class RobotContainer { return m_driverXbox; } + public XboxController getOperatorController() { + return m_operatorXbox; + } + + public ButtonBox getButtonBox() { + return m_buttonBox; + } + + public static void setSoftLimits(boolean set) { + softLimits = set; + } + /** * Get odometry. * @@ -326,10 +402,6 @@ public class RobotContainer { // m_robotSwerveDrive.resetOdometry(pose); // } - public XboxController getOperatorController() { - return m_operatorXbox; - } - /** * Creates a WatchKey for the path planner directory and registers it with the * WatchService. @@ -362,144 +434,145 @@ public class RobotContainer { * Creates a button on the SmartDashboard that will record the path of the * robot. */ - // 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> watchEvents = watchKey.pollEvents(); - // if (!watchEvents.isEmpty()) { - // List> 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."); - // } + 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)")); + } - // 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"); - // } + /** + * 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> watchEvents = watchKey.pollEvents(); + if (!watchEvents.isEmpty()) { + List> 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 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()); - // } + 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"); + } - // // 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); - // // } + 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 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; - // } + 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.getChassisSpeeds()[0], + m_robotSwerveDrive.getChassisSpeeds()[1]); + Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, + SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); + pathPoints.add(waypoint); + } - // private static Pair 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)); - // } + 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 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)); + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 761839b..9638c45 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,16 +10,18 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.WPI_PigeonIMU; - 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_Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.networktables.NetworkTableType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; @@ -40,15 +42,19 @@ import frc4388.robot.subsystems.SwerveModule; public class RobotMap { public RobotMap() { - configureLEDMotorControllers(); + // configureLEDMotorControllers(); configureSwerveMotorControllers(); configureShooterMotorControllers(); + configureIntakeMotors(); + configureExtenderMotors(); + configureSerializerMotors(); + configureStorageMotors(); } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); +// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() {} +// void configureLEDMotorControllers() {} /* Swerve Subsystem */ public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); @@ -143,6 +149,28 @@ public class RobotMap { rightBackSteerMotor.setNeutralMode(mode); rightBackWheelMotor.setNeutralMode(mode);// Coast + // current limits + + leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + + leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, @@ -165,7 +193,7 @@ public class RobotMap { rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } +} /* Climb Subsystem */ public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO @@ -186,7 +214,7 @@ public class RobotMap { public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -202,13 +230,15 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.configFactoryDefault(); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.setInverted(false); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); // m_shooterFalconRight.configPeakOutputForward(0, @@ -217,34 +247,56 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, 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 + shooterFalconRight.follow(shooterFalconLeft); + + // turret + shooterTurret.restoreFactoryDefaults(); + shooterTurret.setIdleMode(IdleMode.kBrake); + shooterTurret.setInverted(true); // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + angleAdjusterMotor.setInverted(true); } - /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); -// public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); /* Intake Subsystem */ public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); - /* Storage Subsystem */ - public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); - public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); + void configureIntakeMotors() { + intakeMotor.configFactoryDefault(); + intakeMotor.setInverted(false); + intakeMotor.setNeutralMode(NeutralMode.Coast); + + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); + intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); + } + + void configureExtenderMotors() { + extenderMotor.restoreFactoryDefaults(); + extenderMotor.setInverted(true); + extenderMotor.setIdleMode(IdleMode.kBrake); + } + + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } + + /* Storage Subsystem */ + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + + void configureStorageMotors() { + storageMotor.restoreFactoryDefaults(); + } + } diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java new file mode 100644 index 0000000..095cc9f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java @@ -0,0 +1,61 @@ +// 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.ButtonBoxCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RunMiddleSwitch extends CommandBase { + + private static boolean manual = false; + private boolean newManual = false; + private boolean changes = false; + + /** Creates a new RunMiddleSwitch. */ + public RunMiddleSwitch() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + changes = (newManual == manual); + + if (manual) { + printManual(); + } else { + printNormal(); + } + + newManual = manual; + } + + public void printNormal(){ + System.out.println("Normal"); + } + + public void printManual(){ + System.out.println("Manual"); + } + + public static void setManual(boolean set) + { + manual = set; + } + + // 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 changes; + } +} diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java new file mode 100644 index 0000000..53d772a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java @@ -0,0 +1,62 @@ +// 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.ButtonBoxCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Turret; + +public class TurretManual extends CommandBase { + + // subsystems + private Turret turret; + + // booleans + private static boolean manual = false; + private boolean newManual = false; + private boolean changes = false; + + /** Creates a new TurretManual. */ + public TurretManual(Turret turret) { + // Use addRequirements() here to declare subsystem dependencies. + this.turret = turret; + + addRequirements(this.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + changes = (newManual != manual); + + if (manual) { + System.out.println("Manual Turret"); // TODO: turret manual controls + } else { + System.out.println("Auto Turret"); // TODO: turret auto controls; + } + + newManual = manual; + } + + public static void setManual(boolean set) + { + manual = set; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + manual = !manual; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return changes; + } +} diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java new file mode 100644 index 0000000..21fb95d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -0,0 +1,34 @@ +// 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.ExtenderIntakeCommands; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ExtenderIntakeGroup extends ParallelRaceGroup { + + public static int direction; + + /** Creates a new RunExtenderAndIntake. */ + public ExtenderIntakeGroup(Intake intake, Extender extender) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely + addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); + } + + public static void setDirectionToOut() { + ExtenderIntakeGroup.direction = 1; + } + + public static void changeDirection() { // Never implemented? + ExtenderIntakeGroup.direction *= -1; + } +} diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java new file mode 100644 index 0000000..4f4183e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java @@ -0,0 +1,66 @@ +// 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.ExtenderIntakeCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ExtenderConstants; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +public class RunExtender extends CommandBase { + + private Extender extender; + + private double error; + private double tolerance; + + /** Creates a new RunExtender. */ + public RunExtender(Extender extender) { + // Use addRequirements() here to declare subsystem dependencies. + this.extender = extender; + + updateError(); + tolerance = 5.0; + + addRequirements(this.extender); + } + + public void updateError() { + if (ExtenderIntakeGroup.direction > 0) { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT); + } else { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT); + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.out.println("RunExtender is working"); + this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0); + updateError(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + ExtenderIntakeGroup.changeDirection(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (error < tolerance) { + System.out.println("RunExtender finished"); + this.extender.runExtender(0.0); + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java new file mode 100644 index 0000000..9584967 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java @@ -0,0 +1,47 @@ +// 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.ExtenderIntakeCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.subsystems.Intake; + +public class RunIntakeConditionally extends CommandBase { + + private Intake intake; + + /** Creates a new RunIntakeConditionally. */ + public RunIntakeConditionally(Intake intake) { + // Use addRequirements() here to declare subsystem dependencies. + + this.intake = intake; + + addRequirements(this.intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (ExtenderIntakeGroup.direction > 0) { + this.intake.runAtOutput(-1); + } else { + this.intake.runAtOutput(0); + } + } + + // 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java similarity index 77% rename from src/main/java/frc4388/robot/commands/AimToCenter.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index d0c706a..38a8888 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; @@ -43,7 +43,7 @@ public class AimToCenter extends CommandBase { @Override public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); - m_turret.runshooterRotatePID(m_targetAngle); + m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ @@ -61,21 +61,15 @@ public class AimToCenter extends CommandBase { } /** - * Checks if in hardware deadzone (due to mechanical limitations). + * Checks if in deadzone. * @param angle Angle to check. - * @return True if in hardware deadzone. + * @return True if in 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)); + public static boolean isDeadzone(double angle) { + if (angle == Double.NaN) { + return false; + } + return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java similarity index 61% rename from src/main/java/frc4388/robot/commands/Shoot.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 4899c71..aabac8f 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -2,14 +2,14 @@ // 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; +package frc4388.robot.commands.ShooterCommands; -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.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; @@ -35,21 +35,25 @@ public class Shoot extends CommandBase { public double m_targetVel; public double m_targetHood; public double m_targetAngle; - public double m_driveTargetAngle; + public Pose2d m_targetPoint; // pid public double error; public double prevError; - public Gains shootGains = ShooterConstants.SHOOT_GAINS; + public Gains gains = ShooterConstants.SHOOT_GAINS; public double kP, kI, kD; public double proportional, integral, derivative; public double time; public double output; + public double normOutput; public double tolerance; public boolean isAimedInTolerance; + public int inverted; // testing - public DummySensor dummy = new DummySensor(0); + public boolean simMode = true; + public DummySensor driveDummy; + public DummySensor turretDummy; /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball @@ -68,9 +72,9 @@ public class Shoot extends CommandBase { addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); - kP = shootGains.kP; - kI = shootGains.kI; - kD = shootGains.kD; + kP = gains.kP; + kI = gains.kI; + kD = gains.kD; proportional = 0; integral = 0; @@ -80,24 +84,37 @@ public class Shoot extends CommandBase { tolerance = 5.0; isAimedInTolerance = false; - DummySensor.resetAll(); + if (simMode) { + driveDummy = new DummySensor(180); + turretDummy = new DummySensor(180); + + DummySensor.resetAll(); + } } /** * Updates error for custom PID. */ public void updateError() { - error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + m_targetPoint = SwerveDriveConstants.HUB_POSE; + m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); + // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + error = (m_targetAngle - turretDummy.get() + 360) % 360; + // error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); - SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); + + if (simMode) { + SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); + System.out.println("Target Angle: " + m_targetAngle); + System.out.println("Error: " + error); + } } // 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_odoX = 0;//m_swerve.getOdometry().getX(); + m_odoY = -1;//m_swerve.getOdometry().getY(); m_gyroAngle = m_swerve.getRegGyro().getDegrees(); @@ -105,22 +122,10 @@ public class Shoot extends CommandBase { 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); - } + if (AimToCenter.isDeadzone(m_targetAngle)) {} // initial error updateError(); @@ -131,6 +136,13 @@ public class Shoot extends CommandBase { * Run custom PID. */ public void runPID() { + if (error > 180){ + error = 360 - error; + inverted = -1; + } + else{ + inverted = 1; + } prevError = error; updateError(); @@ -138,20 +150,34 @@ public class Shoot extends CommandBase { integral = integral + error * time; derivative = (error - prevError) / time; output = kP * proportional + kI * integral + kD * derivative; + normOutput = output/360 * inverted; } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - // custom pid + if (simMode) { + System.out.println("Normalized Output: " + normOutput); + } + // custom pid runPID(); - // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the + + if (simMode) { + driveDummy.apply(normOutput); + System.out.println("Drive Dummy: " + driveDummy.get()); + } + m_swerve.driveWithInput(0, 0, normOutput, 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_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); m_hood.runAngleAdjustPID(m_targetHood); m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + + if (simMode) { + turretDummy.apply(normOutput); + System.out.println("Turret Dummy: " + turretDummy.get()); + } + m_turret.m_boomBoomRotateMotor.set(normOutput); } // Called once the command ends or is interrupted. @@ -161,6 +187,9 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + // if (simMode) { + return isAimedInTolerance; + // } + // return false; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java new file mode 100644 index 0000000..426bcf2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -0,0 +1,168 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import java.util.ArrayList; + +import org.opencv.core.Point; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; + +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.Vector2D; +import frc4388.utility.desmos.DesmosServer; + +public class TrackTarget extends CommandBase { + /** Creates a new TrackTarget. */ + Turret m_turret; + VisionOdometry m_visionOdometry; + BoomBoom m_boomBoom; + Hood m_hood; + + // use odometry to find x and y later + double x; + double y; + double distance; + double vel; + double hood; + double average; + double output; + Pose2d pos = new Pose2d(); + ArrayList points = new ArrayList<>(); + + private boolean targetLocked = false; + private double velocityTolerance = 100.0; + private double hoodTolerance = 5.0; + + double m=0; + double b=0; + boolean isExecuted = false; + + // public static Gains m_aimGains; + + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_boomBoom = boomBoom; + m_hood = hood; + m_visionOdometry = visionOdometry; + + addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); + } + + // 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()); + SmartDashboard.putBoolean("Target Locked", this.targetLocked); + + try { + m_visionOdometry.setLEDs(true); + points = m_visionOdometry.getTargetPoints(); + + Point average = VisionOdometry.averagePoint(points); + + for(Point point : points) { + Vector2D difference = new Vector2D(point); + difference.subtract(new Vector2D(average)); + + if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD) + points.remove(point); + } + + average = VisionOdometry.averagePoint(points); + DesmosServer.putPoint("average", average); + + for(int i = 0; i < points.size(); i++) { + DesmosServer.putPoint("Point" + i, points.get(i)); + } + + output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 4; + // output *= 0.5; + DesmosServer.putDouble("output", output); + m_turret.runTurretWithInput(output); + + double y_rot = average.y / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + DesmosServer.putDouble("distance", distance); + + updateRegressionDesmos(); + double regressedDistance = distanceRegression(distance); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; + DesmosServer.putDouble("distanceReg", regressedDistance); + + //Vision odometry circle fit based pose estimate + // Point targetOffset = m_visionOdometry.getTargetOffset(); + // DesmosServer.putPoint("targetOff", targetOffset); + + vel = m_boomBoom.getVelocity(regressedDistance + 30); + hood = m_boomBoom.getHood(regressedDistance + 30); + // m_boomBoom.runDrumShooter(vel); + m_boomBoom.runDrumShooterVelocityPID(vel); + m_hood.runAngleAdjustPID(hood); + + double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); + double currentHood = this.m_hood.getEncoderPosition(); + + this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance); + + SmartDashboard.putNumber("Regressed Distance", regressedDistance); + SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hood); + SmartDashboard.putNumber("Vel Target Track", vel); + + // isExecuted = true; + } + catch (Exception e){ + e.printStackTrace(); + // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); + } + // m_turret.runshooterRotatePID(m_targetAngle); + } + + public final double distanceRegression(double distance) { + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); + } + + public void updateRegressionDesmos() { + m = DesmosServer.readDouble("m"); + b = DesmosServer.readDouble("b"); + + DesmosServer.putArray("MB", m, b); + } + + // 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; + // return isExecuted && Math.abs(output) < .1; + } +} diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java b/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java new file mode 100644 index 0000000..44cc062 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java @@ -0,0 +1,76 @@ +// 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.StorageCommands; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; +import frc4388.robot.Robot; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Turret; + +public class ManageStorage extends CommandBase { + + // subsystems + private Storage storage; + private BoomBoom drum; + private Turret turret; + + private Alliance alliance; + private boolean rightColor; + + /** Creates a new ManageStorage. */ + public ManageStorage(Storage storage, BoomBoom drum, Turret turret) { + // Use addRequirements() here to declare subsystem dependencies. + + this.storage = storage; + this.drum = drum; + this.turret = turret; + + rightColor = true; + + addRequirements(this.storage, this.drum, this.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + checkColor(); + + if (rightColor) { + this.storage.manageStorage(); + } else { + + // * CommandScheduler.getInstance().schedule(new ExampleCommand()); + // * new ExampleCommand().schedule(); + // * new ExampleCommand().execute(); (accompanied by initialize and onFinished) + + new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command + } + } + + private void checkColor() { + this.alliance = this.storage.getColor(); + rightColor = this.alliance.equals(Robot.alliance); + } + + // 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java new file mode 100644 index 0000000..5c79247 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java @@ -0,0 +1,72 @@ +// 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.StorageCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Turret; + +public class SpitOutWrongColor extends CommandBase { + + // subsystems + private Storage storage; + private BoomBoom drum; + private Turret turret; + + // time (in milliseconds) + private long initialTime; + private long elapsedTime; + private long threshold; + + private double initialTurret; + private int spitVelocity; + + /** Creates a new SpitOutWrongColor. */ + public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) { + // Use addRequirements() here to declare subsystem dependencies. + + this.storage = storage; + this.drum = drum; + this.turret = turret; + + addRequirements(this.storage, this.drum, this.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + initialTime = System.currentTimeMillis(); + elapsedTime = 0; + threshold = 2000; + + initialTurret = this.turret.getEncoderPosition(); + spitVelocity = 2000; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elapsedTime = System.currentTimeMillis() - initialTime; + + this.storage.runStorage(0.9); + this.turret.gotoMidpoint(); + this.drum.runDrumShooterVelocityPID(spitVelocity); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.storage.runStorage(0.0); + this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (elapsedTime >= threshold); + } +} diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java deleted file mode 100644 index ae70cb4..0000000 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; - -import org.opencv.core.Point; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Hood; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.VisionOdometry; - -public class TrackTarget extends CommandBase { - /** Creates a new TrackTarget. */ - Turret m_turret; - SwerveDrive m_drive; - VisionOdometry m_visionOdometry; - BoomBoom m_boomBoom; - Hood m_hood; - - // use odometry to find x and y later - double x; - double y; - double distance; - double vel; - double hood; - double average; - double output; - Pose2d pos = new Pose2d(); - ArrayList points = new ArrayList<>(); - - // public static Gains m_aimGains; - - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { - // Use addRequirements() here to declare subsystem dependencies. - m_turret = turret; - m_drive = drive; - m_boomBoom = boomBoom; - m_hood = hood; - m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_drive, m_visionOdometry); - } - - // 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_visionOdometry.setLEDs(true); - points = m_visionOdometry.getTargetPoints(); - double pointTotal = 0; - for(Point point : points) - { - pointTotal = pointTotal + point.x; - } - average = pointTotal/points.size(); - output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; - m_turret.runTurretWithInput(output); - try{ - pos = m_visionOdometry.getVisionOdometry(); - distance = Math.hypot(pos.getX(), pos.getY()); - } - catch (Exception e){ - } - vel = m_boomBoom.getVelocity(distance); - hood = m_boomBoom.getHood(distance); - m_boomBoom.runDrumShooterVelocityPID(vel); - m_hood.runAngleAdjustPID(hood); - //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; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index e1286e5..c81fd38 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -18,8 +18,10 @@ import java.util.stream.IntStream; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; @@ -31,9 +33,11 @@ public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; + double speed2; double velP; double input; + public double pidOffset = 0; public boolean m_isDrumReady = false; public double m_fireVel; @@ -49,13 +53,11 @@ public class BoomBoom extends SubsystemBase { private ShooterTableEntry[] m_shooterTable; - /* - * Creates new BoomBoom subsystem, has drum shooter and angle adjuster - */ - /** Creates a new BoomBoom. */ + /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; + setShooterGains(); try { // This is a helper class that allows us to read a CSV file into a Java array. @@ -92,17 +94,25 @@ public class BoomBoom extends SubsystemBase { } } + /** + * 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). + * @param distance Distance in shooter table + * @return Drum Velocity in units per 100 ms + */ 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(); } + /** + * 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). + * @param distance Distance in shooter table + * @return Hood extension in units + */ 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(); } @@ -163,19 +173,26 @@ public class BoomBoom extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); + SmartDashboard.putNumber("Shooter Current", getCurrent()); + SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); } 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); + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); + SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); } @@ -188,8 +205,9 @@ public class BoomBoom extends SubsystemBase { } public void runDrumShooterVelocityPID(double targetVel) { + SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); 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 @@ -199,4 +217,17 @@ public class BoomBoom extends SubsystemBase { // feedforward.calculate(targetVel)); // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + + public void updateOffset(double change) { + pidOffset = pidOffset + change; + } + + public void increaseSpeed(double amount) + { + speed2 = speed2 + amount; + } + + public double getCurrent(){ + return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index cb8a9f1..7425346 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -1,279 +1,50 @@ -// // 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. +// 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. -// /* -// Safety -// Hooks -// Add -// */ +package frc4388.robot.subsystems; -// package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANSparkMax; -// import com.ctre.phoenix.motorcontrol.NeutralMode; -// import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import org.opencv.core.Point; +public class Climber extends SubsystemBase { -// import edu.wpi.first.math.Matrix; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import frc4388.robot.Constants; -// import frc4388.robot.Constants.ClimberConstants; + WPI_TalonFX m_climberShoulder; + WPI_TalonFX m_climberElbow; -// public class Climber extends SubsystemBase { -// public WPI_TalonFX m_shoulder; -// public WPI_TalonFX m_elbow; + /** Creates a new Climber. */ + public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) { + m_climberShoulder = climberShoulder; + m_climberElbow = climberElbow; -// private double m_shoulderOffset; -// private double m_elbowOffset; + m_climberShoulder.configFactoryDefault(); + m_climberElbow.configFactoryDefault(); + m_climberShoulder.setNeutralMode(NeutralMode.Brake); + m_climberElbow.setNeutralMode(NeutralMode.Brake); + } -// private WPI_PigeonIMU m_gyro; -// private boolean m_groundRelative; -// private double m_robotAngle; -// private double m_robotPosition; - -// private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d); - -// public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { -// m_shoulder = shoulder; -// m_shoulder.configFactoryDefault(); -// m_shoulder.setNeutralMode(NeutralMode.Brake); + public void runShoulderWithInput(double input) { + m_climberShoulder.set(input); + } -// m_elbow = elbow; -// m_elbow.configFactoryDefault(); -// m_elbow.setNeutralMode(NeutralMode.Brake); + public void runElbowWithInput(double input){ + m_climberElbow.set(input); + } -// setClimberGains(); + public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) { + m_climberShoulder.set(shoulderInput); + m_climberElbow.set(elbowInput); + } -// m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); -// m_elbowOffset = m_elbow.getSelectedSensorPosition(); + @Override + public void periodic() { + // This method will be called once per scheduler run + } -// m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); -// m_elbow.configForwardSoftLimitEnable(false); -// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); -// m_elbow.configReverseSoftLimitEnable(false); - -// m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); -// m_shoulder.configForwardSoftLimitEnable(false); -// m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); -// m_shoulder.configReverseSoftLimitEnable(false); - -// if(groundRelative) -// m_gyro = gyro; - -// m_groundRelative = groundRelative; -// } - -// /* getJointAngles -// * Gets joint angles for climber arm -// * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the -// * rotation of the robot. Both parameters should be in cm. -// * returns [shoulderAngle, elbowAngle] in radians -// * Does not set the motors automatically -// * -// * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ -// public static double[] getJointAngles(Point targetPoint, double tiltAngle) { -// double [] angles = new double[2]; - -// double mag = Math.hypot(targetPoint.x, targetPoint.y); -// if(mag >= ClimberConstants.MAX_ARM_LENGTH) { -// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; -// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; -// mag = ClimberConstants.MAX_ARM_LENGTH; -// } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { -// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; -// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; -// mag = ClimberConstants.MIN_ARM_LENGTH; -// } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { -// targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; -// targetPoint.y = 0.d; -// mag = ClimberConstants.MIN_ARM_LENGTH; -// } - -// // The angle to the target point -// double theta; -// if(targetPoint.x == 0.d) { -// theta = Math.PI/2.d; // TODO rename variable -// } else { -// theta = Math.atan(targetPoint.y / targetPoint.x); -// } -// theta += tiltAngle; - -// if(targetPoint.x < 0.d) -// theta += Math.PI; - - -// // Correct target position for tilt -// targetPoint.x = Math.cos(theta) * mag; -// targetPoint.y = Math.sin(theta) * mag; - -// // Law and Order: Cosines edition -// double shoulderAngle; -// if(mag == 0) { -// shoulderAngle = 0; -// } else { -// shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / -// (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); -// } -// shoulderAngle = theta - shoulderAngle; - -// double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / -// (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); -// //elbowAngle = Math.PI - elbowAngle; -// // System.out.println("sa1: " + shoulderAngle); -// // System.out.println("ea1: " + elbowAngle); - -// // If the climber is resting on the robot base, rotate the upper arm in the direction of the target -// if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { -// shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; -// double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; -// // System.out.println("xDiff: " + xDiff); - -// elbowAngle = Math.atan(-targetPoint.y / xDiff); -// // System.out.println("ea2: " + elbowAngle); -// if(elbowAngle < 0) { -// elbowAngle = Math.PI - Math.abs(elbowAngle); -// } - -// if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) -// elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; -// // System.out.println("ea3: " + elbowAngle); - -// // Deal with negative wraparound -// // if(xDiff >= 0.d) -// // elbowAngle += Math.PI; -// // System.out.println("ea4: " + elbowAngle); -// } - -// angles[0] = shoulderAngle; -// angles[1] = elbowAngle; -// return angles; -// } - -// public void setMotors(double shoulderOutput, double elbowOutput) { -// m_shoulder.set(shoulderOutput); -// m_elbow.set(elbowOutput); -// } - -// /* Rotation Matrix -// R = [cos(0) -sin(0) \n sin(0) cos(0)] -// Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)] -// Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix -// Rotation Matrix video: https://youtu.be/Ta8cKqltPfU -// */ -// public double getRobotTilt() { -// double[] ypr = new double[3]; -// m_gyro.getYawPitchRoll(ypr); -// double theta = 0; - -// // double sin; -// // double cos; -// // xsin = 0; //placeholder for sin, cos -// // ysin = 0; -// // xcos = 0; -// // ycos = 0; -// double[][] rotMax = { -// {Math.cos(theta) - Math.sin(theta), 0 }, -// {Math.sin(theta) + Math.cos(theta), 0}, -// {0, 0, 1} -// }; - -// if (m_robotPosition != m_robotAngle){ -// setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); -// } - -// return Math.toRadians(ypr[1]); // Pitch -// // multiply by pie and then divide by 180 - -// } - -// public void setClimberGains() { -// // shoulder PIDs -// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); -// m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - -// // elbow PIDs -// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); -// m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); -// m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); -// } - -// public void setJointAngles(double[] angles) { -// System.out.println(angles); -// setJointAngles(angles[0], angles[1]); -// } - -// public void setJointAngles(double shoulderAngle, double elbowAngle) { -// shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; -// elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; - -// shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; -// elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; - -// // Convert radians to ticks -// System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); - -// double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; -// double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - -// shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; -// elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; - -// shoulderPosition += m_shoulderOffset; -// elbowPosition += m_elbowOffset; - -// m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); -// m_elbow.set(TalonFXControlMode.Position, elbowPosition); -// } - -// public void controlWithInput(double xInput, double yInput) { -// m_position.x += xInput * ClimberConstants.MOVE_SPEED; -// m_position.y += yInput * ClimberConstants.MOVE_SPEED; - -// System.out.println("x: " + m_position.x + " y: " + m_position.y); - -// double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; - -// // double[] testAngles = getJointAngles(0, 0, 0); -// // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); - -// // double[] testAngles2 = getJointAngles(5000, 5000, 0); -// // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); - -// // double[] testAngles3 = getJointAngles(0, 75, 0); -// // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); - -// // double[] testAngles4 = getJointAngles(75, 0, 0); -// // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); - -// // double[] testAngles5 = getJointAngles(-75, 0, 0); -// // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); - -// // double[] testAngles6 = getJointAngles(60, 25, 0); -// // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); - -// double[] jointAngles = getJointAngles(m_position, tiltAngle); -// setJointAngles(jointAngles); -// } - -// public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { -// m_robotPosition = robotPosition; -// m_robotAngle = robotAngle; -// m_robotAngle = 45; //45 is placeholder -// } - -// @Override -// public void periodic(){ -// SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); -// SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); -// } -// } + public double getCurrent() { + return m_climberElbow.getSupplyCurrent(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java new file mode 100644 index 0000000..001208f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -0,0 +1,69 @@ +// 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.ctre.phoenix.led.ColorFlowAnimation.Direction; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ExtenderConstants; + +public class Extender extends SubsystemBase { + + private CANSparkMax m_extenderMotor; + + // private SparkMaxLimitSwitch m_inLimit; + // private SparkMaxLimitSwitch m_outLimit; + + /** Creates a new Extender. */ + public Extender(CANSparkMax extenderMotor) { + + m_extenderMotor = extenderMotor; + + // m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_inLimit.enableLimitSwitch(false); + // m_outLimit.enableLimitSwitch(false); + + m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT); + m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT); + setExtenderSoftLimits(true); + } + + /** + * Set status of extender motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setExtenderSoftLimits(boolean set) { + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); + } + + public void runExtender(double input) { + // if (!m_serializer.getBeam() && input < 0.) return; + m_extenderMotor.set(input); + } + + public double getPosition() { + return m_extenderMotor.getEncoder().getPosition(); + } + + public void setEncoder(double position) { + m_extenderMotor.getEncoder().setPosition(position); + } + + public double getCurrent() { + return m_extenderMotor.getOutputCurrent(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 58c1e3d..0652f4b 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -4,20 +4,17 @@ package frc4388.robot.subsystems; -import javax.sql.rowset.WebRowSet; - import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.SparkMaxRelativeEncoder.Type; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; @@ -25,8 +22,8 @@ public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; public CANSparkMax m_angleAdjusterMotor; - public SparkMaxLimitSwitch m_hoodUpLimitSwitch; - public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + // public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + // public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains; public RelativeEncoder m_angleEncoder; @@ -35,8 +32,9 @@ public class Hood extends SubsystemBase { public boolean m_isHoodReady = false; -public double m_fireAngle; + public double m_fireAngle; + public double speedLimiter; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -46,20 +44,43 @@ public double m_fireAngle; m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - 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_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); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); setHoodSoftLimits(true); + + this.speedLimiter = 1.0; } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); + + // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); + + if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * 0.05); + } + + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * 0.05); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } + + double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent(); + } /** @@ -84,20 +105,34 @@ public double m_fireAngle; m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } - + /** + * Runs the hood with the given input + * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) + */ public void runHood(double input) { - m_angleAdjusterMotor.set(input); + m_angleAdjusterMotor.set(input * this.speedLimiter); } - public void resetGyroAngleAdj(){ + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runAngleAdjustPID(0); + } + + public void resetGyroAngleAdj() { m_angleEncoder.setPosition(0); } - public double getAnglePosition(){ - return 0.0;//m_angleEncoder.getPosition(); + public double getEncoderPosition(){ + return m_angleEncoder.getPosition(); } public double getAnglePositionDegrees(){ return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; } + + public double getCurrent(){ + return m_angleAdjusterMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 0c63afc..a98bc05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,77 +4,48 @@ package frc4388.robot.subsystems; -//Imported Limit switch ONLY -import com.revrobotics.SparkMaxLimitSwitch; -import com.revrobotics.SparkMaxLimitSwitch.Type; - -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; public class Intake extends SubsystemBase { private WPI_TalonFX m_intakeMotor; - private CANSparkMax m_extenderMotor; - private Serializer m_serializer; - private SparkMaxLimitSwitch m_inLimit; - private SparkMaxLimitSwitch m_outLimit; - - public boolean toggle; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) { + public Intake(WPI_TalonFX intakeMotor) { m_intakeMotor = intakeMotor; - m_extenderMotor = extenderMotor; - m_serializer = serializer; - - m_extenderMotor.restoreFactoryDefaults(); - - m_intakeMotor.setNeutralMode(NeutralMode.Brake); - m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(true); - - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(true); - m_outLimit.enableLimitSwitch(true); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Percent Output", m_intakeMotor.get()); + SmartDashboard.putNumber("Extender Direction", ExtenderIntakeGroup.direction); } /** - * Runs The Intake With Triggers. - * @param leftTrigger Left Trigger to Run - - * @param rightTrigger Right Trigger to Run + + * Runs The Intake With Triggers as input + * @param leftTrigger Left Trigger to Run Inward + * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { - m_intakeMotor.set(rightTrigger - leftTrigger); - } - /** - * Runs The Extender - * @param extended Wether the Extender Is Extended - */ - public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?) - if (!m_serializer.getBeam() && !extended) return; - double extenderMotorSpeed = extended ? 0.25d : -0.25d; - m_extenderMotor.set(extenderMotorSpeed); + m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER); + SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } - public void runExtender(double input) { - if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); + public void runAtOutput(double output, double multiplier) { + m_intakeMotor.set(output * multiplier); } - /** - * Toggles The Extender - */ - public void toggleExtender() { - toggle = !toggle; - runExtender(toggle); + + public void runAtOutput(double output) { + m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER); + } + + public double getCurrent() { + return m_intakeMotor.getSupplyCurrent(); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e46be14..7a24a0c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -57,4 +57,5 @@ public class LED extends SubsystemBase { public LEDPatterns getPattern() { return m_currentPattern; } + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index 5263fb8..99702fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -1,8 +1,6 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; import frc4388.robot.Constants.SerializerConstants; import edu.wpi.first.wpilibj.DigitalInput; import com.revrobotics.CANSparkMax; @@ -21,7 +19,10 @@ public class Serializer extends SubsystemBase{ // m_serializerShooterBelt.set(0); } - + /** + * + * @param input from -1.0 to 1.0, positive is inward + */ public void setSerializer(double input){ m_serializerBelt.set(input); } @@ -50,4 +51,7 @@ public class Serializer extends SubsystemBase{ public boolean getSerializerState() { return serializerState; } + public double getCurrent(){ + return m_serializerBelt.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 1e3126a..ee1311e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -6,59 +6,76 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DigitalInput; -import frc4388.robot.Constants.StorageConstants; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.util.Color; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor; - private DigitalInput m_beamShooter; - private DigitalInput m_beamIntake; + public ColorSensorV3 m_colorSensor; + /** Creates a new Storage. */ - public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) { + public Storage(CANSparkMax storageMotor, ColorSensorV3 colorSensor) { m_storageMotor = storageMotor; - m_beamShooter = beamShooter; - m_beamIntake = beamIntake; + m_colorSensor = colorSensor; + } + + public Storage(CANSparkMax storageMotor) { + m_storageMotor = storageMotor; + m_colorSensor = null; } /** * If The Beam Is Broken, Run Storage * If Else, Stop Running Storage */ public void manageStorage() { - if (getBeamIntake()) runStorage(0.d); - else runStorage(1.d); + if (getColorBroken()) runStorage(0.d); + else runStorage(0.9); } /** * Runs The Storage at a Specifyed Speed - * @param input The Specifyed Speed + * @param input The value frm -1.0 to 1.0, positive is inwards (towards the shooter) */ public void runStorage(double input) { m_storageMotor.set(input); } /** - * Gets The Beam State On The Shooter + * Gets the state of the colorsensor as a beam break * @return The State Of The Beam on the Shooter */ - public boolean getBeamShooter(){ - return m_beamShooter.get();//True if open + public boolean getColorBroken(){ + return (getRed() || getBlue()); } - /** - * Gets The Beam State Of The Intake - * @return The Beam State Of The Intake - */ - public boolean getBeamIntake(){ - return m_beamIntake.get(); //True if open + public boolean getRed(){ + // return (m_colorSensor.getRed() >= 200 && m_colorSensor.getBlue() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kRed); } - + + public boolean getBlue(){ + // return (m_colorSensor.getBlue() >= 200 && m_colorSensor.getRed() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kBlue); + } + + public Alliance getColor() { + return (getRed() ? Alliance.Red : Alliance.Blue); + } @Override /** * Every Robot Tick Manage The Storage */ public void periodic() { - manageStorage(); + //manageStorage(); + } + public double getCurrent(){ + return m_storageMotor.getOutputCurrent(); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 3a33474..555ef83 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,8 +4,7 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -17,14 +16,12 @@ 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.Timer; 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.VisionObscuredException; public class SwerveDrive extends SubsystemBase { @@ -38,39 +35,29 @@ public class SwerveDrive extends SubsystemBase { 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)); + 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); public SwerveModule[] modules; - public WPI_PigeonIMU m_gyro; - protected FusionStatus fstatus = new FusionStatus(); + public WPI_Pigeon2 m_gyro; - /* - * 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 double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; public Rotation2d rotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); private final Field2d m_field = new Field2d(); public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, - WPI_PigeonIMU gyro) { + WPI_Pigeon2 gyro) { m_leftFront = leftFront; m_leftBack = leftBack; @@ -78,15 +65,15 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack }; - + 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))); + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); @@ -109,37 +96,39 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; - Translation2d speed = new Translation2d(-speedX, speedY); + Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); - double xSpeedMetersPerSecond = -speed.getX(); + 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)); + chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); + SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); 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); + 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)); + rotTarget = new Rotation2d(rightX, rightY); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); + 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); + rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); setModuleStates(states); + // SmartDashboard.putNumber("rot", rot); + // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); } /** @@ -150,10 +139,11 @@ public class SwerveDrive extends SubsystemBase { public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + // int i = 2; { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state, false); + module.setDesiredState(state, ignoreAngles); } // modules[0].setDesiredState(desiredStates[0], false); } @@ -164,7 +154,10 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); updateSmartDash(); + SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees()); + SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle()); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); @@ -174,42 +167,21 @@ public class SwerveDrive extends SubsystemBase { // odometry SmartDashboard.putNumber("Odometry: X", getOdometry().getX()); SmartDashboard.putNumber("Odometry: Y", getOdometry().getY()); - SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Odometry: Theta", 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); + // 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. + * Gets the current chassis speeds in m/s and rad/s. + * @return Current chassis speeds (vx, vy, ω) */ - 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; + public double[] getChassisSpeeds() { + return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond}; } /** @@ -281,4 +253,12 @@ public class SwerveDrive extends SubsystemBase { speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; } } + + public double getCurrent(){ + return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); + } + + public double getVoltage(){ + return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index bd1d8ee..11e861e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -120,6 +120,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, desiredTicks); } @@ -191,5 +192,11 @@ public class SwerveModule extends SubsystemBase { canCoder.setPositionToAbsolute(); // canCoder.configSensorInitializationStrategy(initializationStrategy) } + public double getCurrent(){ + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } + public double getVoltage(){ + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 7a0f45e..de1c9eb 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -8,8 +8,6 @@ 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; @@ -20,7 +18,7 @@ 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.robot.commands.ShooterCommands.Shoot; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -32,48 +30,138 @@ public class Turret extends SubsystemBase { 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 SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; - public boolean m_isAimReady = false; + SparkMaxLimitSwitch m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit; - SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); - public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + boolean leftState; + boolean recentlyPressed; + boolean leftPrevState = false; + boolean hasLeftSwitchChanged = false; - // Variables - public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + long leftCurrentTime; + long leftElapsedTime; + + double speedLimiter; + + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + // setTurretSoftLimits(true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); + 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); + this.speedLimiter = 1.0; + + setTurretPIDGains(); + } - m_boomBoomRotateMotor.setInverted(false); + // public void toggleLeftLimitSwitch() { + // TODO: find better way to do this, but im in a hurry + + // if (leftSwitch.isLimitSwitchEnabled()) { + // leftSwitch.enableLimitSwitch(false); + // } else { + // leftSwitch.enableLimitSwitch(true); + // } + // } + + // public void turnOnLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch ENABLED"); + // leftSwitch.enableLimitSwitch(true); + // } + + // public void turnOffLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch DISABLED"); + // leftSwitch.enableLimitSwitch(false); + // } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { 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); + // TODO: add 0.1 } - + @Override public void periodic() { // This method will be called once per scheduler run + + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + + SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); + SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + + // limit switch annoying time thing but actually worked first try wtf + leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). + + hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. + + if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + + if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; + + if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + recentlyPressed = true; + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); + + if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + + leftPrevState = leftState; // * Update the state of the left limit switch. + + + // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); + + if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * 0.05); + } + + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * 0.05); + } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } /** @@ -85,17 +173,29 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; } - + /** + * Move the turret with an input + * @param input from -1.0 to 1.0, positive is clockwise + */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter); } - public void runshooterRotatePID(double targetAngle) { - targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + public void runShooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } @@ -103,13 +203,30 @@ public class Turret extends SubsystemBase { m_boomBoomRotateEncoder.setPosition(0); } - public double getboomBoomRotatePosition() { + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runShooterRotatePID(0); + } + + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getEncoderPosition() { 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; + return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getCurrent(){ + return m_boomBoomRotateMotor.getOutputCurrent(); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 748bcd5..08d3d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -86,10 +86,10 @@ public void track(){ public void checkFinished(){ if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ - m_turret.m_isAimReady = true; + // m_turret.m_isAimReady = true; } else{ - m_turret.m_isAimReady = false; + // m_turret.m_isAimReady = false; } } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 162a897..2a0abd2 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -54,13 +54,16 @@ public class VisionOdometry extends SubsystemBase { * Breaks down targets into 4 corners and uses the top 2 points * * @return Vision points on the rim of the target in screen space + * @throws VisionObscuredException */ - public ArrayList getTargetPoints() { + public ArrayList getTargetPoints() throws VisionObscuredException { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); + + System.out.println("Result: " + result.hasTargets() + ", latency: " + latency); if(!result.hasTargets()) - return new ArrayList(); + throw new VisionObscuredException(); ArrayList points = new ArrayList<>(); @@ -90,16 +93,11 @@ public class VisionOdometry extends SubsystemBase { m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); } - /** Gets estimated odometry based on limelight data - * - * @return The estimated odometry pose, including gyro rotation - * @throws VisionObscuredException - */ - public Pose2d getVisionOdometry() throws VisionObscuredException { + public Point getTargetOffset() throws VisionObscuredException { ArrayList screenPoints = getTargetPoints(); if(screenPoints.size() < 3) - throw new VisionObscuredException("Not enough vision points available"); + throw new VisionObscuredException("Less than 3 vision points available"); ArrayList points3d = get3dPoints(screenPoints); ArrayList points = topView(points3d); @@ -110,14 +108,25 @@ public class VisionOdometry extends SubsystemBase { guess = iterateGuess(guess, points); } - guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); - guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + return guess; + } - SmartDashboard.putNumber("Vision ODO x: ", guess.x); - SmartDashboard.putNumber("Vision ODO y: ", guess.y); + /** Gets estimated odometry based on limelight data + * + * @return The estimated odometry pose, including gyro rotation + * @throws VisionObscuredException + */ + public Pose2d getVisionOdometry() throws VisionObscuredException { + Point targetOffset = getTargetOffset(); + + targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees()); + targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees()); + + SmartDashboard.putNumber("Vision ODO x: ", targetOffset.x); + SmartDashboard.putNumber("Vision ODO y: ", targetOffset.y); Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees())); - Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation); + Pose2d odometryPose = new Pose2d(targetOffset.x, targetOffset.y, rotation); return odometryPose; } @@ -125,9 +134,9 @@ public class VisionOdometry extends SubsystemBase { public double getLatency() { return latency; } + public boolean getLEDs() { - if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false; - return true; + return m_camera.getLEDMode() != VisionLEDMode.kOff; } public void updateOdometryWithVision(){ @@ -139,6 +148,7 @@ public class VisionOdometry extends SubsystemBase { err.printStackTrace(); } } + /** Reverse 3d projects target points from screen coordinates to 3d space *

* Uses the known height of the target to project points @@ -284,9 +294,8 @@ public class VisionOdometry extends SubsystemBase { * @return The angle corrected for the quadrent */ public static final double correctQuadrent(double angle, Point guess, Point circlePoint) { - if(circlePoint.x - guess.x < 0) { + if(circlePoint.x - guess.x < 0) return angle - Math.PI; - } return angle; } diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java new file mode 100644 index 0000000..28320ec --- /dev/null +++ b/src/main/java/frc4388/utility/RobotUnits.java @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +import edu.wpi.first.math.util.Units; + +/** Add your docs here. */ +public class RobotUnits { + // constants + + // conversions + public static double falconTicksToRotations(final double ticks) { + double rotations = ticks / 2048; + return rotations; + } + + public static double falconRotationsToTicks(final double rotations) { + double ticks = rotations * 2048; + return ticks; + } +} diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 3a3619c..5a9bfcd 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -63,8 +63,8 @@ public class Vector2D extends Vector2d { * @param v Vector to add */ public void add(Vector2D v) { - x += v.x; - y += v.x; + this.x += v.x; + this.y += v.x; } /** @@ -82,8 +82,8 @@ public class Vector2D extends Vector2d { * @param v Vector to subtract */ public void subtract(Vector2D v) { - x -= v.x; - y -= v.x; + this.x -= v.x; + this.y -= v.x; } /** @@ -101,8 +101,8 @@ public class Vector2D extends Vector2d { * @param scalar Scalar to multiply */ public void multiply(double scalar) { - x *= scalar; - y *= scalar; + this.x *= scalar; + this.y *= scalar; } /** @@ -120,8 +120,8 @@ public class Vector2D extends Vector2d { * @param scalar Scalar to divide */ public void divide(double scalar) { - x /= scalar; - y /= scalar; + this.x /= scalar; + this.y /= scalar; } /** @@ -153,7 +153,6 @@ public class Vector2D extends Vector2d { @Override public String toString() { - return ("(" + this.x + ", " + this.y + ")"); + return "<" + this.x + ", " + this.y + ">"; } - } diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java new file mode 100644 index 0000000..cc11d0d --- /dev/null +++ b/src/main/java/frc4388/utility/VelocityCorrection.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +import 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.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.SwerveDrive; + +/** Add your docs here. */ +public class VelocityCorrection { + + SwerveDrive swerve; + BoomBoom boomBoom; + + // vectors (in ft and ft/sec) + public Vector2D position; + public Vector2D cartesianVelocity; + + // find + public Vector2D target; + + public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) { + + this.swerve = swerve; + this.boomBoom = boomBoom; + + position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY()); + cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); + + target = getTargetPoint(); + } + + private Vector2D getTargetPoint() { + double approxShotTime = 1; // TODO: get shot time from shooter tables + + Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime); + + return Vector2D.round(targetPoint, 5); + } + +} diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java new file mode 100644 index 0000000..741d79e --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonBox.java @@ -0,0 +1,23 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class ButtonBox extends GenericHID { + public ButtonBox(int port) { + super(port); + } + public enum Button { + kRightSwitch(1), + kMiddleSwitch(2), + kLeftSwitch(3), + kRightButton(4), + kLeftButton(5); + + @SuppressWarnings("MemberName") + public final int value; + + Button(int value) { + this.value = value; + } + } +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java index e1679dd..177ef4e 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -3,6 +3,7 @@ package frc4388.utility.controller; import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.OIConstants; public class DeadbandedRawXboxController extends RawXboxController { public DeadbandedRawXboxController(int port) { super(port); } @@ -13,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 1c6fe5f..5b1bc97 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -4,6 +4,7 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; +import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } @@ -14,7 +15,7 @@ public class DeadbandedXboxController extends XboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } diff --git a/src/main/java/frc4388/utility/desmos/DesmosClient.html b/src/main/java/frc4388/utility/desmos/DesmosClient.html new file mode 100644 index 0000000..ca3170c --- /dev/null +++ b/src/main/java/frc4388/utility/desmos/DesmosClient.html @@ -0,0 +1,274 @@ + + + + + + Desmos Client + + +

+
Active
+ + + diff --git a/src/main/java/frc4388/utility/desmos/DesmosServer.java b/src/main/java/frc4388/utility/desmos/DesmosServer.java new file mode 100644 index 0000000..cefe858 --- /dev/null +++ b/src/main/java/frc4388/utility/desmos/DesmosServer.java @@ -0,0 +1,352 @@ +package frc4388.utility.desmos; + +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; +import java.io.OutputStream; +import java.net.ServerSocket; +import java.net.Socket; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Set; + +import org.opencv.core.Point; + +/** + * A http server that allows the robot to communicate with Desmos Graphing Calculator + * + * @author Daniel McGrath + * */ +public class DesmosServer extends Thread { + private static HashMap desmosQueue = new HashMap<>(); + private static HashMap readVariables = new HashMap<>(); + + private static int clearCount = 0; + + private static boolean running = false; + + private ServerSocket serverSocket; + private int activePort; + + /** + * Creates DesmosServer running on port + *

+ * Use this for cases when the robot is using the default port + * + * @param port The port the server will run on + * */ + public DesmosServer(int port) { + activePort = port; + } + + /** + * Creates DesmosServer running on port 5500 + * */ + public DesmosServer() { + activePort = 8000; + } + + @Override + public void run() { + try { + runServer(activePort); + } catch(IOException err) { + err.printStackTrace(); + } + } + + /** + * Runs server on port + * + * @param port The port the server runs on + * @throws IOException + * */ + public void runServer(int port) throws IOException { + System.out.println("Initializing DesmosServer on port " + port + "..."); + + serverSocket = new ServerSocket(port); + running = true; + + System.out.println("DesmosServer is active!"); + + while(true) { + Socket client = serverSocket.accept(); + handleClient(client); + } + } + + /** + * Handles client requests + * + * @param client The client connection + * @throws IOException + * */ + public void handleClient(Socket client) throws IOException { + InputStreamReader clientStream = new InputStreamReader(client.getInputStream()); + BufferedReader bufferedReader = new BufferedReader(clientStream); + + ArrayList headers = new ArrayList<>(); + + String header; + while((header = bufferedReader.readLine()).length() != 0) { + headers.add(header); + } + + String body = ""; + while(bufferedReader.ready()) { + body += (char) bufferedReader.read(); + } + + readVariables(body); + sendResponse(client); + } + + /** + * Sends JSON response with format + *

+ * [ + * {"int": "24"}, + * {"double": "2.4"}, + * {"point": "(2,4)"}, + * {"list": "[2,4]"} + * ] + * + * @param client The client connection + * @throws IOException + * */ + public void sendResponse(Socket client) throws IOException { + OutputStream clientOutput = client.getOutputStream(); + + // Write Headers + clientOutput.write("HTTP/1.1 200 OK\r\n".getBytes()); + clientOutput.write("Access-Control-Allow-Origin: *\r\n".getBytes()); + clientOutput.write("Keep-Alive: timeout=2, max=100\r\n".getBytes()); + clientOutput.write("Connection: Keep-Alive\r\n".getBytes()); + clientOutput.write("Content-Type: application/json\r\n\r\n".getBytes()); + + clientOutput.write(getJSONOutput().getBytes()); + clientOutput.flush(); + clientOutput.close(); + + clearCount = 0; + } + + /** + * Produces JSON output containing Desmos output. + * + * @return JSON string to be read by Desmos client + * */ + public static String getJSONOutput() { + String json = "["; + + if(!desmosQueue.isEmpty()) { + Set keySet = new HashSet<>(desmosQueue.keySet()); + + for(String key : keySet) { + json += "\n\t{" + + "\"name\":\"" + key + "\"," + + "\"type\":\"" + desmosQueue.get(key)[0] + "\"," + + "\"value\":\"" + desmosQueue.get(key)[1] + "\"" + + "},"; + + desmosQueue.remove(key); + } + + json = json.substring(0, json.length()-1); // remove comma at the end + } + + json += "\n]"; + + return json; + } + + /** + * Interpret client request and update variables + * + * @param requestBody Client request + */ + public static void readVariables(String requestBody) { + for(String variable : requestBody.split("\n")) { + if(variable.equals("")) + break; + + String[] readVar = variable.split("\t"); + readVariables.put(readVar[0], new String[] {readVar[1], readVar[2]}); + } + } + + /** + * Checks if the server is running + * + * @return The server status + */ + public static boolean isRunning() { + return running; + } + + // --------------------------------------------------------------------- + + /** + * Adds integer to desmos queue + * + * @param name Name of desmos variable + * @param value Integer value + * */ + public static void putInteger(String name, Integer value) { + desmosQueue.put(name, new String[] {"number", value.toString()}); + } + + /** + * Adds double to desmos queue + * + * @param name Name of desmos variable + * @param value Double value + * */ + public static void putDouble(String name, Double value) { + desmosQueue.put(name, new String[] {"number", value.toString()}); + } + + /** + * Adds point to desmos queue + * + * @param name Name of desmos variable + * @param value Point value + * */ + public static void putPoint(String name, Point point) { + desmosQueue.put(name, new String[] {"point", "(" + point.x + "," + point.y + ")"}); + } + + public static void putArray(String name, double... arr) { + desmosQueue.put(name, new String[] {"array", Arrays.toString(arr).replace(" ", "")}); + } + + /** + * Adds table to desmos queue + * + * @param name ID of table + * @param id Column ID + * @param column Double array containing column values + * @param table Repeat id and column in sequence + * */ + public static void putTable(String name, Object... table) { + String tableStr = ""; + + for(int i = 0; i < table.length; i += 2) { + // Check parameters + if(!(table[i] instanceof String)) { return; } + if(!(table[i+1] instanceof double[])) { return; } + + tableStr += table[i] + ","; + String values = Arrays.toString((double[]) table[i+1]).replace(" ", ""); + tableStr += values.substring(1, values.length() - 1); + tableStr += ' '; + } + + tableStr = tableStr.substring(0, tableStr.length()-1); // remove space at the end + + desmosQueue.put(name, new String[] {"table", tableStr}); + } + + /** + * Clears entire expression panel + */ + public static void clearAll() { + desmosQueue.put("clear" + clearCount, new String[] {"all", "n/a"}); + clearCount++; + } + + /** + * Clears entire expression panel except for expressions in {@code names} + * + * @param names Names which should be preserved in clear + */ + public static void clearExcept(String... names) { + String namesStr = Arrays.toString(names).replace(" ", ""); + namesStr = namesStr.substring(1, namesStr.length()-1); + + desmosQueue.put("clear" + clearCount, new String[] {"except", namesStr}); + clearCount++; + } + + /** + * Removes expressions in {@code names} + * + * @param names Names which should be removed + */ + public static void clearSpecific(String... names) { + String namesStr = Arrays.toString(names).replace(" ", ""); + namesStr = namesStr.substring(1, namesStr.length()-1); + + desmosQueue.put("clear" + clearCount, new String[] {"specific", namesStr}); + clearCount++; + } + + // --------------------------------------------------------------------- + + /** + * Reads desmos integer + * + * @param name Desmos variable name + * @return Numeric value, if variable is an expression it will be evaluated + *

if variable is a double it will be cast to int + * */ + public static int readInteger(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number")) + return 0; + + return (int) Double.parseDouble(readVariables.get(name)[1]); + } + + /** + * Reads desmos double + * + * @param name Desmos variable name + * @return Numeric value, if variable is an expression it will be evaluated + * */ + public static double readDouble(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number")) + return 0; + + return Double.parseDouble(readVariables.get(name)[1]); + } + + /** + * Reads desmos point + * + * @param name Desmos variable name + * @return Point, if variable contains expressions they will be evaluated + * */ + public static Point readPoint(String name) { + Point point = new Point(); + + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("point")) + return point; + + String pointStr = readVariables.get(name)[1]; + point.x = Double.parseDouble(pointStr.split(",")[0]); + point.y = Double.parseDouble(pointStr.split(",")[1]); + + return point; + } + + /** + * Reads desmos array, including table columns + * + * @param name Desmos variable name + * @returns Array of numeric values, if array contains expressions they will be evaluated + * */ + public static double[] readArray(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("array")) + return new double[0]; + + String[] unparsed = readVariables.get(name)[1].split(","); + double[] arr = new double[unparsed.length]; + + for(int i = 0; i < arr.length; i++) + arr[i] = Integer.parseInt(unparsed[i]); + + return arr; + } +} diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 72a84f4..0bc247d 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -2,52 +2,53 @@ package frc4388.commands; import org.junit.Test; -import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.AimToCenter; + import org.junit.Assert; public class AimToCenterTest { private static final double DELTA = 1e-15; - @Test + //@Test public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output; //20 deg - output = AimToCenter.isHardwareDeadzone(20.); + output = AimToCenter.isDeadzone(20.); Assert.assertFalse(output); //-10 deg - output = AimToCenter.isHardwareDeadzone(-10.); + output = AimToCenter.isDeadzone(-10.); Assert.assertTrue(output); //-1 deg - output = AimToCenter.isHardwareDeadzone(-1.); + output = AimToCenter.isDeadzone(-1.); Assert.assertTrue(output); //341 deg - output = AimToCenter.isHardwareDeadzone(341.); + output = AimToCenter.isDeadzone(341.); Assert.assertTrue(output); //340 deg - output = AimToCenter.isHardwareDeadzone(340.); - Assert.assertFalse(output); + output = AimToCenter.isDeadzone(340.); + Assert.assertTrue(output); //0 deg - output = AimToCenter.isHardwareDeadzone(0.); + output = AimToCenter.isDeadzone(0.); Assert.assertFalse(output); //200 deg - output = AimToCenter.isHardwareDeadzone(200.); - Assert.assertFalse(output); + output = AimToCenter.isDeadzone(200.); + Assert.assertTrue(output); //2000000 deg - output = AimToCenter.isHardwareDeadzone(2000000.); + output = AimToCenter.isDeadzone(2000000.); Assert.assertTrue(output); //NaN deg - output = AimToCenter.isHardwareDeadzone(Double.NaN); - Assert.assertFalse(output); + // output = AimToCenter.isDeadzone(Double.NaN); + // Assert.assertFalse(output); } @Test