import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.DriverStation;
- import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.PS5Controller;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+ import frc.robot.commands.DoNothing;
import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.DynamicAutoBuilder;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
- import frc.robot.commands.gpm.AutoShootCommand;
- import frc.robot.commands.gpm.ClimbDriveCommand;
-import frc.robot.commands.gpm.BrownOutControl;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.LockedShoot;
import frc.robot.commands.gpm.RunSpindexer;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
- import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.LED.LED;
+import frc.robot.subsystems.PowerControl.EMABreaker;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
- private LinearClimb linearClimb = null;
+ private LED led = null;
+
+ private EMABreaker breaker = null;
+
+ // TODO: move to correct robot and put the correct port?
+ private PS5Controller ps5 = new PS5Controller(0);
+
+
// auto Command selection
private final SendableChooser<Command> autoChooser = new SendableChooser<>();
if (turret != null) {
turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
-
- if (shooter != null && spindexer != null && turret != null && intake != null && hood != null && drive != null) {
- CommandScheduler.getInstance().schedule(new BrownOutControl(shooter, spindexer, turret, intake, hood, drive));
- }
- drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ if (drive != null && driver != null) {
+ drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ }
break;
}
updateSetpoints(drivepose);
turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
- SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
+ // SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
+ shuttlingTOFMultiplier = SmartDashboard.getNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier);
+ SmartDashboard.putNumber("Shuttling TOF Multiplier", shuttlingTOFMultiplier);
+
if (phaseManager.isIdle()) {
underLadder();
} else {
// we can be less aggressive: y = 0.65 * (1.34959x + 9.79618)
// will likely be this that requires tuning.
shooterVelocityMap.put(0.0, 9.0);
- shooterVelocityMap.put(4.0, 12.8);
- shooterVelocityMap.put(7.6, 19.0);
- shooterVelocityMap.put(11.4, 28.1); // was 25.2 before
+ shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned
+ shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned
- shooterVelocityMap.put(16.0, 44.0); // untuned
+ shooterVelocityMap.put(16.54, 70.0); // untested
// always shoot at low angle to ground.
- newHoodMap.put(0.0, 60.0);
- newHoodMap.put(27.99, 60.0);
+ newHoodMap.put(0.0, 55.0);
+ newHoodMap.put(27.99, 55.0);
}
}
inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
}
- /**
- * sets supply and stator current limits
- * @param limit the current limit for stator and supply current
- */
- public void setCurrentLimits(double limit) {
- CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
- .withStatorCurrentLimitEnable(true)
- .withStatorCurrentLimit(limit)
- .withSupplyCurrentLimitEnable(true)
- .withSupplyCurrentLimit(limit);
-
- if(limit == TurretConstants.NORMAL_CURRENT_LIMIT){
- limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT;
- limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second
- }
-
- motor.getConfigurator().apply(limits);
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motor.getConfigurator().apply(limitConfig);
}
- public double getSubsystemStatorCurrent() {
- return inputs.motorStatorCurrent;
- }
+ private void calibrate(){
+ setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
+ calibrating = true;
+ }
- public double getSubsystemSupplyCurrent() {
- return inputs.motorSupplyCurrent;
- }
+ private void stopCalibrating(){
+ motor.set(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO);
+ setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
+ calibrating = false;
+ setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
+ }
}
public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06;
- public static final double FEEDFORWARD_KV = 0.06;
+ public static final double FEEDFORWARD_KV = 0.02
+ ;
- public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
- public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A
- public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
+ public static final double STATOR_CURRENT_LIMIT = 40.0; // A
+ public static final double SUPPLY_CURRENT_LIMIT = 40.0; // A
}