diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 5e4b222..b694df7 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,5 +1,12 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -96 ,-25.00 ,0.425 -144 ,-47.57 ,0.475 -192 ,-55.55 ,0.500 -240 ,-96.00 ,0.525 \ 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 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/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index ff9f904..b63994d 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ 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 15a0c51..650a28d 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; @@ -30,28 +33,31 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public static final double TICKS_PER_ROTATION_FX = 2048; + public static final double LOOP_TIME = 0.02; + public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4; - public static final double WIDTH = 23.5; - public static final double HEIGHT = 23.5; + public static final double ROTATION_SPEED = 2.0; + public static final double WIDTH = 23.75; + public static final double HEIGHT = 23.75; 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; // 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 @@ -65,10 +71,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 - 90 - 180) % 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; @@ -104,7 +110,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; @@ -112,6 +117,18 @@ 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; public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0)); @@ -129,12 +146,23 @@ public final class Constants { // CAN IDs 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 = 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.75; + public static final double STORAGE_SPEED = 0.9; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; @@ -142,14 +170,90 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } + public static final class ClimberConstants { + public static final int SHOULDER_ID = 30; + public static final int ELBOW_ID = 31; + public static final int GYRO_ID = 14; + + public static final double INPUT_MULTIPLIER = 1.0; + public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; + public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; + + // TODO Update this stuff too + public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm + public static final double LOWER_ARM_LENGTH = 27; + + public static final double MAX_ARM_LENGTH = 53; + public static final double MIN_ARM_LENGTH = 1; + + public static final double MOVE_SPEED = 30000; // ticks per second + + public static final double SHOULDER_RESTING_ANGLE = 0; + public static final double ELBOW_RESTING_ANGLE = 0; + + public static final double SHOULDER_MAX_ANGLE = 135; + public static final double ELBOW_MAX_ANGLE = 180; + + public static final double ELBOW_GB_RATIO = 1.d; + public static final double SHOULDER_GB_RATIO = 1.d; + + public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869; + public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0; + public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717; + public static final double ELBOW_REVERSE_SOFT_LIMIT = 0; + + // PID Constants + public static final int SHOULDER_POSITION_SLOT_IDX = 0; + public static final int SHOULDER_VELOCITY_SLOT_IDX = 0; + public static final int SHOULDER_PID_LOOP_IDX = 0; + + public static final int ELBOW_POSITION_SLOT_IDX = 0; + public static final int ELBOW_VELOCITY_SLOT_IDX = 0; + public static final int ELBOW_PID_LOOP_IDX = 0; + + public static final Gains SHOULDER_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); + public static final Gains ELBOW_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); + + public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(0.2, 0.0, 5.0, 0.0, 0, 0.2); + public static final Gains ELBOW_VELOCITY_GAINS = new Gains(0.3, 0.005, 5.0, 0.0, 0, 1.0); + + public static final int CLIMBER_TIMEOUT_MS = 50; + + public static final double THRESHOLD = 0; + + // TODO: Update Constants + // Robot Angle + public static final double ROBOT_ANGLE_ID = 0; + + } + + public static final class ClawConstants { + public static final int LEFT_CLAW_ID = 44; + public static final int RIGHT_CLAW_ID = 45; + + public static final double OPEN_POSITION = 0; // TODO: find actual position + public static final double CLOSE_POSITION = 1; // TODO: find actual position + + public static final double THRESHOLD = .1; // TODO: find actual threshold + + public static final double CALIBRATION_SPEED = 0; + + public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; + + public static final int TOP_LIMIT = 1800; + public static final int BOTTOM_LIMIT = 1200; + + } /** * The OIConstants class contains the ID for the XBox controllers */ 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 { @@ -159,36 +263,50 @@ 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 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.4d; - public static final int DEGREES_PER_ROT = 0; + public static final double TURRET_SPEED_MULTIPLIER = 0.4; + 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; + public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_SHOOT_GAINS = new Gains(6.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID public static final int TURRET_MOTOR_CAN_ID = 19; - 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 double TURRET_FORWARD_LIMIT = 61.7; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -42.3; // TODO: find + //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); - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values + public static final double TURRET_FORWARD_HARD_LIMIT = 18.429; + public static final double TURRET_REVERSE_HARD_LIMIT = -117.8; + + 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 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 = 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 double HOOD_FORWARD_LIMIT = 48.69; // TODO: find - public static final double HOOD_REVERSE_LIMIT = -100; // 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; } @@ -199,21 +317,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; //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 bd4b9fe..4af5fb5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.drive.Vector2d; +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; @@ -27,6 +28,7 @@ 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 @@ -41,10 +43,12 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + + private double current; - private SendableChooser odoChooser = new SendableChooser(); - private HashMap odoChoices = new HashMap<>(); - private Pose2d selectedOdo; + 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 @@ -116,6 +120,10 @@ public class Robot extends TimedRobot { return "Not Running"; } }); + + desmosServer.start(); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); + // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } /** @@ -130,8 +138,19 @@ 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 @@ -141,44 +160,8 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // velocity correction tests - VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); - - //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); - //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); - // System.out.println("Position: (" + vc.position.x + ", " + vc.position.y + ")"); - // System.out.println("Velocity: (" + vc.cartesianVelocity.x + ", " + vc.cartesianVelocity.y + ")"); - // System.out.println("Tangential Velocity: (" + vc.tangentialVelocity.x + ", " + vc.tangentialVelocity.y + ")"); - // System.out.println("Radial Velocity: (" + vc.radialVelocity.x + ", " + vc.radialVelocity.y + ")"); - - SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); - - // 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); - // print odometry data to smart dashboard for debugging (if causing timeout // errors, you can comment it) - SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); - } - - 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); - } } /** @@ -190,6 +173,18 @@ public class Robot extends TimedRobot { public void disabledInit() { LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); + if (isTest()) { + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") + .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); + if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + // m_robotContainer.createPath(null, null, false).write(outputFile); + LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); + } else + LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + } + + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override @@ -204,11 +199,7 @@ public class Robot extends TimedRobot { public void autonomousInit() { LOGGER.fine("autonomousInit()"); - selectedOdo = odoChooser.getSelected(); - if (selectedOdo == null) { - selectedOdo = m_robotContainer.getOdometry(); - } - m_robotContainer.resetOdometry(selectedOdo); + Robot.alliance = DriverStation.getAlliance(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -229,13 +220,8 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); - m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); - selectedOdo = odoChooser.getSelected(); - if (selectedOdo == null) { - selectedOdo = m_robotContainer.getOdometry(); - } - m_robotContainer.resetOdometry(selectedOdo); + Robot.alliance = DriverStation.getAlliance(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -252,8 +238,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 3ce7eca..2182819 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,8 +4,12 @@ package frc4388.robot; +import java.util.ArrayList; +import java.util.Objects; import java.util.logging.Logger; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; @@ -15,7 +19,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -26,13 +29,22 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.PathRecorder; +// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Claws; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Serializer; import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -44,26 +56,45 @@ import frc4388.utility.controller.DeadbandedXboxController; */ public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - /* RobotMap */ + + // RobotMap public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ + public final Climber m_robotClimber = new Climber(m_robotMap.elbow); + public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + 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); - // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);*/ - - /* Controllers */ - private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Autonomous */ private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); + // Controllers + private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); + + public static boolean softLimits = true; + + // control mode switching + public static enum ControlMode { SHOOTER, CLIMBER }; + public static ControlMode currentControlMode = ControlMode.SHOOTER; + + // turret mode switching + private enum TurretMode { MANUAL, AUTONOMOUS }; + private TurretMode currentTurretMode = TurretMode.MANUAL; + + // climber mode switching + private enum ClimberMode { MANUAL, AUTONOMOUS }; + private ClimberMode currentClimberMode = ClimberMode.MANUAL; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -71,36 +102,60 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); /* Default Commands */ - - // Swerve Drive with Input + // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true), - m_robotSwerveDrive)); - // Intake with Triggers + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), + getDriverController().getLeftY(), + getDriverController().getRightX(), + getDriverController().getRightY(), + true); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + m_robotSwerveDrive.driveWithInput( 0, + 0, + 0, + 0, + true); + }} + , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), - m_robotIntake)); - // 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(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)); - m_robotHood.setDefaultCommand(new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY() * 0.1), m_robotHood)); - // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); + m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - // 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")); + // Serializer Manual + m_robotSerializer.setDefaultCommand( + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - // Creates a button on the SmartDashboard that will record the path of the robot. - SmartDashboard.putData("Path Recording", m_pathChooser); + // Turret Manual + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); + + // Hood Manual + m_robotHood.setDefaultCommand( + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + }, m_robotHood)); + + //Climber Manual + m_robotClimber.setDefaultCommand( + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } + }, m_robotClimber)); + + // autoInit(); + // recordInit(); } /** @@ -117,7 +172,7 @@ public class RobotContainer { if (binding == XboxController.Button.kLeftBumper) /* Left Bumper > Shift Down */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); else if (binding == XboxController.Button.kRightBumper) - /* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + /* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); else if (binding == XboxController.Button.kLeftStick) /* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kRightStick) @@ -127,45 +182,133 @@ public class RobotContainer { else if (binding == XboxController.Button.kB) /* B > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kX) - /* X > TEMP */ button.whenPressed(() -> { - m_robotMap.leftFront.reset(); - m_robotMap.rightFront.reset(); - m_robotMap.leftBack.reset(); - m_robotMap.rightBack.reset(); - }); + /* X > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kY) /* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kBack) - /* Back > Reset Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + /* Back > Calibrate Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); else if (binding == XboxController.Button.kStart) - /* Start > Reset Gyro */ button.whenPressed(m_robotSwerveDrive::resetGyro); + /* Start > Calibrate Odometry */ button.whenPressed(m_robotSwerveDrive::resetGyro); /* ------------------------------------ Operator ------------------------------------ */ button = new JoystickButton(getDriverController(), binding.value); if (binding == XboxController.Button.kLeftBumper) - /* Left Bumper > Storage Out */ button.whileHeld(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED), m_robotStorage) - .whenReleased(() -> m_robotStorage.runStorage(0.0), m_robotStorage); + /* Left Bumper > Storage In */ + button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); else if (binding == XboxController.Button.kRightBumper) - /* Right Bumper > Storage In */ button.whileHeld(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) - .whenReleased(() -> m_robotStorage.runStorage(0.0), m_robotStorage); + /* Right Bumper > Storage Out */ + button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); else if (binding == XboxController.Button.kLeftStick) /* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kRightStick) /* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kA) - /* A > Unbound */ button.whenPressed(new PrintCommand("Unbound")); + /* A > Spit Out Ball */ + button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret)) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); else if (binding == XboxController.Button.kB) - /* B > Reset Hood*/ button.whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); + /* B > Toggle Claws */ button.whenPressed(new InstantCommand(m_robotClaws::toggleClaws, m_robotClaws)); else if (binding == XboxController.Button.kX) - /* X > Run Shooter */ button.whileHeld(() -> m_robotBoomBoom.runDrumShooter(0.3)) - .whenReleased(() -> m_robotBoomBoom.runDrumShooter(0.0)); + /* X > TEST BUTTON */ button.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + ///* X > Toggles extender in and out */ button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); else if (binding == XboxController.Button.kY) - /* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound")); + /* Y > TEST BUTTON */ button.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + ///* Y > Full aim command */ button.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); else if (binding == XboxController.Button.kBack) /* Back > Unbound */ button.whenPressed(new PrintCommand("Unbound")); else if (binding == XboxController.Button.kStart) /* Start > Unbound */ button.whenPressed(new PrintCommand("Unbound")); } + // Iterate over all button box buttons. + for (ButtonBox.Button binding : ButtonBox.Button.values()) { + /* ---------------------------------- Button Box ---------------------------------- */ + JoystickButton button = new JoystickButton(getButtonBox(), binding.value); + if (binding == ButtonBox.Button.kRightSwitch) + /* Right Switch > Unbound */ button.whenPressed(new PrintCommand("Unbound")); + else if (binding == ButtonBox.Button.kMiddleSwitch) + /* Middle Switch > Climber and Shooter mode switching */ + button.whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); + else if (binding == ButtonBox.Button.kLeftSwitch) + /* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */ + button.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + else if (binding == ButtonBox.Button.kRightButton) + /* Right Button > Extender Out */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + else if (binding == ButtonBox.Button.kLeftButton) + /* Left Button > Extender In */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + } + } + + /** + * Generate autonomous + * @param maxVel max velocity for the path (null to override default value of 5.0) + * @param maxAccel max acceleration for the path (null to override default value of 5.0) + * @param inputs strings (paths) or commands you want to run (in order) + * @return array of commands + */ + public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY); + maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); + + ArrayList commands = new ArrayList(); + + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + // parse input + for (int i=0; i m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); + commands.add(new PPSwerveControllerCommand( + traj, + m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.m_kinematics, + xController, + yController, + thetaController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive)); + } + if (inputs[i] instanceof Command) { + commands.add((Command) inputs[i]); + } + } + + commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + Command[] ret = new Command[commands.size()]; + ret = commands.toArray(ret); + return ret; } /** @@ -174,29 +317,40 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - if (m_pathChooser.getPath() != null) { - PIDController xController = SwerveDriveConstants.X_CONTROLLER; - PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - thetaController.enableContinuousInput(-Math.PI, Math.PI); + // if (loadedPathTrajectory != null) { + // PIDController xController = SwerveDriveConstants.X_CONTROLLER; + // PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); - PathPlannerState initialState = m_pathChooser.getPath().getInitialState(); - Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); - return new SequentialCommandGroup( - new InstantCommand(m_robotSwerveDrive.m_gyro::reset), - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), - new PPSwerveControllerCommand(m_pathChooser.getPath(), m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, - m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), - new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); - } else { - LOGGER.severe("No auto selected."); - return new RunCommand(() -> { - }).withName("No Autonomous Path"); - } + // PathPlannerState initialState = loadedPathTrajectory.getInitialState(); + // Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + // return new SequentialCommandGroup( + // new InstantCommand(m_robotSwerveDrive.m_gyro::reset), + // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), + // new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, + // m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + // new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); + // } else { + // LOGGER.severe("No auto selected."); + // return new RunCommand(() -> { + // }).withName("No Autonomous Path"); + // } + + // ! this will run each of the specified PathPlanner paths in sequence. + // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")); + + // ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths. + // * return new ParallelCommandGroup(buildAuto(null, + // * null, + // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), + // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); + + return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command } - public XboxController getDriverController() { + public static XboxController getDriverController() { return m_driverXbox; } @@ -204,6 +358,14 @@ public class RobotContainer { return m_operatorXbox; } + public ButtonBox getButtonBox() { + return m_buttonBox; + } + + public static void setSoftLimits(boolean set) { + softLimits = set; + } + /** * Get odometry. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0e13aac..19a8264 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,20 +4,29 @@ package frc4388.robot; + +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +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_PigeonIMU; - +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; +import frc4388.robot.Constants.ClimberConstants; +import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; @@ -36,6 +45,11 @@ public class RobotMap { // configureLEDMotorControllers(); configureSwerveMotorControllers(); configureShooterMotorControllers(); + configureIntakeMotors(); + configureExtenderMotors(); + configureSerializerMotors(); + configureStorageMotors(); + configureClimberMotors(); } /* LED Subsystem */ @@ -43,30 +57,30 @@ public class RobotMap { // void configureLEDMotorControllers() {} - /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); +/* Swerve Subsystem */ +public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); +public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); +public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); +public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); +public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); +public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); +public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); +public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); +public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); +public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); +public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); - public SwerveModule leftFront; - public SwerveModule leftBack; - public SwerveModule rightFront; - public SwerveModule rightBack; - - void configureSwerveMotorControllers() { +public SwerveModule leftFront; +public SwerveModule leftBack; +public SwerveModule rightFront; +public SwerveModule rightBack; +void configureSwerveMotorControllers() { + leftFrontSteerMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault(); rightFrontSteerMotor.configFactoryDefault(); @@ -75,106 +89,139 @@ public class RobotMap { leftBackWheelMotor.configFactoryDefault(); rightBackSteerMotor.configFactoryDefault(); rightBackWheelMotor.configFactoryDefault(); - + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + NeutralMode mode = NeutralMode.Coast; - leftFrontSteerMotor.setNeutralMode(mode); + leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); leftFrontWheelMotor.setNeutralMode(mode);// Coast - rightFrontSteerMotor.setNeutralMode(mode); + rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); rightFrontWheelMotor.setNeutralMode(mode);// Coast - leftBackSteerMotor.setNeutralMode(mode); + leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); leftBackWheelMotor.setNeutralMode(mode);// Coast - rightBackSteerMotor.setNeutralMode(mode); + rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(mode);// Coast - + + // current limits + + leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + + leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, - SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, - SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, - SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, - SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - + SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // config cancoder as remote encoder for swerve steer motors leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); +} - // Shooter Config - /* Boom Boom Subsystem */ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); +/* Climb Subsystem */ +public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO - // turret subsystem - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); +private void configureClimberMotors() { + elbow.configFactoryDefault(); + elbow.setNeutralMode(NeutralMode.Brake); +} +/* Hooks Subsystem */ +public final Servo leftClaw = new Servo(1); +public final Servo rightClaw = new Servo(2); - // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); +// Shooter Config +/* Boom Boom Subsystem */ +public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); +public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // Create motor CANSparkMax - void configureShooterMotorControllers() { +// turret subsystem +public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); +// hood subsystem +public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + +// Create motor CANSparkMax +void configureShooterMotorControllers() { + // LEFT FALCON shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); @@ -183,55 +230,76 @@ public class RobotMap { shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + // RIGHT FALCON - 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, // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.follow(shooterFalconLeft); - - // /* Turret 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 - + + // turret + shooterTurret.restoreFactoryDefaults(); + shooterTurret.setIdleMode(IdleMode.kBrake); + shooterTurret.setInverted(true); + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - } - - - - /* 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 Subsytem */ - 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); + angleAdjusterMotor.setInverted(true); +} + +/* Serializer Subsystem */ +public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); +public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); + +/* Intake Subsystem */ +public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); +public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + +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/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java deleted file mode 100644 index 4cdce03..0000000 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ /dev/null @@ -1,85 +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 edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.VisionOdometry; - -public class AimToCenter extends CommandBase { - /** Creates a new AimWithOdometry. */ - Turret m_turret; - SwerveDrive m_drive; - VisionOdometry m_visionOdometry; - - // use odometry to find x and y later - double x; - double y; - double m_targetAngle; - - // public static Gains m_aimGains; - - public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) { - // Use addRequirements() here to declare subsystem dependencies. - m_turret = turret; - m_drive = drive; - 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_turret.runshooterRotatePID(m_targetAngle); - - // Check if limelight is within range (comment out to disable vision odo) - if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ - m_visionOdometry.updateOdometryWithVision(); - m_visionOdometry.setLEDs(true); - } - else{ - m_visionOdometry.setLEDs(false); - } - } - - public static double angleToCenter(double x, double y, double gyro) { - double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) - return angle; - } - - /** - * Checks if in deadzone. - * @param angle Angle to check. - * @return True if in deadzone. - */ - public static boolean isDeadzone(double angle) { - if (angle == Double.NaN) { - return false; - } - return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT)); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} 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/ClimberCommands/RunClaw.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java new file mode 100644 index 0000000..fdcd151 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java @@ -0,0 +1,53 @@ +// 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.ClimberCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ClawConstants; +import frc4388.robot.subsystems.Claws; + +public class RunClaw extends CommandBase { + + // parameters + public Claws m_claws; + public Claws.ClawType clawType; + public boolean open; + + /** + * Creates a new RunClaw, which runs a claw. + * @param sClaws Claws subsystem. + * @param which Which claw to run. + * @param open Whether to open or close the claw. + */ + public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) { + // Use addRequirements() here to declare subsystem dependencies. + m_claws = sClaws; + clawType = which; + this.open = open; + + addRequirements(m_claws); + } + + // 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() { + // m_claws.runClaw(clawType, open); + } + + // 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 m_claws.checkSwitchAndCurrent(clawType); + return false; // TODO: real return + } +} diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java new file mode 100644 index 0000000..84f66d1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -0,0 +1,74 @@ +// 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.ClimberCommands; + +import org.opencv.core.Point; + +import edu.wpi.first.wpilibj.drive.Vector2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ClimberConstants; +import frc4388.robot.subsystems.Claws; +import frc4388.robot.subsystems.Climber; +import frc4388.utility.Vector2D; + +public class RunClimberPath extends CommandBase { + Climber climber; + Claws claws; + + Point[] path; + int nextIndex; + + boolean endPath; + + /** Creates a new RunClimberPath. */ + public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) { + path = _path; + endPath = false; + + climber = _climber; + claws = _claws; + addRequirements(climber, claws); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + claws.setOpen(true); + nextIndex = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // if(!claws.fullyOpen()) + return; + + // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); + + // Vector2D dir = new Vector2D(path[nextIndex]); + // dir.subtract(new Vector2D(climberPos)); + + // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) + // nextIndex++; + // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { + // endPath = true; + // claws.setOpen(false); + // } else if(!endPath) { + // dir = dir.unit(); + // climber.controlWithInput(dir.x, dir.y); + // } + } + + // 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 endPath && claws.fullyClosed(); + return false; + } +} 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/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java deleted file mode 100644 index 9755bc5..0000000 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ /dev/null @@ -1,195 +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 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; -import frc4388.robot.subsystems.Turret; -import frc4388.utility.DummySensor; -import frc4388.utility.Gains; - -public class Shoot extends CommandBase { - - // subsystems - public SwerveDrive m_swerve; - public BoomBoom m_boomBoom; - public Turret m_turret; - public Hood m_hood; - - // given - public double m_gyroAngle; - public double m_odoX; - public double m_odoY; - public double m_distance; - - // targets - public double m_targetVel; - public double m_targetHood; - public double m_targetAngle; - public Pose2d m_targetPoint; - - // pid - public double error; - public double prevError; - 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 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 - * TODO: Velocity Correction - * @param sDrive Drive Train - * @param sShooter Shooter Drum - * @param sTurret Shooter Turret - * @param sHood Shooter Hood - */ - public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { - // Use addRequirements() here to declare subsystem dependencies. - m_swerve = sDrive; - m_boomBoom = sShooter; - m_turret = sTurret; - m_hood = sHood; - - addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); - - kP = gains.kP; - kI = gains.kI; - kD = gains.kD; - - proportional = 0; - integral = 0; - derivative = 0; - time = 0.02; - - tolerance = 5.0; - isAimedInTolerance = false; - - if (simMode) { - driveDummy = new DummySensor(180); - turretDummy = new DummySensor(180); - - DummySensor.resetAll(); - } - } - - /** - * Updates error for custom PID. - */ - public void updateError() { - 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); - - 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 = 0;//m_swerve.getOdometry().getX(); - m_odoY = -1;//m_swerve.getOdometry().getY(); - - m_gyroAngle = m_swerve.getRegGyro().getDegrees(); - - // get targets (shooter tables) - m_targetVel = m_boomBoom.getVelocity(m_distance); - m_targetHood = m_boomBoom.getHood(m_distance); - - m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; - - // deadzone processing - if (AimToCenter.isDeadzone(m_targetAngle)) {} - - // initial error - updateError(); - System.out.println("Error: " + error); - prevError = error; - } - /** - * Run custom PID. - */ - public void runPID() { - if (error > 180){ - error = 360 - error; - inverted = -1; - } - else{ - inverted = 1; - } - prevError = error; - updateError(); - - proportional = error; - 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() { - if (simMode) { - System.out.println("Normalized Output: " + normOutput); - } - // custom pid - runPID(); - - 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_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. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (simMode) { - return isAimedInTolerance; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java new file mode 100644 index 0000000..5fe6a4e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -0,0 +1,118 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +public class AimToCenter extends CommandBase { + /** Creates a new AimWithOdometry. */ + Turret m_turret; + VisionOdometry m_visionOdometry; + + Supplier supplier; + Pose2d odo; + + // use odometry to find x and y later + double x; + double y; + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier supplier) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_visionOdometry = visionOdometry; + + this.supplier = supplier; + + addRequirements(m_turret, m_visionOdometry); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + odo = this.supplier.get(); + // ! Yes I realize this stupid, yes it works I promise, coordinate system is funky + x = -odo.getY(); + y = -odo.getX(); + + SmartDashboard.putNumber("trans x", x); + SmartDashboard.putNumber("trans y", y); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + odo = this.supplier.get(); // * update odometry using really cool supplier -aarav + + m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360; + SmartDashboard.putNumber("Target Angle", m_targetAngle); + m_turret.runShooterRotatePID(m_targetAngle); + + // Check if limelight is within range (comment out to disable vision odo) + if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ + // m_visionOdometry.updateOdometryWithVision(); + // m_visionOdometry.setLEDs(true); + } + else{ + // m_visionOdometry.setLEDs(false); + } + } + + public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + // double angle = Math.toDegrees(Math.atan2(y, -x) - gyro); + return (angle - 360); + } + + public static double aaravAngleToCenter(double x, double y, double gyro) { + + double actualGyro = -gyro + 90; + + double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro; + if (-x > 0) { return (180 + exp); } + if (-x < 0) { return (360 + exp); } + + if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated + if (x == 0 && y < 0) { return (90 - actualGyro); } + + System.err.println("Invalid case."); + return 0; + } + + /** + * Checks if in deadzone. + * @param angle Angle to check. + * @return True if in deadzone. + */ + public static boolean isDeadzone(double angle) { + if (angle == Double.NaN) { + return false; + } + return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java new file mode 100644 index 0000000..73f82d0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Seek extends SequentialCommandGroup { + + /** Seeks. + * Seeking -> Sought + * @author Aarav Shah + * @blame Aarav Shah (thomas did this) + */ + public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry)); + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java new file mode 100644 index 0000000..7055573 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -0,0 +1,217 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import edu.wpi.first.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; +import frc4388.robot.Robot; +import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.DummySensor; +import frc4388.utility.Gains; + +public class Shoot extends CommandBase { + + // subsystems + private SwerveDrive swerve; + private Turret turret; + private BoomBoom drum; + private Hood hood; + private VisionOdometry visionOdometry; + + private boolean toShoot; + + // given + private double odoX, odoY; + private double distance; + private double gyroAngle; + + // targets + private double targetAngle, targetVel, targetHood; + + // pid + private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS; + private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS; + private double error; + private double prevError; + private double kP, kI, kD; + private double proportional, integral, derivative; + private double output, normOutput; + private double tolerance; + + boolean timerStarted; + long startTime; + private double timeTolerance; + + private boolean isAimedInTolerance; + private int inverted; + private double initialSwerveRotation; + + private boolean endsWithLimelight; + + /** + * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball + * + * @param swerve Drive Train + * @param drum Shooter Drum + * @param turret Shooter Turret + * @param hood Shooter Hood + * + * @author Aarav Shah + */ + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerve = swerve; + this.drum = drum; + this.turret = turret; + this.hood = hood; + this.visionOdometry = visionOdometry; + + this.toShoot = toShoot; + this.endsWithLimelight = endsWithLimelight; + + kP = turretGains.kP; + kI = turretGains.kI; + kD = turretGains.kD; + + proportional = 0; + integral = 0; + derivative = 0; + + tolerance = 10.0; + timeTolerance = 500; + isAimedInTolerance = false; + + this.inverted = 1; + + addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry); + } + + // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + // this(swerve, drum, turret, hood, false); + // } + + /** + * Updates error for custom PID. + */ + public void updateError() { + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + + // if (error > 180) { + // error = error - 360; + // this.inverted = -1; + // } else { + // this.inverted = 1; + // } + if (error > 180) { error = error - 360; } + + isAimedInTolerance = (Math.abs(error) <= tolerance); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timerStarted = false; + startTime = 0; + + this.odoX = -this.swerve.getOdometry().getY(); // 3.2766 + this.odoY = -this.swerve.getOdometry().getX(); // -0.9398 + + this.distance = Math.hypot(odoX, odoY); + + this.gyroAngle = this.swerve.getRegGyro().getDegrees(); + this.initialSwerveRotation = gyroAngle; + + // get targets (shooter tables) + this.targetVel = drum.getVelocity(distance); + this.targetHood = drum.getHood(distance); + + this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + // initial error + updateError(); + prevError = error; + } + + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); + + this.proportional = error; + this.integral = integral + (error * Constants.LOOP_TIME); + this.derivative = (error - prevError) / Constants.LOOP_TIME; + this.output = kP * proportional + kI * integral + kD * derivative; + this.normOutput = (output / 360); // inverted; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + runPID(); + + SmartDashboard.putNumber("Error", this.error); + SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); + SmartDashboard.putNumber("Normalized Output", this.normOutput); + + this.turret.runTurretWithCustomPID(normOutput); + this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative + + if (this.toShoot) { + this.hood.runAngleAdjustPID(this.targetHood); + this.drum.runDrumShooterVelocityPID(this.targetVel); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase()); + // ? return to initial swerve rotation + // swerve.driveWithInput(0, 0, initialSwerveRotation, true); + + // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); + // this.turret.m_boomBoomRotateMotor.set(0.0); + + // ? should stop the turret and the swerve + ////this.swerve.stopModules(); + this.turret.runTurretWithInput(0.0); + + if (this.toShoot) { + this.hood.runHood(0.0); + this.drum.runDrumShooter(0.0); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (!endsWithLimelight) { + if (isAimedInTolerance && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { + return this.visionOdometry.m_camera.getLatestResult().hasTargets(); + } + } +} 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..3c83331 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -0,0 +1,164 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.stream.Collector; +import java.util.stream.Collectors; + +import org.opencv.core.Point; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.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.Vision; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.Vector2D; +import frc4388.utility.desmos.DesmosServer; + +public class TrackTarget extends CommandBase { + /** Creates a new TrackTarget. */ + SwerveDrive m_swerve; + Turret m_turret; + VisionOdometry m_visionOdometry; + BoomBoom m_boomBoom; + Hood m_hood; + + static double velocity; + static double hoodPosition; + + ArrayList points = new ArrayList<>(); + + private boolean targetLocked = false; + private double velocityTolerance = 100.0; + private double hoodTolerance = 5.0; + + boolean isExecuted = false; + + public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + m_swerve = swerve; + 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() { + velocity = 0; + hoodPosition = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + try { + m_visionOdometry.setLEDs(true); + + points = m_visionOdometry.getTargetPoints(); + points = filterPoints(points); + Point average = VisionOdometry.averagePoint(points); + + double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2; + + m_turret.runTurretWithInput(output); + double position = m_turret.m_boomBoomRotateEncoder.getPosition(); + + if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || + Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + else + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + true); + + + double regressedDistance = getDistance(average.y); + + // ! add 30 to the distance to get in target. May need to be adjusted + velocity = m_boomBoom.getVelocity(regressedDistance + 30); + hoodPosition = m_boomBoom.getHood(regressedDistance + 30); + + m_boomBoom.runDrumShooterVelocityPID(velocity); + m_hood.runAngleAdjustPID(hoodPosition); + + double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); + double currentHood = this.m_hood.getEncoderPosition(); + + targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance); + + SmartDashboard.putNumber("Regressed Distance", regressedDistance); + // SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); + SmartDashboard.putNumber("Vel Target Track", velocity); + SmartDashboard.putBoolean("Target Locked", targetLocked); + } catch (Exception e){ + e.printStackTrace(); + } + } + + public ArrayList filterPoints(ArrayList points) { + Point average = VisionOdometry.averagePoint(points); + + HashMap pointDistances = new HashMap<>(); + double distanceSum = 0; + + for(Point point : points) { + Vector2D difference = new Vector2D(point); + difference.subtract(new Vector2D(average)); + + double mag = difference.magnitude(); + distanceSum += mag; + + pointDistances.put(point, mag); + } + + final double averageDist = distanceSum / points.size(); + return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); + } + + public final double getDistance(double averageY) { + double y_rot = averageY / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + + double regressedDistance = distanceRegression(distance); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; + + return regressedDistance; + } + + public final double distanceRegression(double distance) { + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // 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..df54662 --- /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 5013b28..0000000 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ /dev/null @@ -1,97 +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.util.ArrayList; - -import org.opencv.core.Point; - -import edu.wpi.first.math.geometry.Pose2d; -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.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); // has been changed for robot reveal night so this variable is percent output - hood = m_boomBoom.getHood(distance); - m_boomBoom.runDrumShooter(vel); - // m_boomBoom.runDrumShooterVelocityPID(vel); - m_hood.runAngleAdjustPID(hood); - //m_turret.runshooterRotatePID(m_targetAngle); - } - - // 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 eec2973..7a6cf7b 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -20,8 +20,10 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; +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; @@ -34,9 +36,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; @@ -52,48 +56,64 @@ 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(); + m_shooterTable = readShooterTable(); // Run a helper method that logs the contents of the table on a new thread. new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); } - public Double getVelocity(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). + /** + * 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) { return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } - public Double getHood(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). + /** + * 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) { return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } @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); + m_shooterFalconLeft.set(speed); + SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); } @@ -106,6 +126,7 @@ public class BoomBoom extends SubsystemBase { } public void runDrumShooterVelocityPID(double targetVel) { + SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init // New BoomBoom controller stuff @@ -151,4 +172,17 @@ public class BoomBoom extends SubsystemBase { return new ShooterTableEntry[] { dummyEntry }; } } + + 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/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java new file mode 100644 index 0000000..f5fcc1b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -0,0 +1,66 @@ +package frc4388.robot.subsystems; + +import java.nio.file.ClosedWatchServiceException; + +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClawConstants; +import frc4388.robot.Constants.ClimberConstants; + +public class Claws extends SubsystemBase { + + public Servo m_leftClaw; + public Servo m_rightClaw; + + // public CANSparkMax m_leftClaw; + // public CANSparkMax m_rightClaw; + + // private SparkMaxLimitSwitch m_leftLimitSwitchF; + // private SparkMaxLimitSwitch m_rightLimitSwitchF; + // private SparkMaxLimitSwitch m_leftLimitSwitchR; + // private SparkMaxLimitSwitch m_rightLimitSwitchR; + + private double m_leftOffset; + private double m_rightOffset; + + private boolean m_open; + public static enum ClawType {LEFT, RIGHT} + + public Claws(Servo leftClaw, Servo rightClaw) { + m_leftClaw = leftClaw; + m_rightClaw = rightClaw; + m_open = false; + } + + /** + * Sets the state of both hooks + * @param open The state of the hooks + */ + public void setOpen(boolean open) { + if(open) { + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100); + SmartDashboard.putBoolean("Claws Closed", false); + } else { + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400); + SmartDashboard.putBoolean("Claws Closed", true); + } + } + + public void toggleClaws() { + m_open = !m_open; + setOpen(m_open); + } + + + @Override + public void periodic() { + } +} \ 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 new file mode 100644 index 0000000..d16f830 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.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.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; +public class Climber extends SubsystemBase { + private WPI_TalonFX elbow; + + /** Creates a new Climber */ + public Climber(WPI_TalonFX elbow) { this.elbow = elbow; } + + public void setEncoders(double value) { this.elbow.setSelectedSensorPosition(value); } + public double getCurrent() { return this.elbow.getSupplyCurrent(); } + public void setMotors(double elbowOutput) { this.elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); } + + @Override + public void periodic() {} +} \ No newline at end of file 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 2305c86..7f81242 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -10,7 +10,9 @@ import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.SparkMaxRelativeEncoder.Type; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -20,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; @@ -30,8 +32,10 @@ public class Hood extends SubsystemBase { public boolean m_isHoodReady = false; -public double m_fireAngle; + public double m_fireAngle; + public double speedLimiter; + public double calibrationSpeed = 1.0; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -41,20 +45,50 @@ 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, (float) ShooterConstants.HOOD_FORWARD_LIMIT); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) 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). + runVelocityRamping(); + } + + public void runVelocityRamping() { + if (areSoftLimitsEnabled()) { + 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 * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } + } + } + + public boolean areSoftLimitsEnabled() { + return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); } /** @@ -79,20 +113,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 * this.calibrationSpeed); } - 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 fa11c34..a98bc05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,74 +4,48 @@ package frc4388.robot.subsystems; -//Imported Limit switch ONLY -import com.revrobotics.SparkMaxLimitSwitch; - -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.CANSparkMax; +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) * 0.3); - } - /** - * Runs The Extender- - * @param extended Wether the Extender Is Extended - */ - public void runExtender(boolean extended) { - 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 e3f2468..99702fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -19,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); } @@ -48,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 dcaddd7..ee1311e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -6,57 +6,67 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DigitalInput; +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_beamShooter = null; - m_beamIntake = null; + 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 /** @@ -65,4 +75,7 @@ public class Storage extends SubsystemBase { public void periodic() { //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 65860f5..6843e51 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,19 +4,15 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; -import com.ctre.phoenix.sensors.PigeonIMUConfiguration; -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.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -38,29 +34,19 @@ 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(halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight)); + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight)); 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 SwerveDriveOdometry m_odometry; public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -70,7 +56,7 @@ public class SwerveDrive extends SubsystemBase { 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,26 +64,25 @@ 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(), + getRegGyro(),//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()); + // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); m_gyro.reset(); SmartDashboard.putData("Field", m_field); } - // https://github.com/ZachOrr/MK3-Swerve-Example /** * Method to drive the robot using joystick info. - * + * @link https://github.com/ZachOrr/MK3-Swerve-Example * @param speeds[0] Speed of the robot in the x direction (forward). * @param speeds[1] Speed of the robot in the y direction (sideways). * @param rot Angular rate of the robot. @@ -105,41 +90,53 @@ public class SwerveDrive extends SubsystemBase { * field. */ public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { - if (speedX == 0 && speedY == 0 && rot == 0) - ignoreAngles = true; - else - ignoreAngles = false; - Translation2d speed = new Translation2d(-speedX, speedY); + ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0); + + 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(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED); + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) + : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) 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).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); + if (ignoreAngles) { + rot = 0; + } + 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); + // if (ignoreAngles) { + // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; + // for (int i = 0; i < states.length; i ++) { + // SwerveModuleState state = states[i]; + // lockedStates[i]= new SwerveModuleState(0, state.angle); + // } + // setModuleStates(lockedStates); + // } setModuleStates(states); + // SmartDashboard.putNumber("rot", rot); + // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); } /** @@ -150,6 +147,7 @@ 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]; @@ -164,9 +162,12 @@ 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()); + // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } @@ -174,7 +175,7 @@ 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 @@ -206,9 +207,12 @@ public class SwerveDrive extends SubsystemBase { * * @return Rotation2d object holding current gyro in radians */ - public Rotation2d getRegGyro() { - double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; - return new Rotation2d(regCur * Math.PI / 180); + public Rotation2d getRegGyro() { + // * test chassis regression + // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + // * new robot regression + double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743; + return new Rotation2d(Math.toRadians(regCur)); } /** @@ -260,4 +264,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..d4905af 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -38,6 +38,9 @@ public class SwerveModule extends SubsystemBase { public double m_currentPos; public double m_lastPos; + public SwerveModuleState lastState = new SwerveModuleState(); + public SwerveModuleState currentState; + /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; @@ -79,6 +82,7 @@ public class SwerveModule extends SubsystemBase { // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); + canCoderConfiguration.sensorCoefficient = 0.087890625; canCoderConfiguration.magnetOffsetDegrees = offset; canCoderConfiguration.sensorDirection = true; canCoder.configAllSettings(canCoderConfiguration); @@ -90,9 +94,7 @@ public class SwerveModule extends SubsystemBase { } private Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback - // coefficient - // and the sensor value reports degrees. + // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } @@ -120,6 +122,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, desiredTicks); } @@ -181,15 +184,26 @@ public class SwerveModule extends SubsystemBase { @Override public void periodic() { + + currentState = this.getState(); + Rotation2d currentRotation = getAngle(); SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); + + lastState = currentState; } public void reset() { 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 1a97942..3ebfabc 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -18,6 +18,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -29,48 +31,149 @@ 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; + + public double speedLimiter; + public double calibrationSpeed = 1.0; + + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit.enableLimitSwitch(true); - // m_boomBoomLeftLimit.enableLimitSwitch(true); - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + // 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); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); - setTurretSoftLimits(true); + this.speedLimiter = 1.0; + + setTurretPIDGains(); + } - m_boomBoomRotateMotor.setInverted(true); + // 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). + runVelocityRamping(); + } + + public void runVelocityRamping() { + if (areSoftLimitsEnabled()) { + + 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 * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } + } + } + + public boolean areSoftLimitsEnabled() { + return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); } /** @@ -82,17 +185,42 @@ 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 * this.calibrationSpeed); } - public void runshooterRotatePID(double targetAngle) { - targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + public void runTurretWithCustomPID(double input) { + m_boomBoomRotateMotor.set(input * this.speedLimiter); + } + + public void runShooterRotatePID(double targetAngle) { + + double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2; + + targetAngle = (targetAngle % 360); + + if (targetAngle > softMid) { + targetAngle = targetAngle - 360; + } + + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } @@ -100,13 +228,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..315623d 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -31,7 +31,7 @@ import frc4388.utility.VisionObscuredException; */ public class VisionOdometry extends SubsystemBase { // roborio ip & port: 10.43.88.2:1735 - private PhotonCamera m_camera; + public PhotonCamera m_camera; private SwerveDrive m_drive; private Turret m_shooter; @@ -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/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java new file mode 100644 index 0000000..5a9bfcd --- /dev/null +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -0,0 +1,158 @@ +// 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 java.security.InvalidParameterException; + +import org.opencv.core.Point; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.drive.Vector2d; + +/** Aarav's good vector class (better than WPILib) + * @author Aarav Shah */ + +public class Vector2D extends Vector2d { + + public double x; + public double y; + public double angle; + + public Vector2D() { + this(0, 0); + } + + public Vector2D(double x, double y) { + super(x, y); + + this.x = x; + this.y = y; + this.angle = Math.atan2(this.y, this.x); + } + + public Vector2D(double[] vec) { + this(vec[0], vec[1]); + + if (vec.length != 2) { + throw new InvalidParameterException(); + } + } + + public Vector2D(Point p) { + this(p.x, p.y); + } + + public Vector2D(Translation2d t) { + this(t.getX(), t.getY()); + } + + /** + * Add two vectors, component-wise. + * @param v1 First vector in the addition. + * @param v2 Second vector in the addition. + * @return New vector which is the sum. + */ + public static Vector2D add(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x + v2.x, v1.y + v2.y); + } + + /** + * Adds vector to current object + * @param v Vector to add + */ + public void add(Vector2D v) { + this.x += v.x; + this.y += v.x; + } + + /** + * Subtract two vectors, component-wise. + * @param v1 First vector in the subtraction. + * @param v2 Second vector in the subtraction. + * @return New vector which is the difference. + */ + public static Vector2D subtract(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x - v2.x, v1.y - v2.y); + } + + /** + * Subtracts vector from current object + * @param v Vector to subtract + */ + public void subtract(Vector2D v) { + this.x -= v.x; + this.y -= v.x; + } + + /** + * Multiply a vector with a scalar, component-wise. + * @param v1 Vector to multiply. + * @param v2 Scalar to multiply. + * @return New vector which is the product. + */ + public static Vector2D multiply(Vector2D v1, double scalar) { + return new Vector2D(scalar * v1.x, scalar * v1.y); + } + + /** + * Multiply a vector with a scalar, component-wise. + * @param scalar Scalar to multiply + */ + public void multiply(double scalar) { + this.x *= scalar; + this.y *= scalar; + } + + /** + * Divide a vector with a scalar, component-wise. + * @param v1 Vector to divide. + * @param v2 Scalar to divide. + * @return New vector which is the division. + */ + public static Vector2D divide(Vector2D v1, double scalar) { + return new Vector2D(v1.x / scalar, v1.y / scalar); + } + + /** + * Divide a vector with a scalar, component-wise. + * @param scalar Scalar to divide + */ + public void divide(double scalar) { + this.x /= scalar; + this.y /= scalar; + } + + /** + * Find unit vector. + * @return The unit vector. + */ + public Vector2D unit() { + return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude()); + } + + /** + * Round a vector to a certain number of places, component-wise. + * @param v Vector to round. + * @param places Number of places to round to. + * @return New rounded vector. + */ + public static Vector2D round(Vector2D v, int places) { + int scale = (int) Math.pow(10, places); + + v = Vector2D.multiply(v, scale); + + v.x = Math.round(v.x); + v.y = Math.round(v.y); + v.x = v.x / scale; + v.y = v.y / scale; + + return v; + } + + @Override + public String toString() { + return "<" + this.x + ", " + this.y + ">"; + } +} diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java index d7c6c0c..147657c 100644 --- a/src/main/java/frc4388/utility/VelocityCorrection.java +++ b/src/main/java/frc4388/utility/VelocityCorrection.java @@ -4,11 +4,6 @@ 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.wpilibj.drive.Vector2d; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.SwerveDrive; @@ -18,80 +13,30 @@ public class VelocityCorrection { SwerveDrive swerve; BoomBoom boomBoom; - // vectors - public Vector2d position; - public Vector2d cartesianVelocity; - public Vector2d tangentialVelocity, unitTangentialVelocity; - public Vector2d radialVelocity, unitRadialVelocity; - - double radius; + // vectors (in ft and ft/sec) + public Vector2D position; + public Vector2D cartesianVelocity; // find - public Vector2d target; + 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(1, 0);//new Vector2d(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); - - tangentialVelocity = getTangentialVelocity(); - radialVelocity = getRadialVelocity(); - - radius = position.magnitude(); + 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 getRadialVelocity() { - Vector2d ret = VelocityCorrection.subtract(cartesianVelocity, tangentialVelocity); - unitRadialVelocity = VelocityCorrection.multiply(ret, (1/ret.magnitude())); - return ret; - } - - private Vector2d getTangentialVelocity() { - - double hubElevation = Math.atan2(position.y, position.x); - unitTangentialVelocity = new Vector2d(Math.cos(hubElevation - (Math.PI/2)), Math.sin(hubElevation - (Math.PI/2))); - double tangentialVelocityMag = cartesianVelocity.scalarProject(unitTangentialVelocity); - - return VelocityCorrection.multiply(unitTangentialVelocity, tangentialVelocityMag); - } - - private Vector2d getTargetPoint() { - - double drumVelocity = boomBoom.getVelocity(radius); - double ballVelocity = drumVelocity; // TODO: convert from drum velocity to actual ball velocity - double approxShotTime = radius / ballVelocity; // TODO: better approximation to get shot time (physics/calculus?) - - Vector2d tangentialTargetPoint = VelocityCorrection.multiply(unitTangentialVelocity, -1 * approxShotTime); - Vector2d radialTargetPoint = VelocityCorrection.multiply(unitRadialVelocity, -1 * approxShotTime); - Vector2d targetPoint = VelocityCorrection.add(tangentialTargetPoint, radialTargetPoint); - - return targetPoint; - } - - public static Vector2d add(Vector2d v1, Vector2d v2) { - return new Vector2d(v1.x + v2.x, v1.y + v2.y); - } - - public static Vector2d subtract(Vector2d v1, Vector2d v2) { - return new Vector2d(v1.x - v2.x, v1.y - v2.y); - } - - public static Vector2d multiply(Vector2d v1, double scalar) { - return new Vector2d(scalar * v1.x, scalar * v1.y); - } - - public static Vector2d round(Vector2d v) { - v = VelocityCorrection.multiply(v, 1000); - v.x = Math.round(v.x); - v.y = Math.round(v.y); - v.x = v.x / 1000; - v.y = v.y / 1000; - return v; + 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 7f0b004..0b68a94 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -2,14 +2,19 @@ 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 + /** + * Unit tests the isDeadzone function in AimToCenter.java + * @author Ryan Manley + * @link www.hoohle.com + */ public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output;