diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bdeb320..94a86e0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -133,7 +133,7 @@ public final class Constants { 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 int SHOOTER_ROTATE_ID = 31; // TODO: find this value public static final double TURRET_SPEED_MULTIPLIER = 0.1d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3d56eca..06f753d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,8 +67,8 @@ public class RobotContainer { 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 Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -85,7 +85,7 @@ public class RobotContainer { //Turret default command - m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); + // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4f1478f..649fcb2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -136,7 +136,7 @@ public class RobotMap { 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); + // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index b3a2f30..5b1d2ff 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -21,13 +21,13 @@ 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 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 RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); //public boolean m_isHoodReady = false; @@ -37,15 +37,15 @@ public double m_fireAngle; /** Creates a new Hood. */ public Hood() { - m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); // m_hoodUpLimitSwitch.enableLimitSwitch(true); - m_hoodDownLimitSwitch.enableLimitSwitch(true); + // m_hoodDownLimitSwitch.enableLimitSwitch(true); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); setHoodSoftLimits(true); } @@ -60,21 +60,21 @@ public double m_fireAngle; * @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); + // 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.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); + // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f945d3c..de9e05d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -59,6 +59,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; public Rotation2d rotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); private final Field2d m_field = new Field2d(); @@ -112,6 +113,24 @@ public class SwerveDrive extends SubsystemBase { : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); setModuleStates(states); } + // public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) + // { + // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + // Translation2d speed = new Translation2d(leftX, leftY); + // speed = speed.times(speed.getNorm() * speedAdjust); + // if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) + // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + // double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); + // double xSpeedMetersPerSecond = -speed.getX(); + // double ySpeedMetersPerSecond = speed.getY(); + // SwerveModuleState[] states = + // m_kinematics.toSwerveModuleStates( + // fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED)); + // setModuleStates(states); + // } + public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; @@ -122,11 +141,12 @@ public class SwerveDrive extends SubsystemBase { double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); + chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED)); + chassisSpeeds); setModuleStates(states); } @@ -148,14 +168,26 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { updateOdometry(); - // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); + updateSmartDash(); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); - // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } + private void updateSmartDash() { + // odometry + SmartDashboard.putNumber("Odometry: X", getOdometry().getX()); + SmartDashboard.putNumber("Odometry: Y", getOdometry().getY()); + SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); + + // chassis speeds + // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds + SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + } /** * Gets the distance between two given poses. * @param p1 The first pose.