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

stuff

Browse Source
This commit is contained in:
Ryan
2022-03-24 10:38:13 -06:00
parent 6ffbdc57c6
commit 5dd99218ae
4 changed files with 27 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -181,7 +181,7 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
m_robotContainer.m_robotVisionOdometry.setLEDs(true);
}
@Override
src/main/java/frc4388/robot/RobotContainer.java
+17 -8
View File
@@ -302,15 +302,22 @@ public class RobotContainer {
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
.whileHeld(new InstantCommand(() -> m_robotExtender.invertExtender(-1.0)))
.whenReleased(new InstantCommand(() -> m_robotExtender.invertExtender(1.0)));
// Left Button > Extender In
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// Left Button > Extender Out
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
@@ -398,7 +405,7 @@ public class RobotContainer {
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! THREE BALL AUTO (HOPEFULLY)
return new SequentialCommandGroup( new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), // * aim with turret to target
return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
@@ -409,15 +416,17 @@ public class RobotContainer {
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363)
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), // * aim with turret to target
//new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363)
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
//new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
//new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
weirdAutoShootingGroup, // * shoot
src/main/java/frc4388/robot/subsystems/Climber.java
+3 -2
View File
@@ -18,7 +18,7 @@ public class Climber extends SubsystemBase {
/** Creates a new Climber */
public Climber(WPI_TalonFX elbow) {
this.elbow = elbow;
elbow.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
elbow.overrideLimitSwitchesEnable(true); //Check
elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); // Tune
@@ -39,6 +39,7 @@ public class Climber extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putBoolean("Climber Folded", (elbow.isFwdLimitSwitchClosed() == 1));
SmartDashboard.putNumber("Elbow Angle", elbow.getSelectedSensorPosition());
SmartDashboard.putBoolean("Climber Folded", (elbow.isRevLimitSwitchClosed() == 1));
}
}
src/main/java/frc4388/robot/subsystems/Extender.java
+6 -1
View File
@@ -16,6 +16,7 @@ import frc4388.robot.Constants.ExtenderConstants;
public class Extender extends SubsystemBase {
private CANSparkMax m_extenderMotor;
private double m_invert = 1;
// private SparkMaxLimitSwitch m_inLimit;
// private SparkMaxLimitSwitch m_outLimit;
@@ -44,6 +45,10 @@ public class Extender extends SubsystemBase {
m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
public void invertExtender(double invert){
m_invert = invert;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
@@ -52,7 +57,7 @@ public class Extender extends SubsystemBase {
public void runExtender(double input) {
// if (!m_serializer.getBeam() && input < 0.) return;
m_extenderMotor.set(input);
m_extenderMotor.set(input * m_invert);
}
public double getPosition() {