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 auto-programming

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-13 01:22:41 +00:00
committed by GitHub
parent 3ec37422c6 27b9f02f37
commit 352d6c2891
14 changed files with 1027 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -4
View File
@@ -120,8 +120,9 @@ public final class Constants {
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final int SHOOTER_TIMEOUT_MS = 30;
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0);
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double ENCODER_TICKS_PER_REV = 2048;
@@ -135,12 +136,16 @@ public final class Constants {
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
public static final double TURRET_CALIBRATE_SPEED = 0.075;
public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; //89.56696;
public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166;
public static final int HOOD_UP_SOFT_LIMIT = 33;
public static final int HOOD_DOWN_SOFT_LIMIT = 3;
public static final double HOOD_CONVERT_SLOPE = 0.47;
public static final double HOOD_CONVERT_B = 40.5;
public static final double HOOD_CALIBRATE_SPEED = 0.2;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
@@ -208,9 +213,9 @@ public final class Constants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 71.5;
public static final double LIME_ANGLE = 24.7;
public static final double TURN_P_VALUE = 0.6;
public static final double TURN_P_VALUE = 0.8;
public static final double X_ANGLE_ERROR = 1.3;
public static final double MOTOR_DEAD_ZONE = 0.3;
public static final double MOTOR_DEAD_ZONE = 0.2;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
public static final double GRAV = 385.83;
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -129,7 +129,7 @@ public class RobotContainer {
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
m_robotLeveler.passRequiredSubsystem(m_robotClimber);
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java
+2 -2
View File
@@ -33,9 +33,9 @@ public class DriveStraightToPositionMM extends CommandBase {
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java
+2 -2
View File
@@ -31,9 +31,9 @@ public class DriveStraightToPositionPID extends CommandBase {
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+12 -11
View File
@@ -34,7 +34,18 @@ public class ShooterVelocityControlPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel());
//Tells whether the target velocity has been reached
m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition();
m_targetVel = m_shooter.addFireVel();
double error = m_actualVel - m_targetVel;
if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){
m_shooter.m_isDrumReady = true;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
else{
m_shooter.m_isDrumReady = false;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
}
// Called once the command ends or is interrupted.
@@ -45,16 +56,6 @@ public class ShooterVelocityControlPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
//Tells whether the target velocity has been reached
double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
if (m_actualVel < upperBound && m_actualVel > lowerBound){
m_shooter.m_isDrumReady = true;
}
else{
m_shooter.m_isDrumReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+1
View File
@@ -110,6 +110,7 @@ public class TrackTarget extends CommandBase {
m_shooter.m_fireVel = fireVel;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterAim.m_targetDistance = distance;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+23 -4
View File
@@ -196,6 +196,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
@@ -708,6 +710,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);
@@ -827,6 +839,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());
@@ -834,6 +847,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());
@@ -848,8 +869,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());
@@ -888,8 +907,8 @@ 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();
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
+6
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());
}
/**
@@ -93,4 +95,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;
}
}