mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
Merge branch 'master' into auto-programming
This commit is contained in:
Keenan D. Buckley
@@ -7,11 +7,9 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.cscore.MjpegServer;
|
||||
import edu.wpi.cscore.UsbCamera;
|
||||
import edu.wpi.cscore.VideoSource;
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
|
||||
|
||||
@@ -13,6 +13,8 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
@@ -20,16 +22,21 @@ public class Climber extends SubsystemBase {
|
||||
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
|
||||
CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
|
||||
|
||||
Servo m_servo;
|
||||
//Spark m_spark = new Spark(4);
|
||||
|
||||
public boolean climberSafety = false;
|
||||
|
||||
/**
|
||||
* Creates a new Climber.
|
||||
*/
|
||||
public Climber() {
|
||||
m_servo = new Servo(4);
|
||||
|
||||
m_climberMotor.restoreFactoryDefaults();
|
||||
|
||||
m_climberMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_climberMotor.setInverted(false);
|
||||
m_climberMotor.setInverted(true);
|
||||
|
||||
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
@@ -48,6 +55,7 @@ public class Climber extends SubsystemBase {
|
||||
*/
|
||||
public void runClimber(double input) {
|
||||
if(climberSafety){
|
||||
input *= 1.0;
|
||||
m_climberMotor.set(input);
|
||||
}
|
||||
else{
|
||||
@@ -66,4 +74,15 @@ public class Climber extends SubsystemBase {
|
||||
{
|
||||
climberSafety = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @param shift true to enage rachet, false to disengage
|
||||
*/
|
||||
public void shiftServo(boolean shift) {
|
||||
if (shift) {
|
||||
m_servo.setPosition(0.5);
|
||||
} else {
|
||||
m_servo.setPosition(0.56);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,7 +24,6 @@ import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
|
||||
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
@@ -34,10 +33,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Constants.PneumaticsConstants;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class Drive extends SubsystemBase {
|
||||
/* Create Motors, Gyros, etc */
|
||||
@@ -53,7 +50,7 @@ public class Drive extends SubsystemBase {
|
||||
public Orchestra m_orchestra;
|
||||
|
||||
/* Pneumatics Subsystem */
|
||||
Pneumatics m_pneumaticsSubsystem;
|
||||
public Pneumatics m_pneumaticsSubsystem;
|
||||
|
||||
/* Low Gear Gains */
|
||||
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
|
||||
@@ -736,16 +733,16 @@ public class Drive extends SubsystemBase {
|
||||
//SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
|
||||
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
|
||||
//SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
|
||||
//SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
|
||||
//SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
//SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Left Motor RPM", leftRPM);
|
||||
SmartDashboard.putNumber("Right Motor RPM", rightRPM);
|
||||
//SmartDashboard.putNumber("Left Motor RPM", leftRPM);
|
||||
//SmartDashboard.putNumber("Right Motor RPM", rightRPM);
|
||||
|
||||
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
@@ -754,8 +751,8 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
//SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
|
||||
SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
|
||||
//SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
|
||||
//SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
|
||||
|
||||
/*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
@@ -763,30 +760,30 @@ public class Drive extends SubsystemBase {
|
||||
SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
*/
|
||||
|
||||
SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
//SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
//SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
|
||||
|
||||
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
|
||||
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
|
||||
//SmartDashboard.putNumber("Odometry Heading", getHeading());
|
||||
|
||||
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
|
||||
|
||||
if (m_currentSong != m_songChooser.getSelected()){
|
||||
|
||||
@@ -8,12 +8,11 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.IntakeConstants;
|
||||
|
||||
@@ -38,10 +37,10 @@ public class Intake extends SubsystemBase {
|
||||
m_intakeMotor.setIdleMode(IdleMode.kCoast);
|
||||
m_extenderMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_intakeMotor.setInverted(false);
|
||||
m_extenderMotor.setInverted(false);
|
||||
m_extenderMotor.setInverted(true);
|
||||
|
||||
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
|
||||
m_extenderForwardLimit.enableLimitSwitch(true);
|
||||
m_extenderReverseLimit.enableLimitSwitch(true);
|
||||
}
|
||||
@@ -58,18 +57,24 @@ public class Intake extends SubsystemBase {
|
||||
public void runIntake(double input) {
|
||||
m_intakeMotor.set(input);
|
||||
}
|
||||
|
||||
|
||||
public void runExtender(double input){
|
||||
m_extenderMotor.set(input);
|
||||
}
|
||||
/**
|
||||
* Runs extender motor
|
||||
* @param input the percent output to run motor at
|
||||
*/
|
||||
public void runExtender(double input) {
|
||||
/*public void runExtender(double input) {
|
||||
if (m_extenderForwardLimit.get()) {
|
||||
isExtended = true;
|
||||
}
|
||||
if (m_extenderReverseLimit.get()) {
|
||||
else if (m_extenderReverseLimit.get()) {
|
||||
isExtended = false;
|
||||
}
|
||||
else{
|
||||
m_extenderMotor.set(-input);
|
||||
}
|
||||
|
||||
if (isExtended == false) {
|
||||
m_extenderMotor.set(input);
|
||||
@@ -77,5 +82,5 @@ public class Intake extends SubsystemBase {
|
||||
if (isExtended == true) {
|
||||
m_extenderMotor.set(-input);
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
@@ -50,6 +50,6 @@ public class LED extends SubsystemBase {
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", currentLED);
|
||||
//SmartDashboard.putNumber("LED", currentLED);
|
||||
}
|
||||
}
|
||||
@@ -7,22 +7,17 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.Talon;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.LevelerConstants;
|
||||
import frc4388.robot.subsystems.*;
|
||||
|
||||
public class Leveler extends SubsystemBase {
|
||||
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
private final Climber m_robotClimber = new Climber();
|
||||
Climber m_climberSubsystem;
|
||||
|
||||
/**
|
||||
* Creates a new Leveler.
|
||||
@@ -44,11 +39,19 @@ public class Leveler extends SubsystemBase {
|
||||
* @param input the percent output to run motor at
|
||||
*/
|
||||
public void runLeveler(double input) {
|
||||
if(m_robotClimber.climberSafety){
|
||||
if(m_climberSubsystem.climberSafety){
|
||||
m_levelerMotor.set(input);
|
||||
}
|
||||
else{
|
||||
m_levelerMotor.set(0);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Passes subsystem needed.
|
||||
* @param subsystem Subsystem needed.
|
||||
*/
|
||||
public void passRequiredSubsystem(Climber subsystem) {
|
||||
m_climberSubsystem = subsystem;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,9 +8,7 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.Constants.PneumaticsConstants;
|
||||
|
||||
public class Pneumatics extends SubsystemBase {
|
||||
|
||||
@@ -11,50 +11,34 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMax.SoftLimitDirection;
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.ControlType;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Trims;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.ShooterTables;
|
||||
import frc4388.utility.Trims;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
|
||||
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
|
||||
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
|
||||
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
|
||||
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
|
||||
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
|
||||
public static Shooter m_shooter;
|
||||
public static IHandController m_controller;
|
||||
|
||||
double velP;
|
||||
double input;
|
||||
|
||||
ShooterTables m_shooterTable;
|
||||
public ShooterTables m_shooterTable;
|
||||
|
||||
public boolean velReached;
|
||||
public boolean m_isDrumReady = false;
|
||||
public double m_fireVel;
|
||||
public double m_fireAngle;
|
||||
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
|
||||
|
||||
public Trims shooterTrims;
|
||||
|
||||
public ShooterHood m_shooterHoodSubsystem;
|
||||
public ShooterAim m_shooterAimSubsystem;
|
||||
|
||||
/*
|
||||
* Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
|
||||
@@ -63,12 +47,14 @@ public class Shooter extends SubsystemBase {
|
||||
//Testing purposes reseting gyros
|
||||
//resetGyroAngleAdj();
|
||||
shooterTrims = new Trims(0, 0);
|
||||
//SmartDashboard.putNumber("Velocity Target", 10000);
|
||||
//SmartDashboard.putNumber("Angle Target", 3);
|
||||
|
||||
m_shooterFalcon.configFactoryDefault();
|
||||
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
|
||||
m_shooterFalcon.setInverted(true);
|
||||
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
|
||||
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
setShooterGains();
|
||||
|
||||
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
@@ -77,47 +63,55 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
int closedLoopTimeMs = 1;
|
||||
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterTable = new ShooterTables();
|
||||
|
||||
SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
|
||||
SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
|
||||
SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
|
||||
SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
|
||||
m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
|
||||
SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
|
||||
SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
|
||||
SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
|
||||
//SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
|
||||
//SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
|
||||
//SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
|
||||
//SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
|
||||
|
||||
|
||||
|
||||
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_hoodUpLimit.enableLimitSwitch(true);
|
||||
m_hoodDownLimit.enableLimitSwitch(true);
|
||||
|
||||
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
|
||||
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
|
||||
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
|
||||
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
|
||||
|
||||
//SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
|
||||
//SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
|
||||
//SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
|
||||
//SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
|
||||
try{
|
||||
SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
|
||||
|
||||
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
|
||||
|
||||
SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
|
||||
|
||||
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
|
||||
}
|
||||
|
||||
catch(Exception e)
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Passes subsystem needed.
|
||||
* @param subsystem Subsystem needed.
|
||||
*/
|
||||
public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) {
|
||||
m_shooterHoodSubsystem = subsystem0;
|
||||
m_shooterAimSubsystem = subsystem1;
|
||||
}
|
||||
|
||||
public double addFireVel() {
|
||||
return m_fireVel;
|
||||
}
|
||||
public double addFireAngle() {
|
||||
return m_fireAngle;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Runs drum shooter motor.
|
||||
@@ -138,41 +132,12 @@ public class Shooter extends SubsystemBase {
|
||||
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Angle Adjustment PID Control */
|
||||
public void runAngleAdjustPID(double targetAngle)
|
||||
{
|
||||
// Set PID Coefficients
|
||||
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
|
||||
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
|
||||
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
|
||||
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
|
||||
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
|
||||
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
|
||||
|
||||
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs drum shooter velocity PID.
|
||||
* @param falcon Motor to use
|
||||
* @param targetVel Target velocity to run motor at
|
||||
*/
|
||||
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
|
||||
SmartDashboard.putNumber("shoot", actualVel);
|
||||
if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
|
||||
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
|
||||
}
|
||||
else{ //PID Based on targetVel
|
||||
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
|
||||
}
|
||||
m_shooterFalcon.feed();
|
||||
}
|
||||
|
||||
public void resetGyroAngleAdj(){
|
||||
m_angleEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getAnglePosition(){
|
||||
return m_angleAdjustMotor.getEncoder().getPosition();
|
||||
public void runDrumShooterVelocityPID(double targetVel) {
|
||||
//System.out.println("Target Velocity" + targetVel);
|
||||
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
|
||||
}
|
||||
}
|
||||
@@ -8,30 +8,31 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.ControlType;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMax.SoftLimitDirection;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
import com.revrobotics.ControlType;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class ShooterAim extends SubsystemBase {
|
||||
public Shooter m_shooterSubsystem;
|
||||
|
||||
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;
|
||||
|
||||
// Configure PID Controllers
|
||||
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
|
||||
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
|
||||
public boolean m_isAimReady = false;
|
||||
|
||||
// Configure PID Controllers
|
||||
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
|
||||
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
|
||||
|
||||
/**
|
||||
* Creates a subsytem for the turret aiming
|
||||
@@ -49,6 +50,21 @@ public class ShooterAim extends SubsystemBase {
|
||||
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
|
||||
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
|
||||
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
|
||||
|
||||
m_shooterRotateMotor.setInverted(false);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
|
||||
/**
|
||||
* Passes subsystem needed.
|
||||
* @param subsystem Subsystem needed.
|
||||
*/
|
||||
public void passRequiredSubsystem(Shooter subsystem) {
|
||||
m_shooterSubsystem = subsystem;
|
||||
}
|
||||
|
||||
public void runShooterWithInput(double input) {
|
||||
@@ -73,17 +89,13 @@ public class ShooterAim extends SubsystemBase {
|
||||
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void resetGyroShooterRotate()
|
||||
{
|
||||
m_shooterRotateEncoder.setPosition(0);
|
||||
}
|
||||
public void resetGyroShooterRotate()
|
||||
{
|
||||
m_shooterRotateEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getShooterRotatePosition(){
|
||||
return m_shooterRotateMotor.getEncoder().getPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
public double getShooterRotatePosition()
|
||||
{
|
||||
return m_shooterRotateMotor.getEncoder().getPosition();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,96 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.subsystems;
|
||||
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.ControlType;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMax.SoftLimitDirection;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class ShooterHood extends SubsystemBase {
|
||||
public Shooter m_shooterSubsystem;
|
||||
|
||||
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
|
||||
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
|
||||
|
||||
public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
|
||||
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
|
||||
|
||||
public boolean m_isHoodReady = false;
|
||||
|
||||
public double m_fireAngle;
|
||||
public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
|
||||
|
||||
/**
|
||||
* Creates a new ShooterHood.
|
||||
*/
|
||||
public ShooterHood() {
|
||||
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
|
||||
|
||||
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
|
||||
m_hoodUpLimit.enableLimitSwitch(true);
|
||||
m_hoodDownLimit.enableLimitSwitch(true);
|
||||
|
||||
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
|
||||
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
|
||||
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
|
||||
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Passes subsystem needed.
|
||||
* @param subsystem Subsystem needed.
|
||||
*/
|
||||
public void passRequiredSubsystem(Shooter subsystem) {
|
||||
m_shooterSubsystem = subsystem;
|
||||
}
|
||||
|
||||
public double addFireAngle() {
|
||||
return m_fireAngle;
|
||||
}
|
||||
|
||||
/* Angle Adjustment PID Control */
|
||||
public void runAngleAdjustPID(double targetAngle)
|
||||
{
|
||||
// Set PID Coefficients
|
||||
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
|
||||
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
|
||||
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
|
||||
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
|
||||
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
|
||||
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
|
||||
|
||||
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void resetGyroAngleAdj(){
|
||||
m_angleEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getAnglePosition(){
|
||||
return m_angleEncoder.getPosition();
|
||||
}
|
||||
}
|
||||
@@ -7,23 +7,17 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import com.revrobotics.CANDigitalInput;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.ControlType;
|
||||
import com.revrobotics.SparkMax;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
|
||||
import com.revrobotics.ControlType;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class Storage extends SubsystemBase {
|
||||
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
|
||||
@@ -37,18 +31,17 @@ public class Storage extends SubsystemBase {
|
||||
|
||||
Intake m_intake;
|
||||
|
||||
public boolean m_isStorageReadyToFire = false;
|
||||
|
||||
/**
|
||||
* Creates a new Storage.
|
||||
*/
|
||||
public Storage() {
|
||||
resetEncoder();
|
||||
m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
|
||||
m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
|
||||
m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
|
||||
m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3);
|
||||
m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4);
|
||||
m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5);
|
||||
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);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -85,8 +78,8 @@ public class Storage extends SubsystemBase {
|
||||
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());
|
||||
//SmartDashboard.putNumber("Storage Position PID Target", targetPos);
|
||||
//SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
|
||||
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user