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

Merge branch 'master' into bouncePath

Browse Source
This commit is contained in:
aarav18
2021-04-22 16:52:18 -06:00
committed by GitHub
parent b26620965e 7f8f57deee
commit 6de50c40d3
16 changed files with 306 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -6
View File
@@ -112,7 +112,8 @@ public final class Constants {
public static final class ShooterConstants {
/* Motor IDs */
public static final int SHOOTER_FALCON_ID = 8;
public static final int SHOOTER_FALCON_BALLER_ID = 8;
public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15;
public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
public static final int SHOOTER_ROTATE_ID = 9;
@@ -120,7 +121,7 @@ public final class Constants {
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final int SHOOTER_TIMEOUT_MS = 30;
public static final Gains DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); //Ff was 0.055
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
@@ -156,7 +157,7 @@ public final class Constants {
}
public static final class LevelerConstants {
public static final int LEVELER_CAN_ID = 15;
public static final int LEVELER_CAN_ID = 30;
}
public static final class IntakeConstants {;
@@ -171,7 +172,7 @@ public final class Constants {
public static final int STORAGE_CAN_ID = 11;
public static final double STORAGE_PARTIAL_BALL = 2;
public static final double STORAGE_FULL_BALL = 7;
public static final double STORAGE_SPEED = 0.5;
public static final double STORAGE_SPEED = 0.75;
public static final double STORAGE_TIMEOUT = 3000;
/* Storage Characteristics */
@@ -211,10 +212,10 @@ public final class Constants {
public static final class VisionConstants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 71.5;
public static final double TARGET_HEIGHT = 67.5;
public static final double LIME_ANGLE = 24.7;
public static final double TURN_P_VALUE = 0.8;
public static final double X_ANGLE_ERROR = 1.3;
public static final double X_ANGLE_ERROR = 0.5;
public static final double MOTOR_DEAD_ZONE = 0.2;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
src/main/java/frc4388/robot/RobotContainer.java
+15 -5
View File
@@ -189,7 +189,7 @@ public class RobotContainer {
// runs the hood with joystick
m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
// moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0), m_robotShooter));
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(12000), m_robotShooter));
// drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// drives the leveler with an axis input from the driver controller
@@ -210,10 +210,17 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
.whenReleased(new InterruptSubystem(m_robotShooter))
.whenReleased(new InterruptSubystem(m_robotShooterHood));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
@@ -254,17 +261,19 @@ public class RobotContainer {
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
//.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
.whenReleased(new InterruptSubystem(m_robotStorage));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
//.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
//.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
@@ -275,10 +284,10 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooterAim))
.whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
//.whenPressed(new StoragePrep(m_robotStorage))
//.whenReleased(new InterruptSubystem(m_robotStorage))
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
//.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID()));
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000)));
//.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
//.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
@@ -291,6 +300,7 @@ public class RobotContainer {
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
//Run drum
new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
+1 -1
View File
@@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase {
}
*/
m_drive.driveWithInput(-moveOutput, steerOutput);
m_drive.driveWithInput(moveOutput, steerOutput * .8);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
+1 -1
View File
@@ -61,7 +61,7 @@ public class CalibrateShooter extends CommandBase {
@Override
public boolean isFinished() {
if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) {
m_shooterHood.m_hoodDownLimit.get()) {
return true;
}
return false;
src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java
+1 -1
View File
@@ -35,7 +35,7 @@ public class ShooterGoalPosition extends CommandBase {
@Override
public void execute() {
m_shooter.runDrumShooterVelocityPID(5000);
m_hood.runAngleAdjustPID(3);
m_hood.runAngleAdjustPID(4);
m_aim.runshooterRotatePID(-26.5);
}
src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java
+3 -3
View File
@@ -34,9 +34,9 @@ public class ShooterTrenchPosition extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.runDrumShooterVelocityPID(5000);
m_hood.runAngleAdjustPID(3);
m_aim.runshooterRotatePID(-26.5);
m_shooter.runDrumShooterVelocityPID(5500);
m_hood.runAngleAdjustPID(11);
//m_aim.runshooterRotatePID(-28);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+1 -1
View File
@@ -35,7 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase {
@Override
public void execute() {
//Tells whether the target velocity has been reached
m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition();
m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition();
m_targetVel = m_shooter.addFireVel();
double error = m_actualVel - m_targetVel;
if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+8 -6
View File
@@ -69,24 +69,26 @@ public class TrackTarget extends CommandBase {
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
realDistance = (1.09 * distance) - 12.8;
if (target == 1.0) { // If target in view
// Aiming Left/Right
xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
turnAmount = 0;
} // Angle Error Zone
// Deadzones
else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = VisionConstants.MOTOR_DEAD_ZONE;
} else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE;
} else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE;
}
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
//m_shooterAim.runshooterRotatePID(targetAngle);
SmartDashboard.putNumber("Distance to Target", realDistance);
SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
src/main/java/frc4388/robot/subsystems/Drive.java
+1 -1
View File
@@ -313,7 +313,7 @@ public class Drive extends SubsystemBase {
public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
m_pneumaticsSubsystem = subsystem;
m_shooter = shooter;
m_orchestra.addInstrument(m_shooter.m_shooterFalcon);
m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
}
public void updateTime() {
src/main/java/frc4388/robot/subsystems/Shooter.java
+46 -25
View File
@@ -21,8 +21,8 @@ import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Shooter m_shooter;
public static IHandController m_controller;
@@ -50,24 +50,42 @@ public class Shooter extends SubsystemBase {
//SmartDashboard.putNumber("Velocity Target", 10000);
//SmartDashboard.putNumber("Angle Target", 3);
m_shooterFalcon.configFactoryDefault();
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(true);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
//LEFT FALCON
m_shooterFalconLeft.configFactoryDefault();
m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
m_shooterFalconLeft.setInverted(true);
m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
//RIGHT FALCON
m_shooterFalconRight.configFactoryDefault();
m_shooterFalconRight.setNeutralMode(NeutralMode.Coast);
m_shooterFalconRight.setInverted(false);
m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
int closedLoopTimeMs = 1;
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
//LEFT FALCON
m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
//RIGHT FALCON
//m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterTable = new ShooterTables();
m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
//SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
//SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
//SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
@@ -83,13 +101,13 @@ public class Shooter extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
try{
SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature());
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
}
@@ -118,18 +136,20 @@ public class Shooter extends SubsystemBase {
* @param speed Speed to set the motor at
*/
public void runDrumShooter(double speed) {
m_shooterFalcon.set(speed);
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
m_shooterFalconRight.follow(m_shooterFalconLeft);
//m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed);
}
/**
* Configures gains for shooter PID.
*/
public void setShooterGains() {
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
@@ -138,6 +158,7 @@ public class Shooter extends SubsystemBase {
*/
public void runDrumShooterVelocityPID(double targetVel) {
//System.out.println("Target Velocity" + targetVel);
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID
m_shooterFalconRight.follow(m_shooterFalconLeft);
}
}