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 'master' into GalacticSearch

Browse Source
This commit is contained in:
Ryan
2021-04-15 16:50:23 -06:00
committed by GitHub
parent 69e1c851e7 b3373e68e9
commit dee0b71033
37 changed files with 2146 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+34 -7
View File
@@ -54,6 +54,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;
@@ -96,7 +97,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.
@@ -196,6 +198,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
@@ -281,11 +285,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();
@@ -304,8 +309,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() {
@@ -709,6 +716,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);
@@ -828,6 +845,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());
@@ -835,6 +853,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());
@@ -849,8 +875,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());
@@ -889,12 +913,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) {
@@ -902,4 +927,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;
}
}