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

Merge branch 'auto-programming' of https://github.com/Team4388/RiseOfRidgebotics2020 into auto-programming

Browse Source
This commit is contained in:
Aarav Shah
2020-03-10 20:14:09 -06:00
parent 59fa78f6e3 b5155fa341
commit 8bf1eb445f
24 changed files with 530 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+25 -21
View File
@@ -109,12 +109,6 @@ public final class Constants {
public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
}
public static final class IntakeConstants {;
public static final double EXTENDER_SPEED = 0.3;
public static final int INTAKE_SPARK_ID = 12;
public static final int EXTENDER_SPARK_ID = 13;
}
public static final class ShooterConstants {
/* Motor IDs */
@@ -126,8 +120,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(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 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 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;
@@ -160,29 +153,40 @@ public final class Constants {
public static final class LevelerConstants {
public static final int LEVELER_CAN_ID = 15;
}
public static final class IntakeConstants {;
public static final double EXTENDER_SPEED = 0.3;
public static final double INTAKE_SPEED = 1.0;
public static final int INTAKE_SPARK_ID = 12;
public static final int EXTENDER_SPARK_ID = 13;
}
public static final class StorageConstants {
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_TIMEOUT = 2000;
public static final double STORAGE_SPEED = 1.0;
public static final double STORAGE_TIMEOUT = 3000;
/* Storage Characteristics */
public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt
public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt
/* Ball Indexes */
public static final int BEAM_SENSOR_SHOOTER = 1;
public static final int BEAM_SENSOR_USELESS = 2;
public static final int BEAM_SENSOR_STORAGE = 3;
public static final int BEAM_SENSOR_INTAKE = 4;
public static final int BEAM_SENSOR_SHOOTER = 11;
public static final int BEAM_SENSOR_USELESS = 12;
public static final int BEAM_SENSOR_STORAGE = 13;
public static final int BEAM_SENSOR_INTAKE = 14;
/* PID Gains */
public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
/* PID Values */
public static final int SLOT_DISTANCE = 0;
/* PID Indexes */
public static final int PID_PRIMARY = 0;
/* PID Gains */
public static final double STORAGE_MIN_OUTPUT = -1.0;
public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class PneumaticsConstants {
@@ -202,9 +206,9 @@ 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;
public static final double LIME_ANGLE = 25;
public static final double TURN_P_VALUE = 0.65;
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 X_ANGLE_ERROR = 1.3;
public static final double MOTOR_DEAD_ZONE = 0.3;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
src/main/java/frc4388/robot/RobotContainer.java
+20 -8
View File
@@ -38,6 +38,8 @@ import frc4388.robot.commands.auto.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.InterruptSubystem;
import frc4388.robot.commands.auto.AutoPath1FromCenter;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.commands.climber.DisengageRachet;
import frc4388.robot.commands.climber.RunClimberWithTriggers;
@@ -50,6 +52,12 @@ import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.ShootFireGroup;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.shooter.ShootPrepGroup;
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;
@@ -136,7 +144,7 @@ public class RobotContainer {
// runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), 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
@@ -145,7 +153,7 @@ public class RobotContainer {
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_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
@@ -190,17 +198,19 @@ 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)
//.whileHeld(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))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
.whenReleased(new InterruptSubystem(m_robotStorage));
// shoots one ball
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.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))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
.whenReleased(new InterruptSubystem(m_robotStorage));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -220,7 +230,8 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooterAim))
.whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
//.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage))
//.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 HoldTarget(m_robotShooter, m_robotShooterAim))
@@ -247,7 +258,8 @@ public class RobotContainer {
//Run drum
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), 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()));
}
src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java → src/main/java/frc4388/robot/commands/InterruptSubystem.java
+7 -5
View File
@@ -5,16 +5,18 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.storage;
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class StorageIntakeFinal extends CommandBase {
public class InterruptSubystem extends CommandBase {
/**
* Creates a new StorageIntakeFinal.
* Creates a new InterruptSubystem.
*/
public StorageIntakeFinal() {
public InterruptSubystem(SubsystemBase subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@@ -35,6 +37,6 @@ public class StorageIntakeFinal extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return true;
}
}
src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
-75
View File
@@ -1,75 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
public class PrepChecker extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
Storage m_storage;
boolean m_isDrumReady = false;
boolean m_isAimReady = false;
boolean m_isHoodReady = false;
boolean m_isStorageReady = false;
/**
* Creates a new PrepChecker.
* @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands.
* @param storage reads storage in a similar way to shooter. Not used as a requirement.
*/
public PrepChecker(Shooter shooter, Storage storage) {
m_shooter = shooter;
m_shooterAim = m_shooter.m_shooterAimSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
m_storage = storage;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isDrumReady = false;
m_isAimReady = false;
m_isHoodReady = false;
m_isStorageReady = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady);
m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady);
m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady);
m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_isDrumReady = false;
m_shooterAim.m_isAimReady = false;
m_shooterHood.m_isHoodReady = false;
m_storage.m_isStorageReadyToFire = false;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
-36
View File
@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.commands.storage.StorageRun;
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:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootFireGroup extends ParallelRaceGroup {
/**
* Fires the shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
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)
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
-32
View File
@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.SequentialCommandGroup;
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:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootFullGroup extends SequentialCommandGroup {
/**
* Preps and Fires the Shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
addCommands(
new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage)
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+2 -4
View File
@@ -8,7 +8,7 @@
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import frc4388.robot.commands.storage.StoragePrepAim;
import frc4388.robot.commands.storage.StoragePrep;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
@@ -26,11 +26,9 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
*/
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
super(
new PrepChecker(m_shooter, m_storage),
new TrackTarget(m_shooterAim),
new ShooterVelocityControlPID(m_shooter),
new HoodPositionPID(m_shooterHood),
new StoragePrepAim(m_storage)
new HoodPositionPID(m_shooterHood)
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+1 -4
View File
@@ -34,10 +34,7 @@ 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()*/13000);
//m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
//SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
//SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel());
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+14 -7
View File
@@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
@@ -31,6 +32,7 @@ public class TrackTarget extends CommandBase {
double yAngle = 0;
double target = 0;
public double distance;
public double realDistance;
public static double fireVel;
public static double fireAngle;
@@ -48,6 +50,7 @@ public class TrackTarget extends CommandBase {
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
// Called when the command is initially scheduled.
@@ -65,8 +68,14 @@ public class TrackTarget extends CommandBase {
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
// Finding Distance
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;
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
turnAmount = 0;
@@ -79,10 +88,8 @@ public class TrackTarget extends CommandBase {
}
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
SmartDashboard.putNumber("Distance to Target", distance);
SmartDashboard.putNumber("Distance to Target", realDistance);
SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
//START Equation Code
double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
double xVel = (distance * VisionConstants.GRAV) / (yVel);
@@ -92,8 +99,8 @@ public class TrackTarget extends CommandBase {
//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
fireVel = m_shooter.m_shooterTable.getVelocity(realDistance);
fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
@@ -115,7 +122,7 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (xAngle < 1 && xAngle > -1 && target == 1)
if (xAngle < 0.5 && xAngle > -0.5 && target == 1)
{
m_shooterAim.m_isAimReady = true;
} else {
src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+141
View File
@@ -0,0 +1,141 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class ManageStorage extends CommandBase {
Storage m_storage;
/* Timer */
long m_resetStartTime;
/* Keeps track of which beam breaks are pressed */
boolean m_isBallInIntake = false;
boolean m_isBallInStorage = false;
boolean m_isBallInUseless = false;
boolean m_isBallInShooter = false;
/* 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) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
m_storageMode = storageMode;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
m_isStorageEmpty = !m_isBallInStorage;
if (m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
/// TODO: Delete/Comment these when done
SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storageMode == StorageMode.RESET) {
runReset();
}
}
/**
* Intakes a ball.
* Runs until the storage ball has moved past the
* storage sensor and the intake ball has taken its place.
*/
private void runIntake() {
if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty
m_isStorageEmpty = true;
}
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;
}
} else {
m_storageMode = StorageMode.IDLE;
}
}
/**
* Idles until a ball is in the intake.
* Also updates the status of the storage position
*/
private void runIdle() {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Runs Storage out until the Intake Sensor is tripped.
* Then switches into intake mode. This effectively
* resets the position of the balls back to the bottom of the storage.
*/
private void runReset() {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storageMode = StorageMode.IDLE;
}
m_isStorageEmpty = !m_isBallInStorage;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storageMode = StorageMode.RESET;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java
+145
View File
@@ -0,0 +1,145 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class ManageStoragePID extends CommandBase {
Storage m_storage;
/* Timer */
long m_resetStartTime;
/* Start Positions */
double m_intakeStartPos;
/* Keeps track of which beam breaks are pressed */
boolean m_isBallInIntake = false;
boolean m_isBallInStorage = false;
boolean m_isBallInUseless = false;
boolean m_isBallInShooter = false;
/* 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) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
m_storageMode = storageMode;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
m_isStorageEmpty = !m_isBallInStorage;
m_intakeStartPos = m_storage.getEncoderPosInches();
if (m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
/// TODO: Delete/Comment these when done
SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storageMode == StorageMode.RESET) {
runReset();
}
}
/**
* Intakes a ball.
* Runs until the storage ball has moved past the
* storage sensor and the intake ball has taken its place.
*/
private void runIntake() {
if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL);
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;
}
} else {
m_storageMode = StorageMode.IDLE;
}
}
/**
* Idles until a ball is in the intake.
* Also updates the status of the storage position
*/
private void runIdle() {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_intakeStartPos = m_storage.getEncoderPosInches();
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Runs Storage out until the Intake Sensor is tripped.
* Then switches into intake mode. This effectively
* resets the position of the balls back to the bottom of the storage.
*/
private void runReset() {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storageMode = StorageMode.INTAKE;
m_intakeStartPos = m_storage.getEncoderPosInches();
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storageMode = StorageMode.IDLE;
}
m_isStorageEmpty = !m_isBallInStorage;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storageMode = StorageMode.RESET;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StorageIntake.java
-63
View File
@@ -1,63 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
public class StorageIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public boolean intook;
/**
* Runs the Storage in for intaking
* @param inSub The Intake subsystem
* @param storeSub The Storage subsytem
*/
public StorageIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
intook = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
else{
m_storage.runStorage(0);
}
}
// 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_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StorageOutake.java
-46
View File
@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StorageOutake extends CommandBase {
Storage m_storage;
/**
* Runs the Storage out for outaking
* @param storeSub The Storage subsystem
*/
public StorageOutake(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
// 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_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
// 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/storage/StoragePositionPID.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StoragePositionPID extends CommandBase {
public Storage m_storage;
double startPos;
/**
* Moves the storage a number of rotations
* @param subsystem The Storage subsystem
*/
public StoragePositionPID(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
}
// 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_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
}
// 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 (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
{
return true;
}*/
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java → src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+10 -17
View File
@@ -7,19 +7,18 @@
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StoragePrepAim extends CommandBase {
public class StoragePrep extends CommandBase {
Storage m_storage;
double startTime;
double m_startTime;
/**
* Prepares the Storage for aiming
* @param storeSub The Storage subsystem
*/
public StoragePrepAim(Storage storeSub) {
public StoragePrep(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
@@ -27,17 +26,18 @@ public class StoragePrepAim extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
m_startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(1)){
//m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
m_storage.m_isStorageReadyToFire = false;
} else {
m_storage.runStorage(0);
m_storage.m_isStorageReadyToFire = true;
}
}
@@ -49,13 +49,6 @@ 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()){
m_storage.m_isStorageReadyToFire = true;
return true;
} else {
m_storage.m_isStorageReadyToFire = false;
return false;
}*/
return true;
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
-62
View File
@@ -1,62 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
public class StoragePrepIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public double startTime;
/**
* Prepares the Storage for intaking
* @param inSub The Intake subsystem
* @param storeSub the Storage subsystem
*/
public StoragePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
}
}
// 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_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StorageRun.java
-46
View File
@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StorageRun extends CommandBase {
Storage m_storage;
/**
* Runs the Storage at a speed
* @param storageSub The Storage subsytem
*/
public StorageRun(Storage storageSub) {
m_storage = storageSub;
}
// 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_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storage.runStorage(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+34 -1
View File
@@ -17,6 +17,7 @@ import frc4388.utility.LEDPatterns;
public class LED extends SubsystemBase {
public static float currentLED;
public static float defaultLED;
public static Spark LEDController;
/**
@@ -25,7 +26,8 @@ public class LED extends SubsystemBase {
*/
public LED(){
LEDController = new Spark(LEDConstants.LED_SPARK_ID);
setPattern(LEDConstants.DEFAULT_PATTERN);
defaultLED = LEDConstants.DEFAULT_PATTERN.getValue();
runDefaultLED();
LEDController.set(currentLED);
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
@@ -38,6 +40,37 @@ public class LED extends SubsystemBase {
LEDController.set(currentLED);
}
/**
*
*/
public void runDefaultLED() {
setPattern(defaultLED);
}
/**
* Changes the default LED by an amount
* @param amount the amount to increment led by
*/
public void incrementLED(float amount) {
defaultLED += amount;
if (defaultLED > 1) {
defaultLED -= 2;
}
if (defaultLED < -1) {
defaultLED += 2;
}
}
/**
* Sets the current LED pattern. This method should only be run
* whenever you want to change the current LED.
* @param pattern LEDPattern to set the Blinkin to.
*/
public void setPattern(float pattern){
currentLED = pattern;
LEDController.set(pattern);
}
/**
* Sets the current LED pattern. This method should only be run
* whenever you want to change the current LED.
src/main/java/frc4388/robot/subsystems/Shooter.java
+2
View File
@@ -92,6 +92,8 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
}
catch(Exception e)
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+2
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.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
@@ -57,6 +58,7 @@ public class ShooterAim extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
}
/**
src/main/java/frc4388/robot/subsystems/Storage.java
+65 -29
View File
@@ -21,7 +21,10 @@ import frc4388.utility.Gains;
public class Storage extends SubsystemBase {
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -38,26 +41,29 @@ public class Storage extends SubsystemBase {
*/
public Storage() {
resetEncoder();
m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
// Set PID Coefficients
m_storagePIDController.setP(storageGains.m_kP);
m_storagePIDController.setI(storageGains.m_kI);
m_storagePIDController.setD(storageGains.m_kD);
m_storagePIDController.setIZone(storageGains.m_kIzone);
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput);
}
@Override
public void periodic() {
//SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
//SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
//SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
//SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
SmartDashboard.putBoolean("Intake Beam", getBeamIntake());
SmartDashboard.putBoolean("Storage Beam", getBeamStorage());
SmartDashboard.putBoolean("Upper Beam", getBeamUseless());
SmartDashboard.putBoolean("Shooter Beam", getBeamShooter());
}
/**
* Runs storage motor
*
* @param input the voltage to run motor at
* @param input the percent output to run motor at
*/
public void runStorage(double input) {
m_storageMotor.set(input);
}
@@ -66,36 +72,66 @@ public class Storage extends SubsystemBase {
m_encoder.setPosition(0);
}
public void testBeams(){
SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
}
/* Storage PID Control */
/**
* Runs Storage to a particular position
* @param targetPos in inches
*/
public void runStoragePositionPID(double targetPos){
// Set PID Coefficients
m_storagePIDController.setP(storageGains.m_kP);
m_storagePIDController.setI(storageGains.m_kI);
m_storagePIDController.setD(storageGains.m_kD);
m_storagePIDController.setIZone(storageGains.m_kIzone);
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
//SmartDashboard.putNumber("Storage Position PID Target", targetPos);
//SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
targetPos = InchesToMotorRots(targetPos);
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
/**
* Runs Storage to a particular position
* @param position in motor rotations
*/
public void setStoragePID(double position){
m_storagePIDController.setReference(position, ControlType.kPosition);
}
public double getEncoderPos(){
return m_encoder.getPosition();
}
public boolean getBeam(int id){
return m_beamSensors[id].get();
public double getEncoderPosInches(){
return motorRotsToInches(getEncoderPos());
}
public void setStoragePID(double position){
m_storagePIDController.setReference(position , ControlType.kPosition);
public double getEncoderVel(){
return m_encoder.getVelocity();
}
/**
* @param motorRots
* @return inches
*/
public double motorRotsToInches(double motorRots) {
return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
}
/**
* @param inches
* @return motorRots
*/
public double InchesToMotorRots(double inches) {
return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT);
}
public boolean getBeamShooter(){
return m_beamShooter.get();
}
public boolean getBeamUseless(){
return m_beamUseless.get();
}
public boolean getBeamStorage(){
return m_beamStorage.get();
}
public boolean getBeamIntake(){
return m_beamIntake.get();
}
}
src/main/java/frc4388/utility/Gains.java
+25 -1
View File
@@ -27,7 +27,7 @@ public class Gains {
* @param kD The D value.
* @param kF The F value.
* @param kIzone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at.
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
{
@@ -37,5 +37,29 @@ public class Gains {
m_kF = kF;
m_kIzone = kIzone;
m_kPeakOutput = kPeakOutput;
m_kmaxOutput = m_kPeakOutput;
m_kminOutput = -m_kPeakOutput;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIzone The zone of the I value.
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
{
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kminOutput = kMinOutput;
m_kmaxOutput = kMaxOutput;
m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
}
}
src/main/java/frc4388/utility/ShooterTables.java
+23 -1
View File
@@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
* Add your docs here.
*/
public class ShooterTables {
double[][] m_distance = new double[50][3];
double[][] m_distance = new double[50][4];
double[][] m_angle = new double[50][2];
final int m_columnDistance = 0;
final int m_columnHood = 1;
final int m_columnVel = 2;
final int m_columnCenterDisplacement = 3;
final int m_columnAngle = 0;
final int m_columnDisplacement = 1;
@@ -55,6 +56,7 @@ public class ShooterTables {
m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]);
lineNum ++;
}
@@ -80,6 +82,9 @@ public class ShooterTables {
m_angleLength = lineNum-1;
distanceReader.close();
angleReader.close();
} catch (FileNotFoundException e) {
e.printStackTrace();
} catch (IOException e) {
@@ -123,6 +128,23 @@ public class ShooterTables {
}
}
public double getCenterDisplacement(double distance) { //Degrees of Lime FOV
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;
}
if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
return m_distance[i][m_columnCenterDisplacement];
} else {
if (i >= m_distanceLength) {
i = m_distanceLength - 1;
}
return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance);
}
}
public double getAngleDisplacement(double angle) {
int i = 0;
while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {