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

Merge branch 'master' into add-autonomous-paths

Browse Source
This commit is contained in:
aarav18
2020-03-01 14:53:39 -07:00
committed by GitHub
parent ec040e3432 f836dfe472
commit 48df9f80d6
27 changed files with 922 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Camera.java
+2 -2
View File
@@ -11,6 +11,7 @@ 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;
@@ -38,8 +39,7 @@ public class Camera extends SubsystemBase {
catch(Exception e){
System.err.println("Camera broken, pls nerf");
}
}
}
@Override
public void periodic() {
src/main/java/frc4388/robot/subsystems/Climber.java
+4 -4
View File
@@ -31,10 +31,10 @@ public class Climber extends SubsystemBase {
m_climberMotor.setIdleMode(IdleMode.kBrake);
m_climberMotor.setInverted(false);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberForwardLimit.enableLimitSwitch(false);
m_climberReverseLimit.enableLimitSwitch(false);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberForwardLimit.enableLimitSwitch(true);
m_climberReverseLimit.enableLimitSwitch(true);
}
@Override
src/main/java/frc4388/robot/subsystems/Drive.java
+1
View File
@@ -107,6 +107,7 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
/* Config Open Loop Ramp so we don't make sudden output changes */
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
src/main/java/frc4388/robot/subsystems/Intake.java
+3 -3
View File
@@ -41,9 +41,9 @@ public class Intake extends SubsystemBase {
m_extenderMotor.setInverted(false);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_extenderForwardLimit.enableLimitSwitch(true);
m_extenderReverseLimit.enableLimitSwitch(true);
}
@Override
src/main/java/frc4388/robot/subsystems/LimeLight.java
+35
View File
@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class LimeLight extends SubsystemBase {
/**
* Creates a new LimeLight.
*/
public LimeLight() {
}
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+59 -87
View File
@@ -13,9 +13,12 @@ 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;
@@ -23,6 +26,7 @@ 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.ShooterTables;
import frc4388.utility.controller.IHandController;
@@ -31,22 +35,15 @@ 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);
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
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;
// Configure PID Controllers
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
double velP;
double input;
@@ -55,20 +52,23 @@ public class Shooter extends SubsystemBase {
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
public Trims shooterTrims;
/*
* Creates a new Shooter subsystem.
* Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
*/
public Shooter() {
//Testing purposes reseting gyros
resetGyroAngleAdj();
resetGyroShooterRotate();
//resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
m_shooterFalcon.configFactoryDefault();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(false);
m_shooterFalcon.setInverted(true);
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -90,6 +90,19 @@ public class Shooter extends SubsystemBase {
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));
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
@@ -105,6 +118,7 @@ public class Shooter extends SubsystemBase {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -118,89 +132,47 @@ public class Shooter extends SubsystemBase {
*/
public void setShooterGains() {
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* 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) {
velP = actualVel/targetVel; //Percent of target
double runSpeed = actualVel + (12000*velP); //Ramp up equation
SmartDashboard.putNumber("shoot", actualVel);
runSpeed = runSpeed/targetVel; //Convert to percent
if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
//Tells wether the target velocity has been reached
double upperBound = targetVel + 300;
double lowerBound = targetVel - 300;
if (actualVel < upperBound && actualVel > lowerBound)
{
velReached = true;
}
else{
velReached = false;
}
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
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_shooterTurretGains.m_kP);
m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
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);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.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();
}
/* For Testing Purposes, reseting gyro for angle adjuster */
public void resetGyroAngleAdj()
{
m_angleEncoder.setPosition(0);
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
public double getAnglePosition(){
return m_angleAdjustMotor.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* 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.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;
public class ShooterAim extends SubsystemBase {
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();
/**
* Creates a subsytem for the turret aiming
*/
public ShooterAim() {
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit.enableLimitSwitch(true);
m_shooterLeftLimit.enableLimitSwitch(true);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
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);
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
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
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+10 -1
View File
@@ -19,12 +19,14 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
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;
public class Storage extends SubsystemBase {
private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -68,6 +70,11 @@ 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 */
public void runStoragePositionPID(double targetPos){
// Set PID Coefficients
@@ -78,6 +85,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());
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}