diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..d5eb42e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,6 +91,9 @@ ], "robotJoysticks": [ { +// "guid": "78696e70757401000000000000000000" +// }, +// { "guid": "78696e70757401000000000000000000", "useGamepad": true } diff --git a/simgui.json b/simgui.json index 662c52f..52a4996 100644 --- a/simgui.json +++ b/simgui.json @@ -1,16 +1,19 @@ { "HALProvider": { "Other Devices": { + "SPARK MAX [10]": { "Talon FX[2]": { "header": { "open": true } }, + "SPARK MAX [30]": { "Talon FX[3]": { "header": { "open": true } }, + "SPARK MAX [31]": { "Talon FX[4]": { "header": { "open": true @@ -21,10 +24,14 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Hood": "Subsystem", "/LiveWindow/LED": "Subsystem", "/LiveWindow/SwerveDrive": "Subsystem", "/LiveWindow/SwerveModule": "Subsystem", + "/LiveWindow/Turret": "Subsystem", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Vision": "Subsystem" "/SmartDashboard/Driver Controller": "Mechanism2d", "/SmartDashboard/Field": "Field2d" }, @@ -38,6 +45,9 @@ }, "NetworkTables": { "SmartDashboard": { + "Recording": { + "open": true + }, "open": true } } diff --git a/src/main/deploy/Distances.csv b/src/main/deploy/Distances.csv new file mode 100644 index 0000000..e69de29 diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..99413bc --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,3 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,16 ,12000 +999 ,28.8 ,12000 \ 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 e31c27d..bdeb320 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,12 +4,15 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; + import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -52,9 +55,8 @@ public final class Constants { public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int GYRO_ID = 14; - // ofsets are in degrees - - //NATHAN if you truncate or round or simplify these i will cry + // offsets are in degrees + // NATHAN if you truncate or round or simplify these i will cry public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0; public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0; public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0; @@ -120,4 +122,62 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; } + + public static final class ShooterConstants { + /* PID Constants Shooter */ + public static final int CLOSED_LOOP_TIME_MS = 1; + + public static final int SHOOTER_TIMEOUT_MS = 32; + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; + public static final int SHOOTER_ROTATE_ID = 31; // TODO: find + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; + public static final int DEGREES_PER_ROT = 0; + public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; + public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; + public static final double ENCODER_TICKS_PER_REV = 2048; + + + + /* Turret Constants */ + //ID + public static final int TURRET_MOTOR_CAN_ID = 30; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final double SHOOTER_TURRET_MIN = -1.0; + public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + + // deadzones + public static final double HARD_DEADZONE_LEFT = 0.0; + public static final double HARD_DEADZONE_RIGHT = 340.0; + + public static final double DIG_DEADZONE_LEFT = 40.0; + public static final double DIG_DEADZONE_RIGHT = 60.0; + + public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// + + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values + + /* Hood Constants */ + public static final int SHOOTER_ANGLE_ADJUST_ID = 32; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find + public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find + public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find + + + } + public static final class VisionConstants { + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double GRAV = 385.83; + public static final double TARGET_HEIGHT = 67.5; + public static final double FOV = 29.8; //Field of view limelight + public static final double LIME_ANGLE = 24.7; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 86ec010..4afde0a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,13 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; + import java.util.ArrayList; import java.util.Objects; @@ -27,8 +34,13 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AimToCenter; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.DeadbandedXboxController; @@ -50,7 +62,22 @@ public class RobotContainer { private final TalonFX m_testMotor = new TalonFX(23); private final LED m_robotLED = new LED(m_robotMap.LEDController); - + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + private final Hood m_robotHood = new Hood(); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, + m_robotMap.leftFrontWheelMotor, + m_robotMap.rightFrontSteerMotor, + m_robotMap.rightFrontWheelMotor, + m_robotMap.leftBackSteerMotor, + m_robotMap.leftBackWheelMotor, + m_robotMap.rightBackSteerMotor, + m_robotMap.rightBackWheelMotor, + m_robotMap.leftFrontEncoder, + m_robotMap.rightFrontEncoder, + m_robotMap.leftBackEncoder, + m_robotMap.rightBackEncoder); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -61,7 +88,14 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); /* Default Commands */ - // drives the swerve drive with a two-axis input from the driver controller + + // continually sends updates to the Blinkin LED controller to keep the lights on + // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + + //Turret default command + + m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); + m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), @@ -73,8 +107,6 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); - - m_testMotor.set(TalonFXControlMode.PercentOutput, 0); } /** @@ -105,10 +137,23 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + // activates "BoomBoom" + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) + .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); + /* Driver Buttons */ + // activates intake + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON); + // .whenPressed() -> m_robot + /* operator button */ + // activates hood + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_robotHood.runHood(0.5d)) + .whenReleased(() -> m_robotHood.runHood(0.d)); + // new JoystickButton(getOperatorJoystick()); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 926cfc6..4f1478f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -5,13 +5,20 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -24,16 +31,18 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); configureSwerveMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { - + } - + /* Swerve Subsystem */ + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); @@ -46,6 +55,7 @@ public class RobotMap { public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); public SwerveModule leftFront; @@ -107,10 +117,56 @@ public class RobotMap { rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); // config cancoder as remote encoder for swerve steer motors - //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + } + + // Shooter Config + /* Boom Boom Subsystem */ + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + + // Create motor CANSparkMax + void configureShooterMotorControllers() { + + // LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + // RIGHT FALCON + shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + /* Turret Subsytem */ + shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore + shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore + } } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java new file mode 100644 index 0000000..70e65f3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class AimToCenter extends CommandBase { + /** Creates a new AimWithOdometry. */ + Turret m_turret; + SwerveDrive m_drive; + + // use odometry to find x and y later + double x; + double y; + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, SwerveDrive drive) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + addRequirements(m_turret, m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + x = 0; + y = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle()); + m_turret.runshooterRotatePID(m_targetAngle); + } + + public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return angle; + } + + /** + * Checks if in hardware deadzone (due to mechanical limitations). + * @param angle Angle to check. + * @return True if in hardware deadzone. + */ + public static boolean isHardwareDeadzone(double angle) { + return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); + } + + /** + * Checks if in digital deadzone (due to climber). + * @param angle Angle to check. + * @return True if in digital deadzone. + */ + public static boolean isDigitalDeadzone(double angle) { + return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java new file mode 100644 index 0000000..b962d4b --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -0,0 +1,152 @@ +// 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.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class Shoot extends CommandBase { + + // subsystems + public SwerveDrive m_swerve; + public BoomBoom m_boomBoom; + public Turret m_turret; + public Hood m_hood; + + // given + public double m_gyroAngle; + public double m_odoX; + public double m_odoY; + public double m_distance; + + // targets + public double m_targetVel; + public double m_targetHood; + public double m_targetAngle; + public double m_driveTargetAngle; + + // pid + public double error; + public double prevError; + public double kP, kI, kD; + public double integral, derivative; + public double time; + public double output; + public double tolerance = 5.0; + + // // dummy motor + // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420); + // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration(); + + /** Creates a new Shoot. */ + 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 = 0.1; + kI = 0.0; + kD = 0.0; + + integral = 0; + derivative = 0; + time = 0.02; + } + + /** + * Updates error for custom PID. + */ + public void updateError() { + error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_odoX = 0; //TODO: get this value using odometry + m_odoY = 0; //TODO: get this value using odometry + m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + + // 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.; + m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees(); + + // // normal (i think) PID stuff + // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID(); + // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + + // dummyConfiguration.slot0.kP = 0.1; + // dummyConfiguration.slot0.kI = 0; + // dummyConfiguration.slot0.kD = 0; + // dummyConfiguration.slot0.kF = 0; + + // // weird PID stuff + // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID; + // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0; + + // dummyConfiguration.slot1.kP = 0.1; + // dummyConfiguration.slot1.kI = 0; + // dummyConfiguration.slot1.kD = 0; + // dummyConfiguration.slot1.kF = 0; + + // dummy.configAllSettings(dummyConfiguration); + + // initial error + updateError(); + prevError = error; + } + + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); + integral = integral + error * time; + derivative = (error - prevError) / time; + output = kP * error + kI * integral + kD * derivative; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // dummy.selectProfileSlot(0, 0); + // dummy.selectProfileSlot(1, 1); + // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle); + // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true); + // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch + + // custom pid + runPID(); + m_swerve.driveWithInput(0, 0, output, true); + + m_hood.runAngleAdjustPID(m_targetHood); + m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + updateError(); + return Math.abs(error) <= tolerance; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java new file mode 100644 index 0000000..9dfd332 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -0,0 +1,144 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import java.io.File; +import java.io.IOException; +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.regex.Pattern; +import java.util.stream.IntStream; + +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.CSV; +import frc4388.utility.Gains; +import frc4388.utility.controller.IHandController; + +public class BoomBoom extends SubsystemBase { +public WPI_TalonFX m_shooterFalconLeft; +public WPI_TalonFX m_shooterFalconRight; +public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; +public static BoomBoom m_boomBoom; +public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 +// BangBangController m_controller = new BangBangController(); + +double velP; +double input; + +public boolean m_isDrumReady = false; +public double m_fireVel; + +public Hood m_hoodSubsystem; +public Turret m_turretSubsystem; + +// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later + +public static class ShooterTableEntry { + public Double distance, hoodExt, drumVelocity; +} + +private ShooterTableEntry[] m_shooterTable; + +/* +* Creates new BoomBoom subsystem, has drum shooter and angle adjuster +*/ + /** Creates a new BoomBoom. */ +public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { + m_shooterFalconLeft = shooterFalconLeft; + m_shooterFalconRight = shooterFalconRight; + + + try { + CSV csv = new CSV<>(ShooterTableEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + + }; + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); + new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException e) { + e.printStackTrace(); + // throw new RuntimeException(e); + } +} + +public Double getVelocity(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); +} + +public Double getHood(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); +} + +private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); + final E closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); +} + +private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); + return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); +} + +private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { + final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); + return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); +} + + @Override + public void periodic() { + // This method will be called once per scheduler run + + } + + public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { + m_hoodSubsystem = subsystem0; + m_turretSubsystem = subsystem1; + } +/** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ +public void runDrumShooter(double speed) { + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + + } + +public void setShooterGains() { + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); +} + + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init + m_shooterFalconRight.follow(m_shooterFalconLeft); + // New BoomBoom controller stuff + //Controls a motor with the output of the BangBang controller + //Controls a motor with the output of the BangBang conroller and a feedforward + //Shrinks the feedforward slightly to avoid over speeding the shooter + + + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java new file mode 100644 index 0000000..b3a2f30 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; + +public class Hood extends SubsystemBase { + public BoomBoom m_shooterSubsystem; + + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + + + //public boolean m_isHoodReady = false; + +public double m_fireAngle; + + + /** Creates a new Hood. */ + public Hood() { + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); + + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + setHoodSoftLimits(true); + } + + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Set status of hood motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setHoodSoftLimits(boolean set) { + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + public void runAngleAdjustPID(double targetAngle) + { + //Set PID Coefficients + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + } + + + public void runHood(double input) { + // m_angleAdjusterMotor.set(input); + } + + public void resetGyroAngleAdj(){ + // m_angleEncoder.setPosition(0); + } + + public double getAnglePosition(){ + return 0.0;//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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f725e06..342bbca 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -19,9 +19,11 @@ 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.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; @@ -61,31 +63,13 @@ 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) { - // m_leftFrontSteerMotor = leftFrontSteerMotor; - // m_leftFrontWheelMotor = leftFrontWheelMotor; - // m_rightFrontSteerMotor = rightFrontSteerMotor; - // m_rightFrontWheelMotor = rightFrontWheelMotor; - // m_leftBackSteerMotor = leftBackSteerMotor; - // m_leftBackWheelMotor = leftBackWheelMotor; - // m_rightBackSteerMotor = rightBackSteerMotor; - // m_rightBackWheelMotor = rightBackWheelMotor; - // m_leftFrontEncoder = leftFrontEncoder; - // m_rightFrontEncoder = rightFrontEncoder; - // m_leftBackEncoder = leftBackEncoder; - // m_rightBackEncoder = rightBackEncoder; + m_leftFront = leftFront; m_leftBack = leftBack; m_rightFront = rightFront; m_rightBack = rightBack; m_gyro = gyro; - // modules = new SwerveModule[] { - // new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left - // new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right - // new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left - // new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right - // }; - modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java new file mode 100644 index 0000000..3a00f02 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -0,0 +1,115 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import java.util.concurrent.TimeoutException; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.Shoot; +import frc4388.utility.Gains; + +public class Turret extends SubsystemBase { + + /** Creates a new Turret. */ + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; + public Gyro m_turretGyro; + + public double m_targetDistance = 0; + + public boolean m_isAimReady = false; + + SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); + public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + + // Variables + public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + + m_boomBoomRotateMotor = boomBoomRotateMotor; + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + setTurretSoftLimits(true); + + m_boomBoomRotateMotor.setInverted(false); + + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setTurretSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + + public void runTurretWithInput(double input) { + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runshooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroShooterRotate() { + m_boomBoomRotateEncoder.setPosition(0); + } + + public double getboomBoomRotatePosition() { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getBoomBoomAngleDegrees() { + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 + / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; + } + +} \ 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 new file mode 100644 index 0000000..686ef2c --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,133 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTableEntry; + +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.controller.IHandController; + +public class Vision extends SubsystemBase { +//setup + Turret m_turret; + BoomBoom m_boomBoom; + Hood m_hood; + +NetworkTableEntry xEntry; +IHandController m_driverController; +//Aiming +double turnAmount = 0; +double xAngle = 0; +double yAngle = 0; +double target = 0; +public double distance; +public double realDistance; +public static double fireVel; +public static double fireAngle; + +public double m_hoodTrim; +public double m_turretTrim; +public double m_fireAngle; + + +public Vision(Turret aimSubsystem, BoomBoom boomBoom) { + m_turret = aimSubsystem; + m_boomBoom = boomBoom; + m_hood = m_boomBoom.m_hoodSubsystem; + //addRequirements(m_turret); + limeOff(); + changePipeline(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3); +} + +public void track(){ + target = getV(); + xAngle = getX(); + yAngle = getY(); + + //find distance + distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + // if (target == 1.0) { //checks if target is in view + // //aims left and right + // turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); + // if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + // turnAmount = 0; + // } + // else if (turnAmount > 0 && turnAmount < 0.1){ + // turnAmount = 0.1; + // } + // else if (turnAmount < 0 && turnAmount > -0.1){ + // turnAmount = -0.1; + // } + // } + + SmartDashboard.putNumber("Distance to Target", realDistance); + + + // //start CSV + + // fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); + // fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); + // //fire angle unknown so far + // //end of CSV + + // m_boomBoom.m_fireVel = fireVel; + // m_hood.m_fireAngle = fireAngle; + // m_turret.m_targetDistance = distance; + + // checkFinished(); +} + +public void checkFinished(){ + if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ + m_turret.m_isAimReady = true; + } + else{ + m_turret.m_isAimReady = false; + } +} + +public void limeOff(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); +} + + public void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); +} + + public void changePipeline(int pipelineId) + { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); + } + + public double getV() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); + } + + public double getX() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); + } + + public double getY() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + } + @Override + public void periodic(){ + //called once per scheduler run + } +} + + diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..522980a --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,240 @@ +/*----------------------------------------------------------------------------*/ +/* 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.utility; + +import java.awt.Color; +import java.io.BufferedReader; +import java.io.IOException; +import java.lang.invoke.MethodHandleProxies; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.text.MessageFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.function.BiConsumer; +import java.util.function.Function; +import java.util.function.IntFunction; +import java.util.function.Predicate; +import java.util.function.Supplier; +import java.util.regex.Pattern; +import java.util.stream.Collectors; +import java.util.stream.IntStream; +import java.util.stream.Stream; + +public class CSV { + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; + + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by + * the given generator. Private fields and fields of primitive types are not supported. + * @param generator a parameterless supplier which produces a new object with any number of fields + * corresponding to header names from a CSV file. The first character of the names + * from the header in the CSV file will be made lowercase and invalid characters + * will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array filled with populated + * objects created with the previously given generator. Cells are parsed using their corresponding + * field's {@code valueOf(String)} function. + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); + final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + if (colored) + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00); + private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } + } + + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; + } + + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); + } + + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } + } + + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; + } + + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); + } + + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); + } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } +} diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java new file mode 100644 index 0000000..72a84f4 --- /dev/null +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -0,0 +1,113 @@ +package frc4388.commands; + +import org.junit.Test; + +import frc4388.robot.commands.AimToCenter; +import org.junit.Assert; + +public class AimToCenterTest { + + private static final double DELTA = 1e-15; + + @Test + public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { + boolean output; + + //20 deg + output = AimToCenter.isHardwareDeadzone(20.); + Assert.assertFalse(output); + + //-10 deg + output = AimToCenter.isHardwareDeadzone(-10.); + Assert.assertTrue(output); + + //-1 deg + output = AimToCenter.isHardwareDeadzone(-1.); + Assert.assertTrue(output); + + //341 deg + output = AimToCenter.isHardwareDeadzone(341.); + Assert.assertTrue(output); + + //340 deg + output = AimToCenter.isHardwareDeadzone(340.); + Assert.assertFalse(output); + + //0 deg + output = AimToCenter.isHardwareDeadzone(0.); + Assert.assertFalse(output); + + //200 deg + output = AimToCenter.isHardwareDeadzone(200.); + Assert.assertFalse(output); + + //2000000 deg + output = AimToCenter.isHardwareDeadzone(2000000.); + Assert.assertTrue(output); + + //NaN deg + output = AimToCenter.isHardwareDeadzone(Double.NaN); + Assert.assertFalse(output); + } + + @Test + public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() { + double actual; + double expected; + + //(5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., 5., 0.); + expected = 180. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, 5., 0.); + expected = 180. + 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, -5., 0.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., -5., 0.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(0,0) Gyro = 0 deg + actual = AimToCenter.angleToCenter(0., 0., 0.); + Assert.assertNotNull(actual); + + //(5,5) Gyro = 180 deg + actual = AimToCenter.angleToCenter(5., 5., 180.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(100,100) Gyro = 90 deg + actual = AimToCenter.angleToCenter(100., 100., 90.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(null,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = null deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(5,5) Gyro = -20 deg + actual = AimToCenter.angleToCenter(5., -5., -45.); + expected = 180.; + Assert.assertEquals(expected, actual, DELTA); + + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 69cb243..9fe8f58 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -17,6 +17,9 @@ import frc4388.utility.LEDPatterns; * Add your docs here. */ public class LEDSubsystemTest { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); @Test public void testConstructor() { // Arrange diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..fb19ccf --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +}