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

Fix Storage Subsystem

Browse Source 
- Also improved the Gains object to have two constructors depending on what kind of output limits you want.
This commit is contained in:
Keenan D. Buckley
2020-03-08 12:14:24 -06:00
parent a7558a7511
commit 0b452d3309
5 changed files with 99 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -4
View File
@@ -164,21 +164,24 @@ public final class Constants {
public static final double STORAGE_SPEED = 0.5;
public static final double STORAGE_TIMEOUT = 2000;
/* 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;
/* 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 {
src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+13 -9
View File
@@ -18,6 +18,7 @@ public class ManageStorage extends CommandBase {
/* Keeps track of which beam breaks are pressed */
boolean isBallInIntake = false;
boolean isBallInStorage = false;
boolean isBallInUseless = false;
boolean isBallInShooter = false;
/* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
@@ -39,9 +40,10 @@ public class ManageStorage extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
isBallInIntake = !m_storage.getBeamIntake();
isBallInStorage = !m_storage.getBeamStorage();
isBallInUseless = !m_storage.getBeamUseless();
isBallInShooter = !m_storage.getBeamShooter();
m_isStorageEmpty = !isBallInStorage;
}
@@ -49,13 +51,15 @@ public class ManageStorage extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
isBallInIntake = !m_storage.getBeamIntake();
isBallInStorage = !m_storage.getBeamStorage();
isBallInUseless = !m_storage.getBeamUseless();
isBallInShooter = !m_storage.getBeamShooter();
SmartDashboard.putBoolean("Ball in Intake", isBallInIntake);
SmartDashboard.putBoolean("Ball in Storage", isBallInStorage);
SmartDashboard.putBoolean("Ball in Shooter", isBallInShooter);
/// TODO: Delete/Comment these when done
SmartDashboard.putBoolean("!Ball in Intake!", isBallInIntake);
SmartDashboard.putBoolean("!Ball Storage!", isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", isBallInShooter);
if (m_storageMode == StorageMode.IDLE) {
runIdle();
src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+1 -1
View File
@@ -33,7 +33,7 @@ public class StoragePrep extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(1)){
if (m_storage.getBeamShooter()){
//m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
src/main/java/frc4388/robot/subsystems/Storage.java
+53 -25
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,10 +41,14 @@ 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
@@ -55,9 +62,8 @@ public class Storage extends SubsystemBase {
/**
* 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,58 @@ 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();
/**
* @param motorRots
* @return inches
*/
public double motorRotsToInches(double motorRots) {
return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
}
public void setStoragePID(double position){
m_storagePIDController.setReference(position , ControlType.kPosition);
/**
* @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();
}
}