Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
aarav18
2022-03-18 23:08:04 -06:00
parent 46396a704f e19dc2d77c
commit cff0eb9c3f
2 changed files with 35 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+2 -32
View File
@@ -153,8 +153,8 @@ public class RobotContainer {
public static boolean softLimits = true;
// control mode switching
private enum ControlMode { SHOOTER, CLIMBER };
private ControlMode currentControlMode = ControlMode.SHOOTER;
public static enum ControlMode { SHOOTER, CLIMBER };
public static ControlMode currentControlMode = ControlMode.SHOOTER;
// turret mode switching
private enum TurretMode { MANUAL, AUTONOMOUS };
@@ -337,38 +337,8 @@ public class RobotContainer {
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
// .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null)))
// .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null)))
// .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber))))
// .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null)))
// .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand(
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret))))
// .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand(
// new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood))));
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
// .whenReleased(EnableClimber()));
// control turret manual mode
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
// .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whenPressed(new InstantCommand(() -> {
// this.currentClimberMode = ClimberMode.AUTONOMOUS;
// }))
// .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL)))
// .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS))
// .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL)))
// .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL));
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+33 -27
View File
@@ -20,40 +20,42 @@ import frc4388.utility.Gains;
public class Shoot extends CommandBase {
// subsystems
public SwerveDrive m_swerve;
public BoomBoom m_boomBoom;
public Turret m_turret;
public Hood m_hood;
private SwerveDrive m_swerve;
private BoomBoom m_boomBoom;
private Turret m_turret;
private Hood m_hood;
// given
public double m_gyroAngle;
public double m_odoX;
public double m_odoY;
public double m_distance;
private double m_gyroAngle;
private double m_odoX;
private double m_odoY;
private double m_distance;
// targets
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public Pose2d m_targetPoint;
private double m_targetVel;
private double m_targetHood;
private double m_targetAngle;
private Pose2d m_targetPoint;
// pid
public double error;
public double prevError;
public Gains gains = ShooterConstants.SHOOT_GAINS;
public double kP, kI, kD;
public double proportional, integral, derivative;
public double time;
public double output;
public double normOutput;
public double tolerance;
public boolean isAimedInTolerance;
public int inverted;
private double error;
private double prevError;
private Gains gains = ShooterConstants.SHOOT_GAINS;
private double kP, kI, kD;
private double proportional, integral, derivative;
private double time;
private double output;
private double normOutput;
private double tolerance;
private boolean isAimedInTolerance;
private int inverted;
private double initialSwerveRotation;
// testing
public boolean simMode = true;
public DummySensor driveDummy;
public DummySensor turretDummy;
private boolean simMode = true;
private DummySensor driveDummy;
private DummySensor turretDummy;
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
@@ -117,6 +119,7 @@ public class Shoot extends CommandBase {
m_odoY = -1;//m_swerve.getOdometry().getY();
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
initialSwerveRotation = m_gyroAngle;
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
@@ -182,7 +185,10 @@ public class Shoot extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
// return to initial swerve rotation
m_swerve.driveWithInput(0, 0, initialSwerveRotation, true);
}
// Returns true when the command should end.
@Override