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 autonomous-commands

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-26 04:22:16 +00:00
committed by GitHub
parent f1edcbc29c ee39181c7e
commit 6939bc449d
16 changed files with 477 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+37 -12
View File
@@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
@@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -126,16 +128,24 @@ public class Drive extends SubsystemBase {
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_rightFrontMotor.setInverted(false);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
setDriveTrainNeutralMode(NeutralMode.Coast);
float rampRate = 0.1f;
m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
//SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01);
//m_rightFrontMotor.configSupplyCurrentLimit(c);
//m_leftFrontMotor.configSupplyCurrentLimit(c);
/* deadbands */
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
//m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
//m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
//m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
@@ -312,9 +322,11 @@ public class Drive extends SubsystemBase {
-getDistanceInches(m_rightFrontMotor));
try {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
//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 Back Output", m_leftBackMotor.get());
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
@@ -322,18 +334,30 @@ public class Drive extends SubsystemBase {
//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());
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());
SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
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 Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.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("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
@@ -342,10 +366,10 @@ public class Drive extends SubsystemBase {
//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.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_deltaTime);
if (currentSong != m_songChooser.getSelected()){
@@ -584,6 +608,7 @@ public class Drive extends SubsystemBase {
* Returns the current yaw of the pigeon
*/
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
src/main/java/frc4388/robot/subsystems/Intake.java
+19 -2
View File
@@ -17,16 +17,21 @@ import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
public class Intake extends SubsystemBase {
CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_extenderForwardLimit;
CANDigitalInput m_extenderReverseLimit;
public boolean isExtended = false;
/**
* Creates a new Intake.
*/
public Intake() {
m_intakeMotor.restoreFactoryDefaults();
m_extenderMotor.restoreFactoryDefaults();
@@ -59,6 +64,18 @@ public class Intake extends SubsystemBase {
* @param input the percent output to run motor at
*/
public void runExtender(double input) {
m_extenderMotor.set(input);
if (m_extenderForwardLimit.get()) {
isExtended = true;
}
if (m_extenderReverseLimit.get()) {
isExtended = false;
}
if (isExtended == false) {
m_extenderMotor.set(input);
}
if (isExtended == true) {
m_extenderMotor.set(-input);
}
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+23 -1
View File
@@ -52,6 +52,10 @@ public class Shooter extends SubsystemBase {
ShooterTables m_shooterTable;
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
/*
* Creates a new Shooter subsystem.
*/
@@ -94,6 +98,13 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
}
public double addFireVel() {
return m_fireVel;
}
public double addFireAngle() {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -120,15 +131,26 @@ public class Shooter extends SubsystemBase {
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
velP = actualVel/targetVel; //Percent of target
double runSpeed = actualVel + (12000*velP); //Ramp up equation
//if (runSpeed > targetVel) {runSpeed = targetVel;}
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;
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+30 -17
View File
@@ -9,16 +9,19 @@ 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 edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
@@ -29,12 +32,16 @@ public class Storage extends SubsystemBase {
CANEncoder m_encoder = m_storageMotor.getEncoder();
Gains storageGains = StorageConstants.STORAGE_GAINS;
Intake m_intake;
/**
* 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);
@@ -53,32 +60,38 @@ public class Storage extends SubsystemBase {
*
* @param input the voltage to run motor at
*/
public void runStorage(final double input) {
public void runStorage(double input) {
m_storageMotor.set(input);
final boolean beam_on = m_beamSensors[0].get();
}
public void resetEncoder()
{
public void resetEncoder(){
m_encoder.setPosition(0);
}
/* Storage PID Control */
public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
{
public void runStoragePositionPID(double targetPos){
// Set PID Coefficients
m_storagePIDController.setP(kP);
m_storagePIDController.setI(kI);
m_storagePIDController.setD(kD);
m_storagePIDController.setIZone(kIz);
m_storagePIDController.setFF(kF);
m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
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);
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
public void getEncoderPos()
{
m_encoder.getPosition();
public double getEncoderPos(){
return m_encoder.getPosition();
}
public boolean getBeam(int id){
return m_beamSensors[id].get();
}
public void setStoragePID(double position){
m_storagePIDController.setReference(position , ControlType.kPosition);
}
}