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

SHOOTER WORKS SEMI MANUAL

Browse Source
This commit is contained in:
ryan123rudder
2020-03-03 20:59:00 -07:00
parent b8a6e59fac
commit 2f6b219303
14 changed files with 71 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -1
View File
@@ -125,12 +125,15 @@ public final class Constants {
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 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, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double ENCODER_TICKS_PER_REV = 2048;
public static final double NEO_UNITS_PER_REV = 42;
public static final double DEGREES_PER_ROT = 360;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
public static final int TURRET_RIGHT_SOFT_LIMIT = -2;
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
src/main/java/frc4388/robot/RobotContainer.java
+26 -19
View File
@@ -36,9 +36,12 @@ import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.TurnDegrees;
import frc4388.robot.commands.intake.RunIntakeWithTriggers;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.ShootFireGroup;
import frc4388.robot.commands.shooter.ShootFullGroup;
import frc4388.robot.commands.shooter.ShootPrepGroup;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.storage.StorageIntake;
import frc4388.robot.commands.storage.StoragePrepIntake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Climber;
@@ -119,6 +122,7 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the storage not
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
@@ -138,7 +142,6 @@ public class RobotContainer {
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new TurnDegrees(m_robotDrive, 90));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new Wait(m_robotDrive, 0, 0));
@@ -163,15 +166,17 @@ public class RobotContainer {
/* Operator Buttons */
// shoots until released
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)));
//.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// shoots one ball
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)));
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -188,8 +193,10 @@ public class RobotContainer {
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage))
.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
.whileHeld(new TrackTarget(m_robotShooterAim))
.whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
//.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage))
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
//.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
//.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
@@ -203,20 +210,20 @@ public class RobotContainer {
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
//Prepares storage for intaking
new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
//.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
//Runs storage to outtake
new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
//.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//Run drum
//new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
//.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
//.whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
//.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
}
/**
src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
-5
View File
@@ -40,8 +40,6 @@ public class CalibrateShooter extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooterHood.m_angleEncoder.setPosition(0);
m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
@@ -54,9 +52,6 @@ public class CalibrateShooter extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
}
src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java
+4 -3
View File
@@ -31,9 +31,10 @@ public class HoodPositionPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
double b = ShooterConstants.HOOD_CONVERT_B;
firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
//double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
//double b = ShooterConstants.HOOD_CONVERT_B;
//firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
firingAngle = m_shooterHood.addFireAngle();
//SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
//SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooterHood.runAngleAdjustPID(firingAngle);
src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+2 -2
View File
@@ -29,8 +29,8 @@ public class ShootFireGroup extends ParallelRaceGroup {
addCommands(
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
new TrackTarget(m_shooterAim),
new StorageRun(m_storage)
new TrackTarget(m_shooterAim)
//new StorageRun(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
-1
View File
@@ -31,7 +31,6 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
new ShooterVelocityControlPID(m_shooter),
new HoodPositionPID(m_shooterHood),
new StoragePrepAim(m_storage)
//new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
);
}
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+5 -2
View File
@@ -88,15 +88,18 @@ public class TrackTarget extends CommandBase {
double xVel = (distance * VisionConstants.GRAV) / (yVel);
//fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//END Equation Code
//START CSV Code
fireVel = m_shooter.m_shooterTable.getVelocity(distance);
//fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
//fireVel = SmartDashboard.getNumber("Velocity Target", 0);
//fireAngle = SmartDashboard.getNumber("Angle Target", 3);
m_shooter.m_fireVel = fireVel;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
src/main/java/frc4388/robot/commands/storage/StorageIntake.java
+1 -1
View File
@@ -38,7 +38,7 @@ public class StorageIntake extends CommandBase {
@Override
public void execute() {
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java
+4 -3
View File
@@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase {
@Override
public void execute() {
if (m_storage.getBeam(1)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
//m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
@@ -49,12 +49,13 @@ public class StoragePrepAim extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
/*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
m_storage.m_isStorageReadyToFire = true;
return true;
} else {
m_storage.m_isStorageReadyToFire = false;
return false;
}
}*/
return true;
}
}
src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
+1 -1
View File
@@ -55,7 +55,7 @@ public class StoragePrepIntake extends CommandBase {
@Override
public boolean isFinished() {
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return true;
return false;
}
return false;
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+10 -2
View File
@@ -47,12 +47,14 @@ public class Shooter extends SubsystemBase {
//Testing purposes reseting gyros
//resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
//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(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -64,6 +66,8 @@ public class Shooter extends SubsystemBase {
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,6 +87,10 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
}
@@ -129,7 +137,7 @@ public class Shooter extends SubsystemBase {
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel) {
System.out.println("Target Velocity" + targetVel);
//System.out.println("Target Velocity" + targetVel);
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+2
View File
@@ -50,6 +50,8 @@ public class ShooterAim extends SubsystemBase {
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
m_shooterRotateMotor.setInverted(false);
}
@Override
src/main/java/frc4388/robot/subsystems/ShooterHood.java
+4 -4
View File
@@ -48,10 +48,10 @@ public class ShooterHood extends SubsystemBase {
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override