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 autoRevert

Browse Source
This commit is contained in:
Nirvan Bhalala
2021-04-15 16:05:37 -07:00
committed by GitHub
parent d470e37239 b3373e68e9
commit d5473e3204
37 changed files with 2102 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+11 -5
View File
@@ -120,8 +120,9 @@ 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(0.55, 0.0, 100, 0.0, 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 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.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);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double ENCODER_TICKS_PER_REV = 2048;
@@ -135,12 +136,16 @@ public final class Constants {
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
public static final double TURRET_CALIBRATE_SPEED = 0.075;
public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; //89.56696;
public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166;
public static final int HOOD_UP_SOFT_LIMIT = 33;
public static final int HOOD_DOWN_SOFT_LIMIT = 3;
public static final double HOOD_CONVERT_SLOPE = 0.47;
public static final double HOOD_CONVERT_B = 40.5;
public static final double HOOD_CALIBRATE_SPEED = 0.2;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
@@ -166,7 +171,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 = 1.0;
public static final double STORAGE_SPEED = 0.5;
public static final double STORAGE_TIMEOUT = 3000;
/* Storage Characteristics */
@@ -208,9 +213,9 @@ public final class Constants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 71.5;
public static final double LIME_ANGLE = 24.7;
public static final double TURN_P_VALUE = 0.6;
public static final double TURN_P_VALUE = 0.8;
public static final double X_ANGLE_ERROR = 1.3;
public static final double MOTOR_DEAD_ZONE = 0.3;
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;
public static final double GRAV = 385.83;
@@ -219,5 +224,6 @@ public final class Constants {
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final int BUTTON_FOX_ID = 2;
}
}
src/main/java/frc4388/robot/RobotContainer.java
+83 -20
View File
@@ -56,17 +56,23 @@ import frc4388.robot.commands.climber.RunLevelerWithJoystick;
import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
import frc4388.robot.commands.drive.DriveStraightToPositionMM;
import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.PlaySongDrive;
import frc4388.robot.commands.drive.SkipSong;
import frc4388.robot.commands.drive.TurnDegrees;
import frc4388.robot.commands.intake.RunIntakeWithTriggers;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.shooter.RunHoodWithJoystick;
import frc4388.robot.commands.shooter.ShootPrepGroup;
import frc4388.robot.commands.shooter.ShooterGoalPosition;
import frc4388.robot.commands.shooter.ShooterManual;
import frc4388.robot.commands.shooter.ShooterTrenchPosition;
import frc4388.robot.commands.shooter.ShooterVelocityControlPID;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.storage.ManageStorage;
import frc4388.robot.commands.storage.StoragePrep;
import frc4388.robot.commands.storage.ManageStorage.StorageMode;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Drive;
@@ -79,7 +85,10 @@ import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Storage.StorageMode;
import frc4388.utility.controller.ButtonFox;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.JoystickManualButton;
import frc4388.utility.controller.XboxController;
/**
@@ -108,8 +117,10 @@ public class RobotContainer {
public final LimeLight m_robotLime = new LimeLight();
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private static XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID);
private static XboxController m_manualXbox = new XboxController(3);
/* Autos */
double m_totalTimeAuto;
@@ -142,17 +153,20 @@ public class RobotContainer {
SequentialTest m_sequentialTest;
public static boolean m_isShooterManual = false;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
/* Passing Drive and Pneumatics Subsystems */
m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter);
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
m_robotLeveler.passRequiredSubsystem(m_robotClimber);
@@ -171,6 +185,8 @@ public class RobotContainer {
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// 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));
// drives climber with input from triggers on the opperator controller
@@ -180,8 +196,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 ManageStorage(m_robotStorage, StorageMode.IDLE));
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
@@ -194,8 +209,6 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 5000));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -223,6 +236,9 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
.whileHeld(new DisengageRachet(m_robotClimber));
/* Operator Buttons */
// shoots until released
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -273,25 +289,54 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.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)));
//.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)));
//Run drum
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
//Run drum manual
new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true)
.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000)))
.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
/* Button Fox */
// Storage Manual
new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH)
.whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL)))
.whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET)));
// Meg
new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH)
.whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter))
.whenReleased(new InterruptSubystem(m_robotDrive));
// Shooter Manual
new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH)
.whileHeld(new ShooterManual(true))
.whenReleased(new ShooterManual(false));
// Goal Shooter Position
new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON)
.whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter))
.whenReleased(new InterruptSubystem(m_robotDrive));
// Trench Shooter Position
new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON)
.whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
.whenReleased(new InterruptSubystem(m_robotShooter))
.whenReleased(new InterruptSubystem(m_robotShooterHood))
.whenReleased(new InterruptSubystem(m_robotShooterAim));
//.whenPressed(new SkipSong(m_robotDrive, 1));
}
public void buildAutos() {
//resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
};
@@ -368,6 +413,7 @@ public class RobotContainer {
m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
String[] tenBallAutoMiddlePaths = new String[]{
"SixBallMidComplete",
"TenBallMidComplete"
};
@@ -541,6 +587,12 @@ public class RobotContainer {
return m_operatorXbox;
}
public IHandController getButtonFoxObject()
{
return m_buttonFox;
}
/**
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
@@ -560,4 +612,15 @@ public class RobotContainer {
{
return m_driverXbox.getJoyStick();
}
/**
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Button Fox.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
* @return The IHandController interface for the Button Fox.
*/
public Joystick getButtonFox()
{
return m_buttonFox.getJoyStick();
}
}
src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java
+33 -2
View File
@@ -7,9 +7,21 @@
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.PrepChecker;
import frc4388.robot.commands.shooter.ShootPrepGroup;
import frc4388.robot.commands.storage.RunStorage;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
@@ -18,11 +30,30 @@ public class TenBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new TenBallAutoMiddle.
*/
public TenBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
new ParallelDeadlineGroup(
new Wait(drive, 0.1, 0),
new CalibrateShooter(shooter, shooterAim, shooterHood)
),
new ParallelDeadlineGroup(
new Wait(drive, 1, 0),
new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim)
),
new ParallelDeadlineGroup(
new Wait(drive, 4, 0),
new PrepChecker(shooter, shooterAim),
new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake),
new ShootPrepGroup(shooter, shooterAim, shooterHood, storage)
),
new ParallelDeadlineGroup(
new ShootPrepGroup(shooter, shooterAim, shooterHood, storage),
new RunStorage(storage)
)
//paths[0],
//paths[1]
);
}
}
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java
+2 -2
View File
@@ -33,9 +33,9 @@ public class DriveStraightToPositionMM extends CommandBase {
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java
+2 -2
View File
@@ -31,9 +31,9 @@ public class DriveStraightToPositionPID extends CommandBase {
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
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);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java
+3 -2
View File
@@ -9,6 +9,7 @@ package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
public class PlaySongDrive extends CommandBase {
private Drive m_drive;
@@ -16,10 +17,10 @@ public class PlaySongDrive extends CommandBase {
/**
* Creates a new PlaySongDrive.
*/
public PlaySongDrive(Drive subsystem) {
public PlaySongDrive(Drive subsystem, Shooter shooter) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
addRequirements(m_drive);
addRequirements(m_drive, shooter);
}
// Called when the command is initially scheduled.
src/main/java/frc4388/robot/commands/drive/SkipSong.java
+56
View File
@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
public class SkipSong extends CommandBase {
Drive m_drive;
int m_index;
/**
* Creates a new SkipSong.
*/
public SkipSong(Drive m_robotDrive, int index) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = m_robotDrive;
m_index = index;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
String[] songs = m_drive.songsStrings;
String song = m_drive.m_currentSong;
for (int i = 0; i < songs.length; i++) {
if (songs[i] == song) {
m_drive.selectSong(songs[i + m_index]);
break;
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return true;
}
}
src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
+5
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands.shooter;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -59,6 +60,10 @@ public class CalibrateShooter extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
+52
View File
@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
public class PrepChecker extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
/**
* Creates a new PrepChecker.
*/
public PrepChecker(Shooter shooter, ShooterAim shooterAim) {
// Use addRequirements() here to declare subsystem dependencies.
m_shooter = shooter;
m_shooterAim = shooterAim;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_shooterAim.m_isAimReady && m_shooter.m_isDrumReady) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java
+54
View File
@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class RunHoodWithJoystick extends CommandBase {
private ShooterHood m_hood;
private IHandController m_controller;
/**
* Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
* @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) {
m_hood = subsystem;
m_controller = controller;
addRequirements(m_hood);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double input = m_controller.getRightYAxis();
m_hood.runHood(input);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java
+52
View File
@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
public class ShooterGoalPosition extends CommandBase {
Shooter m_shooter;
ShooterHood m_hood;
ShooterAim m_aim;
/**
* Creates a new ShooterGoalPosition.
*/
public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
m_shooter = shooterSub;
m_hood = hoodSub;
m_aim = aimSub;
addRequirements(m_shooter,m_hood,m_aim);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// 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);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterManual.java
+45
View File
@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
public class ShooterManual extends CommandBase {
public boolean isManual = false;
/**
* Creates a new ShooterManual.
*/
public ShooterManual(boolean man) {
isManual = man;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
RobotContainer.m_isShooterManual = isManual;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
RobotContainer.m_isShooterManual = isManual;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java
+52
View File
@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
public class ShooterTrenchPosition extends CommandBase {
Shooter m_shooter;
ShooterHood m_hood;
ShooterAim m_aim;
/**
* Creates a new ShooterTrenchPosition.
*/
public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
m_shooter = shooterSub;
m_hood = hoodSub;
m_aim = aimSub;
addRequirements(m_shooter,m_hood,m_aim);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// 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);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+12 -11
View File
@@ -34,7 +34,18 @@ public class ShooterVelocityControlPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel());
//Tells whether the target velocity has been reached
m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition();
m_targetVel = m_shooter.addFireVel();
double error = m_actualVel - m_targetVel;
if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){
m_shooter.m_isDrumReady = true;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
else{
m_shooter.m_isDrumReady = false;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
}
// Called once the command ends or is interrupted.
@@ -45,16 +56,6 @@ public class ShooterVelocityControlPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
//Tells whether the target velocity has been reached
double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
if (m_actualVel < upperBound && m_actualVel > lowerBound){
m_shooter.m_isDrumReady = true;
}
else{
m_shooter.m_isDrumReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+1
View File
@@ -110,6 +110,7 @@ public class TrackTarget extends CommandBase {
m_shooter.m_fireVel = fireVel;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterAim.m_targetDistance = distance;
}
}
src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+21 -15
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Storage.StorageMode;
public class ManageStorage extends CommandBase {
Storage m_storage;
@@ -26,17 +27,13 @@ public class ManageStorage extends CommandBase {
/* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
boolean m_isStorageEmpty = true;
public enum StorageMode{IDLE, INTAKE, RESET};
StorageMode m_storageMode = StorageMode.IDLE;
/**
* Creates a new ManageStorage.
*/
public ManageStorage(Storage m_robotStorage, StorageMode storageMode) {
public ManageStorage(Storage m_robotStorage) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
m_storageMode = storageMode;
addRequirements(m_storage);
}
@@ -50,7 +47,7 @@ public class ManageStorage extends CommandBase {
m_isStorageEmpty = !m_isBallInStorage;
if (m_storageMode == StorageMode.RESET) {
if (m_storage.m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
@@ -68,12 +65,14 @@ public class ManageStorage extends CommandBase {
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storageMode == StorageMode.IDLE) {
if (m_storage.m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storageMode == StorageMode.INTAKE) {
} else if (m_storage.m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storageMode == StorageMode.RESET) {
} else if (m_storage.m_storageMode == StorageMode.RESET) {
runReset();
} else if (m_storage.m_storageMode == StorageMode.MANUAL) {
runManual();
}
}
@@ -91,10 +90,10 @@ public class ManageStorage extends CommandBase {
}
if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
m_isStorageEmpty = false;
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
} else {
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
}
@@ -106,7 +105,7 @@ public class ManageStorage extends CommandBase {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_storage.changeStorageMode(StorageMode.INTAKE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
@@ -120,17 +119,24 @@ public class ManageStorage extends CommandBase {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_storage.changeStorageMode(StorageMode.INTAKE);
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Switches Storage to Manual only
*/
private void runManual() {
m_storage.runStorage(0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storageMode = StorageMode.RESET;
m_storage.changeStorageMode(StorageMode.RESET);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java
+12 -14
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Storage.StorageMode;
public class ManageStoragePID extends CommandBase {
Storage m_storage;
@@ -30,16 +31,13 @@ public class ManageStoragePID extends CommandBase {
/* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
boolean m_isStorageEmpty = true;
public enum StorageMode{IDLE, INTAKE, RESET};
StorageMode m_storageMode = StorageMode.IDLE;
/**
* Creates a new ManageStorage.
*/
public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) {
public ManageStoragePID(Storage m_robotStorage) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
m_storageMode = storageMode;
addRequirements(m_storage);
}
@@ -55,7 +53,7 @@ public class ManageStoragePID extends CommandBase {
m_intakeStartPos = m_storage.getEncoderPosInches();
if (m_storageMode == StorageMode.RESET) {
if (m_storage.m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
@@ -73,11 +71,11 @@ public class ManageStoragePID extends CommandBase {
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storageMode == StorageMode.IDLE) {
if (m_storage.m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storageMode == StorageMode.INTAKE) {
} else if (m_storage.m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storageMode == StorageMode.RESET) {
} else if (m_storage.m_storageMode == StorageMode.RESET) {
runReset();
}
}
@@ -93,10 +91,10 @@ public class ManageStoragePID extends CommandBase {
double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
} else {
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
}
@@ -108,7 +106,7 @@ public class ManageStoragePID extends CommandBase {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_storage.changeStorageMode(StorageMode.INTAKE);
m_intakeStartPos = m_storage.getEncoderPosInches();
}
m_isStorageEmpty = !m_isBallInStorage;
@@ -123,10 +121,10 @@ public class ManageStoragePID extends CommandBase {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_storage.changeStorageMode(StorageMode.INTAKE);
m_intakeStartPos = m_storage.getEncoderPosInches();
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storageMode = StorageMode.IDLE;
m_storage.changeStorageMode(StorageMode.IDLE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
@@ -134,7 +132,7 @@ public class ManageStoragePID extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storageMode = StorageMode.RESET;
m_storage.changeStorageMode(StorageMode.RESET);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/subsystems/Drive.java
+34 -7
View File
@@ -55,6 +55,7 @@ public class Drive extends SubsystemBase {
/* Pneumatics Subsystem */
public Pneumatics m_pneumaticsSubsystem;
Shooter m_shooter;
/* Low Gear Gains */
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
@@ -97,7 +98,8 @@ public class Drive extends SubsystemBase {
SendableChooser<String> m_songChooser = new SendableChooser<String>();
/* Misc */
String m_currentSong = "";
public String m_currentSong = "";
public String[] songsStrings;
/**
* Add your docs here.
@@ -197,6 +199,8 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSelectedFeedbackCoefficient(0.5, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
/*
* Configure the Pigeon IMU to the other Remote Slot available on the right
* Talon
@@ -282,11 +286,12 @@ public class Drive extends SubsystemBase {
/* Create chooser to choose song to play */
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
String[] songsStrings = songsDir.list();
songsStrings = songsDir.list();
for (String songString : songsStrings) {
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
selectSong(songsStrings[0]);
/* Start counting time */
m_lastTimeMs = System.currentTimeMillis();
@@ -305,8 +310,10 @@ public class Drive extends SubsystemBase {
*
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Pneumatics subsystem) {
public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
m_pneumaticsSubsystem = subsystem;
m_shooter = shooter;
m_orchestra.addInstrument(m_shooter.m_shooterFalcon);
}
public void updateTime() {
@@ -726,6 +733,16 @@ public class Drive extends SubsystemBase {
return meters * DriveConstants.INCHES_PER_METER;
}
/**
* Converts a value in inches per second to miles per hour
* @param ips The value in inches per second to convert
* @return The value in miles per hour
*/
public double inchesPerSecondToMilesPerHour(double ips) {
double mph = ips * (3600) * (1/63360);
return mph;
}
public void setRightMotorGains(boolean isHighGear) {
if (!isHighGear) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -845,6 +862,7 @@ public class Drive extends SubsystemBase {
public void updateSmartDashboard() {
try {
// SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
// SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
// SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
@@ -852,6 +870,14 @@ public class Drive extends SubsystemBase {
SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro);
SmartDashboard.putData("Drive Train", m_driveTrain);
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition());
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
SmartDashboard.putNumber("Average Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
double avgSpeedMPH = inchesPerSecondToMilesPerHour(10 * ticksToInches(m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)));
SmartDashboard.putNumber("Avg Speed MPH", avgSpeedMPH);
//SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
//SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
@@ -866,8 +892,6 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
//SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
@@ -906,12 +930,13 @@ public class Drive extends SubsystemBase {
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
//SmartDashboard.putNumber("Odometry Heading", getHeading());
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
if (m_currentSong != m_songChooser.getSelected()){
m_currentSong = m_songChooser.getSelected();
selectSong(m_currentSong);
//System.err.println(m_currentSong);
}
} catch (Exception e) {
@@ -919,4 +944,6 @@ public class Drive extends SubsystemBase {
// e.printStackTrace(System.err);
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+6 -8
View File
@@ -54,7 +54,7 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(true);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -83,17 +83,15 @@ 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_shooterFalcon.getSelectedSensorVelocity());
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
}
catch(Exception e)
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+77 -3
View File
@@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
@@ -24,10 +25,14 @@ import frc4388.utility.Gains;
public class ShooterAim extends SubsystemBase {
public Shooter m_shooterSubsystem;
public Drive m_driveSubsystem;
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
public GyroBase m_turretGyro;
public double m_targetDistance = 0;
public boolean m_isAimReady = false;
@@ -42,6 +47,8 @@ public class ShooterAim extends SubsystemBase {
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_turretGyro = getGyroInterface();
m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit.enableLimitSwitch(true);
@@ -58,6 +65,10 @@ public class ShooterAim extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition());
SmartDashboard.putData("Turret Angle", m_turretGyro);
SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
}
@@ -65,8 +76,9 @@ public class ShooterAim extends SubsystemBase {
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Shooter subsystem) {
m_shooterSubsystem = subsystem;
public void passRequiredSubsystem(Shooter subsystem0, Drive subsystem1) {
m_shooterSubsystem = subsystem0;
m_driveSubsystem = subsystem1;
}
public void runShooterWithInput(double input) {
@@ -98,6 +110,68 @@ public class ShooterAim extends SubsystemBase {
public double getShooterRotatePosition()
{
return m_shooterRotateMotor.getEncoder().getPosition();
return m_shooterRotateEncoder.getPosition();
}
public double getShooterAngleDegrees() {
return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
}
/**
* Gets the angle of the Shooter relative to the target.
*/
public double getTargetAngleDegrees() {
return m_driveSubsystem.getHeading() + getShooterAngleDegrees();
}
public double getTargetXDisplacement() {
return m_targetDistance * Math.cos(getTargetAngleDegrees());
}
public double getTargetYDisplacement() {
return m_targetDistance * Math.sin(getTargetAngleDegrees());
}
/**
* Gets the angle of the Shooter relative to the inner port.
* Use for tuning the Shooter Displacement
*/
public double getInnerPortAngleDegrees() {
return Math.atan( getTargetYDisplacement() / (getTargetXDisplacement() + 29.25) );
}
public GyroBase getGyroInterface() {
return new GyroBase(){
@Override
public void close() throws Exception {
// TODO Auto-generated method stub
}
@Override
public void reset() {
// TODO Auto-generated method stub
}
@Override
public double getRate() {
// TODO Auto-generated method stub
return 0;
}
@Override
public double getAngle() {
// TODO Auto-generated method stub
return getShooterAngleDegrees();
}
@Override
public void calibrate() {
// TODO Auto-generated method stub
}
};
}
}
src/main/java/frc4388/robot/subsystems/ShooterHood.java
+11
View File
@@ -58,6 +58,8 @@ public class ShooterHood extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle);
SmartDashboard.putNumber("Hood Angle Raw", getAnglePositionDegrees());
}
/**
@@ -86,6 +88,11 @@ public class ShooterHood extends SubsystemBase {
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
public void runHood(double input)
{
m_angleAdjustMotor.set(input);
}
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
@@ -93,4 +100,8 @@ public class ShooterHood extends SubsystemBase {
public double getAnglePosition(){
return m_angleEncoder.getPosition();
}
public double getAnglePositionDegrees(){
return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+7
View File
@@ -36,6 +36,9 @@ public class Storage extends SubsystemBase {
public boolean m_isStorageReadyToFire = false;
public enum StorageMode{IDLE, INTAKE, RESET, MANUAL};
public StorageMode m_storageMode = StorageMode.IDLE;
/**
* Creates a new Storage.
*/
@@ -134,4 +137,8 @@ public class Storage extends SubsystemBase {
public boolean getBeamIntake(){
return m_beamIntake.get();
}
public void changeStorageMode(StorageMode storageMode){
m_storageMode = storageMode;
}
}