From 4aedf1373c9c5046deca9ae96e16208aa478f05e Mon Sep 17 00:00:00 2001 From: aborunda <90010674+aborunda@users.noreply.github.com> Date: Thu, 21 Oct 2021 17:57:31 -0600 Subject: [PATCH] Try demo mode! --- .../java/frc4388/robot/DemoConstants.java | 244 ++++++++++++++++++ src/main/java/frc4388/robot/Robot.java | 8 + .../java/frc4388/robot/RobotContainer.java | 1 + .../commands/drive/DriveWithJoystick.java | 13 +- .../frc4388/robot/subsystems/Shooter.java | 25 +- 5 files changed, 277 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/DemoConstants.java diff --git a/src/main/java/frc4388/robot/DemoConstants.java b/src/main/java/frc4388/robot/DemoConstants.java new file mode 100644 index 0000000..38d57b8 --- /dev/null +++ b/src/main/java/frc4388/robot/DemoConstants.java @@ -0,0 +1,244 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class DemoConstants { + public static final int SELECTED_AUTO = 0; + + public static final class DriveConstants { + /* Drive Train IDs */ + public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; + public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; + public static final int DRIVE_LEFT_BACK_CAN_ID = 3; + public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; + public static final int PIGEON_ID = 6; + + /* Drive Inversions */ + public static final boolean isRightMotorInverted = true; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; + + /* Drive Configuration */ + public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Seconds from 0 to full power on motor + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(false, 60, 40, 2); + public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + public static final double COS_MULTIPLIER_LOW = 1.0; + public static final double COS_MULTIPLIER_HIGH = 0.8; + + /* Drive Train Characteristics */ + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 7.29; + public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; + public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; + public static final double TICKS_PER_MOTOR_REV = 2048; + + /* PID Constants Drive*/ + public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 30000; + public static final int DRIVE_ACCELERATION = 23000; + + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; + public static final int DRIVE_ACCELERATION_HIGH = 7000; + + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); + + /* Trajectory Constants */ + public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; + public static final double TRACK_WIDTH_METERS = 0.648; + public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); + + /* Remote Sensors */ + public final static int REMOTE_0 = 0; + public final static int REMOTE_1 = 1; + + /* PID Indexes */ + public final static int PID_PRIMARY = 0; + public final static int PID_TURN = 1; + + /* PID SLOTS */ + public final static int SLOT_DISTANCE = 0; + public final static int SLOT_VELOCITY = 1; + public final static int SLOT_TURNING = 2; + public final static int SLOT_MOTION_MAGIC = 3; + + /* Ratio Calculation */ + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS; + 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_ROT_PER_MOTOR_ROT_HIGH = 1/MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_HIGH = 1/TICKS_PER_INCH_HIGH; + + public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1/MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; + } + + public static final class ShooterConstants { + /* Motor IDs */ + public static final int SHOOTER_FALCON_BALLER_ID = 8; + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15; + public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final int SHOOTER_ROTATE_ID = 9; + + /* PID Constants Shooter */ + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); //Ff was 0.055 + //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); + 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 ENCODER_TICKS_PER_REV = 2048; + public static final double NEO_UNITS_PER_REV = 42; + public static final double DEGREES_PER_ROT = 360; + + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + + public static final int TURRET_RIGHT_SOFT_LIMIT = -2; + public static final int TURRET_LEFT_SOFT_LIMIT = -55; + public static final double TURRET_SPEED_MULTIPLIER = 0.3; + public static final double TURRET_CALIBRATE_SPEED = 0.075; + public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; //89.56696; + public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166; + + public static final int HOOD_UP_SOFT_LIMIT = 33; + public static final int HOOD_DOWN_SOFT_LIMIT = 3; + public static final double HOOD_CONVERT_SLOPE = 0.47; + public static final double HOOD_CONVERT_B = 40.5; + public static final double HOOD_CALIBRATE_SPEED = 0.2; + 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 DRUM_RAMP_LIMIT = 1000; + public static final double DRUM_VELOCITY_BOUND = 300; + } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 14; + } + + public static final class LevelerConstants { + public static final int LEVELER_CAN_ID = 30; + } + + public static final class IntakeConstants {; + public static final double EXTENDER_SPEED = 0.3; + public static final double INTAKE_SPEED = 1.0; + + public static final int INTAKE_SPARK_ID = 12; + public static final int EXTENDER_SPARK_ID = 13; + } + + public static final class StorageConstants { + public static final int STORAGE_CAN_ID = 11; + public static final double STORAGE_PARTIAL_BALL = 2; + public static final double STORAGE_FULL_BALL = 7; + public static final double STORAGE_SPEED = 0.75; + public static final double STORAGE_TIMEOUT = 3000; + + /* Storage Characteristics */ + public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt + public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt + + /* Ball Indexes */ + public static final int BEAM_SENSOR_SHOOTER = 11; + public static final int BEAM_SENSOR_USELESS = 12; + public static final int BEAM_SENSOR_STORAGE = 13; + public static final int BEAM_SENSOR_INTAKE = 14; + + /* PID Gains */ + public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + + /* PID Values */ + public static final int SLOT_DISTANCE = 0; + + /* PID Indexes */ + public static final int PID_PRIMARY = 0; + } + + public static final class PneumaticsConstants { + public static final int PCM_MODULE_ID = 7; + + public static final int SPEED_SHIFT_FORWARD_ID = 0; + public static final int SPEED_SHIFT_REVERSE_ID = 1; + + public static final int COOL_FALCON_FORWARD_ID = 3; + public static final int COOL_FALCON_REVERSE_ID = 2; + } + + public static final class LEDConstants { + public static final int LED_SPARK_ID = 0; + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } + + public static final class VisionConstants { + public static final double FOV = 29.8; //Field of view of limelight + public static final double TARGET_HEIGHT = 67.5; + public static final double LIME_ANGLE = 24.7; + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double MOTOR_DEAD_ZONE = 0.2; + public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; + public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; + public static final double GRAV = 385.83; + + //Galactic Search + public static final double searchError = 2; + /*public static final double bothCloseVisibleY = -17.69; + public static final double closeLeftVisibleY = -12.57; + public static final double closeRightVisibleY = -11.35; + public static final double farLeftVisibleX = 3.58; + public static final double farRightVisibleX = 7.04;*/ + + public static final double[] aRed = {1.6, -11.7}; + public static final double[] bRed = {2.5, -5.5}; + public static final double[] aBlue = {9.9, 9.0}; + public static final double[] bBlue = {5.5, 13.3}; + + } + + 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_FOX_ID = 2; + } +} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ad222b4..30b626e 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,8 @@ package frc4388.robot; +import java.util.function.Supplier; + import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; @@ -30,6 +32,11 @@ public class Robot extends TimedRobot { double m_currentTime; double m_deltaTime; + protected static Supplier lastIsTestMethod; + public static Boolean lastIsTest() { + return lastIsTestMethod.get(); + } + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -39,6 +46,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + Robot.lastIsTestMethod = this::isTest; SmartDashboard.putString("Is Auto Start?", "NAH"); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 701d6cf..2891b60 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,6 +8,7 @@ package frc4388.robot; import java.nio.file.Path; +import java.util.function.Supplier; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index b39d6b9..3406397 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -8,6 +8,7 @@ package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Robot; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; @@ -42,8 +43,16 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); + double moveInput; + double steerInput; + if (Robot.lastIsTest()) { + moveInput = m_controller.getLeftYAxis() / 2; + steerInput = m_controller.getRightXAxis() / 2; + } else { + moveInput = m_controller.getLeftYAxis(); + steerInput = m_controller.getRightXAxis(); + } + double moveOutput = 0; double steerOutput = 0; if (moveInput >= 0){ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8bc8daa..0256ed0 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -84,19 +84,20 @@ public class Shooter extends SubsystemBase { m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - try { - m_shooterTable = new CSV<>(ShooterTableEntry::new) { - private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + m_shooterTable = new ShooterTableEntry[] {new ShooterTableEntry()}; + // try { + // m_shooterTable = new CSV<>(ShooterTableEntry::new) { + // private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); - @Override - protected String headerSanitizer(final String header) { - return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); - } - }.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); - new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable))).start(); - } catch (final IOException e) { - throw new RuntimeException(e); - } + // @Override + // protected String headerSanitizer(final String header) { + // return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + // } + // }.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); + // new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable))).start(); + // } catch (final IOException e) { + // throw new RuntimeException(e); + // } } @Override