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

se5dr

Browse Source
This commit is contained in:
aarav18
2022-03-18 23:56:01 -06:00
parent 6570f81cf7
commit 113a26d831
3 changed files with 33 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -256,7 +256,7 @@ public class Robot extends TimedRobot {
Robot.alliance = DriverStation.getAlliance();
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
// m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
src/main/java/frc4388/robot/RobotContainer.java
+24 -18
View File
@@ -212,26 +212,26 @@ public class RobotContainer {
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
// m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentControlMode.equals(ControlMode.SHOOTER)) {
// if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
// }
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
// }, m_robotTurret));
m_robotTurret.setDefaultCommand(
new RunCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) {
if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
}
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
}, m_robotTurret));
// m_robotHood.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
// }, m_robotHood));
m_robotHood.setDefaultCommand(
new RunCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
// m_robotClimber.setDefaultCommand(
// // new RunCommand(() -> {
// // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
// /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(),
// getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //}
// }, m_robotClimber));
m_robotClimber.setDefaultCommand(
new RunCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
getOperatorController().getRightY() * 10000); }}
, m_robotClimber));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -269,6 +269,12 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true))
.whenReleased(() -> m_robotSwerveDrive.stopModules());
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
/* Operator Buttons */
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+8 -7
View File
@@ -35,29 +35,30 @@ public class AimToCenter extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = 0;
y = 0;
x = m_drive.getOdometry().getX();
y = m_drive.getOdometry().getY();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
System.out.println("Target Angle" + m_targetAngle);
m_turret.runShooterRotatePID(m_targetAngle);
// Check if limelight is within range (comment out to disable vision odo)
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
m_visionOdometry.updateOdometryWithVision();
m_visionOdometry.setLEDs(true);
// m_visionOdometry.updateOdometryWithVision();
// m_visionOdometry.setLEDs(true);
}
else{
m_visionOdometry.setLEDs(false);
// m_visionOdometry.setLEDs(false);
}
}
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return angle;
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return (angle - 360);
}
/**