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

Shoot Command Works now

Browse Source 
Untested on robot, but in sim
This commit is contained in:
Ryan Manley
2022-03-06 13:02:17 -07:00
parent 645f4bfe07
commit 01957ce6de
4 changed files with 58 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -1
View File
@@ -172,7 +172,8 @@ public final class Constants {
public static final double ENCODER_TICKS_PER_REV = 2048;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_DRIVE_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_TURRET_GAINS = new Gains(2.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
src/main/java/frc4388/robot/RobotContainer.java
+14 -15
View File
@@ -95,10 +95,10 @@ public class RobotContainer {
private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
// private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
// private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
// private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
// private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
// private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
/* Controllers */
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -196,7 +196,6 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whileHeld(new RunCommand(() -> m_robotSerializer.setSerializer(-0.25)))
.whenReleased(new RunCommand(() -> m_robotSerializer.setSerializer(0.0)));
}
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
@@ -210,25 +209,25 @@ public class RobotContainer {
/* Operator Buttons */
// X > Extend Intake
/*new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(() -> m_robotIntake.runExtender(true));
// Y > Retract Intake
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(() -> m_robotIntake.runExtender(false));*/
.whenPressed(() -> m_robotIntake.runExtender(false));
// Right Bumper > Storage In
/*new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// Left Bumper > Storage Out (note: neccessary?)
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// // Left Bumper > Storage Out (note: neccessary?)
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// A > Shoot with Odo
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
// B > Shoot with Lime
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));/*
.whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+3 -3
View File
@@ -163,7 +163,7 @@ public class RobotMap {
// Shooter Config
/* Boom Boom Subsystem */
/*public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
// turret subsystem
@@ -202,7 +202,7 @@ public class RobotMap {
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
ShooterConstants.SHOOTER_TIMEOUT_MS);*/
ShooterConstants.SHOOTER_TIMEOUT_MS);
// /* Turret Subsytem */
// shooterFalconRight.configStatorCurrentLimit(new
@@ -215,7 +215,7 @@ public class RobotMap {
// hood subsystem
// angleAdjusterMotor.restoreFactoryDefaults();
// angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
// }
}
src/main/java/frc4388/robot/commands/Shoot.java
+39 -18
View File
@@ -40,16 +40,20 @@ public class Shoot extends CommandBase {
// pid
public double error;
public double prevError;
public Gains shootGains = ShooterConstants.SHOOT_GAINS;
public Gains driveGains = ShooterConstants.SHOOT_DRIVE_GAINS;
public Gains turretGains = ShooterConstants.SHOOT_TURRET_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;
// testing
public DummySensor dummy = new DummySensor(0);
public DummySensor driveDummy;
public DummySensor turretDummy;
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
@@ -68,9 +72,9 @@ public class Shoot extends CommandBase {
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = shootGains.kP;
kI = shootGains.kI;
kD = shootGains.kD;
kP = driveGains.kP;
kI = driveGains.kI;
kD = driveGains.kD;
proportional = 0;
integral = 0;
@@ -80,6 +84,9 @@ public class Shoot extends CommandBase {
tolerance = 5.0;
isAimedInTolerance = false;
driveDummy = new DummySensor(180);
turretDummy = new DummySensor(180);
DummySensor.resetAll();
}
@@ -87,17 +94,22 @@ public class Shoot extends CommandBase {
* Updates error for custom PID.
*/
public void updateError() {
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
error = (m_targetAngle - turretDummy.get() + 360) % 360;
// error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
isAimedInTolerance = (Math.abs(error) <= tolerance);
SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
System.out.println("Target Angle: " + m_targetAngle);
System.out.println("Error: " + error);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_odoX = m_swerve.getOdometry().getX();
m_odoY = m_swerve.getOdometry().getY();
m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
m_odoX = 0;//m_swerve.getOdometry().getX();
m_odoY = -1;//m_swerve.getOdometry().getY();
m_distance = Math.hypot(m_odoX, m_odoY);//Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
@@ -105,11 +117,6 @@ public class Shoot extends CommandBase {
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
// target angle tests
m_gyroAngle = 0;
m_odoX = -1;
m_odoY = 1;
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
@@ -131,6 +138,13 @@ public class Shoot extends CommandBase {
* Run custom PID.
*/
public void runPID() {
if (error > 180){
error = 360 - error;
inverted = -1;
}
else{
inverted = 1;
}
prevError = error;
updateError();
@@ -138,20 +152,27 @@ public class Shoot extends CommandBase {
integral = integral + error * time;
derivative = (error - prevError) / time;
output = kP * proportional + kI * integral + kD * derivative;
normOutput = output/360 * inverted;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("Normalized Output: " + normOutput);
// custom pid
runPID();
// m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
driveDummy.apply(normOutput);
System.out.println("Drive Dummy: " + driveDummy.get());
m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
// entire swerve drive or its the line below
m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
// m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
turretDummy.apply(normOutput);
System.out.println("Turret Dummy: " + turretDummy.get());
m_turret.m_boomBoomRotateMotor.set(normOutput);
}
// Called once the command ends or is interrupted.
@@ -161,6 +182,6 @@ public class Shoot extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return isAimedInTolerance;
}
}