2024AcrossTheRidgebotiverse/src/main/java/frc4388/robot/Constants.java

/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.math.trajectory.TrapezoidProfile;

import edu.wpi.first.math.geometry.Translation2d;

import frc4388.utility.LEDPatterns;
import frc4388.utility.Gains;

/**
 * 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.
 *
 * <p>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 Constants {
    public static final class SwerveDriveConstants {

        public static final double MAX_ROT_SPEED        = 3.5;
        public static final double AUTO_MAX_ROT_SPEED = 1.5;
        public static final double MIN_ROT_SPEED        = 0.8;
        public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
        public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
        public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
    
        public static final double CORRECTION_MIN = 10;
        public static final double CORRECTION_MAX = 50;
    
        public static final double SLOW_SPEED = 0.2;
        public static final double FAST_SPEED = 1.0;
        public static final double TURBO_SPEED = 4.0;
    
        public static final class DefaultSwerveRotOffsets {
          public static final double FRONT_LEFT_ROT_OFFSET = 130.957 + 90;
          public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 45 + 90 ;//-202.588;
          public static final double BACK_LEFT_ROT_OFFSET = 347.871 + 90;
          public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90;
        }

        public static final class IDs {
          public static final int RIGHT_FRONT_WHEEL_ID = 2;
          public static final int RIGHT_FRONT_STEER_ID = 3;
          public static final int RIGHT_FRONT_ENCODER_ID = 10;
          
          public static final int LEFT_FRONT_WHEEL_ID = 4;
          public static final int LEFT_FRONT_STEER_ID = 5;
          public static final int LEFT_FRONT_ENCODER_ID = 11;
    
          public static final int LEFT_BACK_WHEEL_ID = 6;
          public static final int LEFT_BACK_STEER_ID = 7;
          public static final int LEFT_BACK_ENCODER_ID = 12;
          
          public static final int RIGHT_BACK_WHEEL_ID = 8;
          public static final int RIGHT_BACK_STEER_ID = 9;
          public static final int RIGHT_BACK_ENCODER_ID = 13;
        }
    
        public static final class PIDConstants {
            public static final int SWERVE_SLOT_IDX = 0;
            public static final int SWERVE_PID_LOOP_IDX = 1;
            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.0, 0, 1.0);
        }
    
        public static final class AutoConstants {
            public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
            public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
            public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
            public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
            
            public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
            public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
        }
    
        public static final class Conversions {
            public static final int CANCODER_TICKS_PER_ROTATION = 4096;
        
            public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
            public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
        
            public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
            public static final double MOTOR_REV_PER_STEER_REV = 12.8;
        
            public static final double TICKS_PER_MOTOR_REV = 2048;
            public static final double WHEEL_DIAMETER_INCHES = 3.9;
            public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
        
            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;
            public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
        
            public static final double TICK_TIME_TO_SECONDS = 10;
            public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
        }
    
        public static final class Configurations {
            public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
            public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
            public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
        }
    
        public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
        public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
    
        // dimensions
        public static final double WIDTH = 18.5;
        public static final double HEIGHT = 18.5;
        public static final double HALF_WIDTH = WIDTH / 2.d;
        public static final double HALF_HEIGHT = HEIGHT / 2.d;
    
        // misc
        public static final int TIMEOUT_MS = 30;
        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
      }
    
    public static final class VisionConstants {
        // public static final String NAME = "photonCamera";
    
        // public static final int LIME_HIXELS = 640;
        // public static final int LIME_VIXELS = 480;
    
        // public static final double H_FOV = 59.6;
        // public static final double V_FOV = 45.7;
    
        // public static final double LIME_HEIGHT = 6.0;
        // public static final double LIME_ANGLE = 55.0;
    
        // // public static final double HIGH_TARGET_HEIGHT = 46.0;
        // public static final double HIGH_TAPE_HEIGHT = 44.0;
        
        // // public static final double MID_TARGET_HEIGHT = 34.0;
        // public static final double MID_TAPE_HEIGHT = 24.0;
    
        // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value

        public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
        public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593);

        public static final double SpeakerBubbleDistance = 3;
        public static final double targetPosDistance = 1.5;
    
    }
   
    public static final class AutoAlignConstants {
        public static final double MoveSpeed = 0.0;
        public static final double RotSpeed = 0.0;
    }

    public static final class DriveConstants {
        public static final int DRIVE_PIGEON_ID = 14;

        public static final int SMARTDASHBOARD_UPDATE_FRAME = 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 ShooterConstants {
        public static final int LEFT_SHOOTER_ID = 15; // final
        public static final int RIGHT_SHOOTER_ID = 16; // final
        public static final double SHOOTER_SPEED = 0.50; // final
        public static final double SHOOTER_IDLE = 0.35; // final
        public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
    }

    public static final class IntakeConstants {
        public static final int INTAKE_MOTOR_ID = 18;
        public static final int PIVOT_MOTOR_ID = 17;
        public static final double INTAKE_SPEED = 0.75;
        public static final double INTAKE_OUT_SPEED_UNPRESSED =  1.0;       
        public static final double INTAKE_OUT_SPEED_PRESSED = 0.5;        
        public static final double HANDOFF_SPEED = 0.4;
        public static final double PIVOT_SPEED = 0.2;

        public static final class ArmPID {
            public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
        }
    }

    public static final class OIConstants {
        public static final int XBOX_DRIVER_ID = 0;
        public static final int XBOX_OPERATOR_ID = 1;
        public static final double LEFT_AXIS_DEADBAND = 0.1;

    }
}