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 working no optimization

Browse Source
This commit is contained in:
aarav18
2022-03-20 14:25:38 -06:00
parent 0eca600135
commit e39c81d556
6 changed files with 120 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -1
View File
@@ -278,7 +278,8 @@ public final class Constants {
public static final double TURRET_CLIMBING_POS = -3.76;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SWERVE_SHOOT_GAINS = new Gains(4.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
src/main/java/frc4388/robot/RobotContainer.java
+25 -20
View File
@@ -179,19 +179,19 @@ public class RobotContainer {
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
// if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
m_robotSwerveDrive.driveWithInput( 0,
0,
0,
0,
true);
}}
true); //}
// if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
// m_robotSwerveDrive.driveWithInput( 0,
// 0,
// 0,
// 0,
// true);
}//}
, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
@@ -278,24 +278,29 @@ public class RobotContainer {
.whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
.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());
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
// new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// .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());
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
/* Operator Buttons */
/* Operator Buttons */
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
.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());
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+1 -1
View File
@@ -85,7 +85,7 @@ public class AimToCenter extends CommandBase {
if (-x > 0) { return (180 + exp); }
if (-x < 0) { return (360 + exp); }
if (x == 0 && y > 0) { return (270 - actualGyro); }
if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated
if (x == 0 && y < 0) { return (90 - actualGyro); }
System.err.println("Invalid case.");
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+86 -68
View File
@@ -37,13 +37,18 @@ public class Shoot extends CommandBase {
private double targetAngle, targetVel, targetHood;
// pid
private Gains gains = ShooterConstants.SHOOT_GAINS;
private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS;
private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS;
private double error;
private double prevError;
private double kP, kI, kD;
private double proportional, integral, derivative;
private double output, normOutput;
private double tolerance;
boolean timerStarted;
long startTime;
private double timeTolerance;
private boolean isAimedInTolerance;
private int inverted;
@@ -67,88 +72,95 @@ public class Shoot extends CommandBase {
this.hood = hood;
this.toShoot = toShoot;
addRequirements(this.swerve, this.drum, this.turret, this.hood);
kP = gains.kP;
kI = gains.kI;
kD = gains.kD;
kP = turretGains.kP;
kI = turretGains.kI;
kD = turretGains.kD;
proportional = 0;
integral = 0;
derivative = 0;
tolerance = 10.0;
timeTolerance = 500;
isAimedInTolerance = false;
}
this.inverted = 1;
addRequirements(this.swerve, this.drum, this.turret, this.hood);
}
// 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;
if (error > 180) {
error = 360 - error;
this.inverted = -1; } else { this.inverted = 1; }
isAimedInTolerance = (Math.abs(error) <= tolerance);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
// this.turret.gotoMidpoint();
this.odoX = 0;//-m_swerve.getOdometry().getY();
this.odoY = -8;//-m_swerve.getOdometry().getX();
this.distance = Math.hypot(odoX, odoY);
this.gyroAngle = this.swerve.getRegGyro().getDegrees();
this.initialSwerveRotation = gyroAngle;
// get targets (shooter tables)
this.targetVel = drum.getVelocity(distance);
this.targetHood = drum.getHood(distance);
this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
// this(swerve, drum, turret, hood, false);
// }
// initial error
updateError();
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
prevError = error;
updateError();
/**
* Updates error for custom PID.
*/
public void updateError() {
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
// if (error > 180) {
// error = 360 - error; // TODO: error - 360
// this.inverted = -1; } else { this.inverted = 1; }
isAimedInTolerance = (Math.abs(error) <= tolerance);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timerStarted = false;
startTime = 0;
// this.turret.gotoMidpoint();
this.odoX = 0;//-m_swerve.getOdometry().getY();
this.odoY = -8;//-m_swerve.getOdometry().getX();
this.distance = Math.hypot(odoX, odoY);
this.gyroAngle = this.swerve.getRegGyro().getDegrees();
this.initialSwerveRotation = gyroAngle;
// get targets (shooter tables)
this.targetVel = drum.getVelocity(distance);
this.targetHood = drum.getHood(distance);
this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
// initial error
updateError();
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
prevError = error;
updateError();
this.proportional = error;
this.integral = integral + (error * Constants.LOOP_TIME);
this.derivative = (error - prevError) / Constants.LOOP_TIME;
this.output = kP * proportional + kI * integral + kD * derivative;
this.normOutput = (output / 360) * inverted;
}
this.proportional = error;
this.integral = integral + (error * Constants.LOOP_TIME);
this.derivative = (error - prevError) / Constants.LOOP_TIME;
this.output = kP * proportional + kI * integral + kD * derivative;
this.normOutput = (output / 360) * inverted;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
runPID();
SmartDashboard.putNumber("Error", this.error);
SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
SmartDashboard.putNumber("Normalized Output", this.normOutput);
this.turret.m_boomBoomRotateMotor.set(normOutput);
this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative
this.turret.runTurretWithCustomPID(normOutput);
// this.turret.m_boomBoomRotateMotor.set(normOutput);
this.swerve.driveWithInput(0, 0, normOutput * (this.swerveGains.kP/this.turretGains.kP), false); // ? should the output be field relative
if (this.toShoot) {
this.hood.runAngleAdjustPID(this.targetHood);
@@ -160,6 +172,7 @@ public class Shoot extends CommandBase {
@Override
public void end(boolean interrupted) {
System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase());
// ? return to initial swerve rotation
// swerve.driveWithInput(0, 0, initialSwerveRotation, true);
@@ -179,7 +192,12 @@ public class Shoot extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance);
return isAimedInTolerance;
if (isAimedInTolerance && !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2 -6
View File
@@ -82,10 +82,9 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putData("Field", m_field);
}
// https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @link https://github.com/ZachOrr/MK3-Swerve-Example
* @param speeds[0] Speed of the robot in the x direction (forward).
* @param speeds[1] Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
@@ -93,10 +92,7 @@ public class SwerveDrive extends SubsystemBase {
* field.
*/
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
if (speedX == 0 && speedY == 0 && rot == 0)
ignoreAngles = true;
else
ignoreAngles = false;
ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0);
Translation2d speed = new Translation2d(speedX, speedY);
double mag = speed.getNorm();
src/main/java/frc4388/robot/subsystems/Turret.java
+4
View File
@@ -206,6 +206,10 @@ public class Turret extends SubsystemBase {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed);
}
public void runTurretWithCustomPID(double input) {
m_boomBoomRotateMotor.set(input * this.speedLimiter);
}
public void runShooterRotatePID(double targetAngle) {
double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2;