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

shoot is janky

Browse Source
This commit is contained in:
aarav18
2022-03-20 01:42:34 -06:00
parent 1c8ad0a0d1
commit e34a89a46f
5 changed files with 31 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -37,7 +37,7 @@ public final class Constants {
public static final double LOOP_TIME = 0.02;
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 3.0;
public static final double ROTATION_SPEED = 2.0;
public static final double WIDTH = 23.75;
public static final double HEIGHT = 23.75;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -278,7 +278,7 @@ public final class Constants {
public static final double TURRET_CLIMBING_POS = -3.76;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
src/main/java/frc4388/robot/RobotContainer.java
+5 -5
View File
@@ -265,7 +265,7 @@ public class RobotContainer {
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true))
.whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true))
.whenReleased(() -> m_robotSwerveDrive.stopModules());
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
@@ -273,7 +273,7 @@ public class RobotContainer {
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
.whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood))
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
.whenReleased(() -> m_robotSwerveDrive.stopModules());
@@ -306,9 +306,9 @@ public class RobotContainer {
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
// .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+1 -1
View File
@@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup {
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry));
addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+10 -14
View File
@@ -82,16 +82,19 @@ public class Shoot extends CommandBase {
isAimedInTolerance = false;
}
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
this(swerve, drum, turret, hood, false);
}
// public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
// this(swerve, drum, turret, hood, false);
// }
/**
* Updates error for custom PID.
*/
public void updateError() {
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360;
error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
if (error > 180) {
error = 360 - error;
this.inverted = -1; } else { this.inverted = 1; }
isAimedInTolerance = (Math.abs(error) <= tolerance);
}
@@ -99,7 +102,7 @@ public class Shoot extends CommandBase {
@Override
public void initialize() {
this.turret.gotoMidpoint();
// this.turret.gotoMidpoint();
this.odoX = 0;//-m_swerve.getOdometry().getY();
this.odoY = -8;//-m_swerve.getOdometry().getX();
@@ -124,13 +127,6 @@ public class Shoot extends CommandBase {
* Run custom PID.
*/
public void runPID() {
if (error > 180) {
error = 360 - error;
this.inverted = -1;
}
else{
this.inverted = 1;
}
prevError = error;
updateError();
@@ -151,8 +147,8 @@ public class Shoot extends CommandBase {
SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
SmartDashboard.putNumber("Normalized Output", this.normOutput);
this.swerve.driveWithInput(0, 0, normOutput, true);
this.turret.m_boomBoomRotateMotor.set(normOutput);
this.swerve.driveWithInput(0, 0, normOutput, true);
if (this.toShoot) {
this.hood.runAngleAdjustPID(this.targetHood);
@@ -167,7 +163,7 @@ public class Shoot extends CommandBase {
// ? return to initial swerve rotation
// swerve.driveWithInput(0, 0, initialSwerveRotation, true);
this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
// this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
this.turret.m_boomBoomRotateMotor.set(0.0);
if (this.toShoot) {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -10
View File
@@ -105,13 +105,13 @@ public class SwerveDrive extends SubsystemBase {
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
// new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(leftX, leftY);
@@ -119,6 +119,9 @@ public class SwerveDrive extends SubsystemBase {
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
if (ignoreAngles) {
rot = 0;
}
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
@@ -127,14 +130,14 @@ public class SwerveDrive extends SubsystemBase {
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
if (ignoreAngles) {
SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
for (int i = 0; i < states.length; i ++) {
SwerveModuleState state = states[i];
lockedStates[i]= new SwerveModuleState(0, state.angle);
}
setModuleStates(lockedStates);
}
// if (ignoreAngles) {
// SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
// for (int i = 0; i < states.length; i ++) {
// SwerveModuleState state = states[i];
// lockedStates[i]= new SwerveModuleState(0, state.angle);
// }
// setModuleStates(lockedStates);
// }
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());