/* ---------------- Constants ---------------- */
- private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE);
- private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE);
+ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values
+ private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180);
- private static final double MAX_VEL_RAD_PER_SEC = 600;
+ private static final double MAX_VEL_RAD_PER_SEC = 15;
+ //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
+ // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
- private static final double VERSA_RATIO = 5.0;
- private static final double TURRET_RATIO = 140.0 / 10.0;
- private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO;
+ private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;
- private static final PIDController positionPID = new PIDController(15.0, 0, 0.0);
- // private static final PIDController positionPID = new PIDController(15, 0, 0.25);
- private static final PIDController velocityPID = new PIDController(0.2, 0.0, 0.0);
+ private static final PIDController positionPID = new PIDController(15, 0, 0.25);
+ //private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
+ private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
+
+ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02
+ , 0.02);
+ //hello
+ private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
+ private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();