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 pull request #53 from Team4388/buckmeister's-wacky-code-changes

Browse Source 
Buckmeister's wacky code changes
This commit is contained in:
ryan123rudder
2020-03-06 17:38:41 -07:00
committed by GitHub
parent faae8cfa6a 63471517c2
commit 733ed01d9e
58 changed files with 705 additions and 631 deletions
Download Patch File Download Diff File Expand all files Collapse all files
build.gradle
+1 -1
View File
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2020.2.2"
id "edu.wpi.first.GradleRIO" version "2020.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11
src/main/deploy/Robot Data - Angles.csv
+1 -4
View File
@@ -1,6 +1,3 @@
Angle (deg),Displacement (deg)
-20,-5
-10,-2
0,0
10,2
20,5
0,0
1 Angle (deg) Displacement (deg)
-20 -5
-10 -2
2 0 0
3 10 0 2 0
20 5
src/main/deploy/Robot Data - Distances.csv
+8 -5
View File
@@ -1,6 +1,9 @@
Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
21,10,10000
100,23,11000
200,30,14000
300,56,17000
480,100,20000
74,20,8000
144,23,10000
162,28,10000
208,29,10000
296,32,12500
418,33,16000
430,31,16000
450,31,16000
1 Distance (in) Hood Ext. (u) Drum Velocity (u/ds)
2 21 74 10 20 10000 8000
3 100 144 23 11000 10000
4 200 162 30 28 14000 10000
5 300 208 56 29 17000 10000
6 480 296 100 32 20000 12500
7 418 33 16000
8 430 31 16000
9 450 31 16000
src/main/java/frc4388/robot/Constants.java
+13 -11
View File
@@ -10,6 +10,7 @@ package frc4388.robot;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
/**
@@ -121,15 +122,18 @@ 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.4, 0.0005, 13, 0.05, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 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 SHOOTER_ANGLE_GAINS = new Gains(0.05, 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;
public static final double NEO_UNITS_PER_REV = 42;
public static final double DEGREES_PER_ROT = 360;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
public static final int TURRET_RIGHT_SOFT_LIMIT = -2;
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
@@ -139,7 +143,7 @@ public final class Constants {
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.1;
public static final double HOOD_CALIBRATE_SPEED = 0.2;
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
@@ -161,12 +165,10 @@ public final class Constants {
public static final double STORAGE_TIMEOUT = 2000;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
public static final int BEAM_SENSOR_DIO_1 = 1;
public static final int BEAM_SENSOR_DIO_2 = 2;
public static final int BEAM_SENSOR_DIO_3 = 3;
public static final int BEAM_SENSOR_DIO_4 = 4;
public static final int BEAM_SENSOR_DIO_5 = 5;
public static final int BEAM_SENSOR_SHOOTER = 1;
public static final int BEAM_SENSOR_USELESS = 2;
public static final int BEAM_SENSOR_STORAGE = 3;
public static final int BEAM_SENSOR_INTAKE = 4;
/* PID Values */
public static final int SLOT_DISTANCE = 0;
@@ -196,7 +198,7 @@ public final class Constants {
public static final class VisionConstants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 64;
public static final double TARGET_HEIGHT = 71;
public static final double LIME_ANGLE = 25;
public static final double TURN_P_VALUE = 0.65;
public static final double X_ANGLE_ERROR = 1.3;
src/main/java/frc4388/robot/RobotContainer.java
+90 -94
View File
@@ -11,79 +11,54 @@ import java.util.List;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoPath1FromCenter;
import frc4388.robot.commands.AutoPath2FromRight;
import frc4388.robot.commands.CalibrateShooter;
import frc4388.robot.commands.DrivePositionMPAux;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
import frc4388.robot.commands.HoldTarget;
import frc4388.robot.commands.HoodPositionPID;
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.ShooterVelocityControlPID;
import frc4388.robot.commands.StorageIntake;
import frc4388.robot.commands.GotoCoordinates;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.commands.ShootFireGroup;
import frc4388.robot.commands.ShootFullGroup;
import frc4388.robot.commands.ShootPrepGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.commands.TrimShooter;
import frc4388.robot.commands.StorageOutake;
import frc4388.robot.commands.StoragePrepAim;
import frc4388.robot.commands.StoragePrepIntake;
import frc4388.robot.commands.StorageRun;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.auto.AutoPath1FromCenter;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.commands.climber.DisengageRachet;
import frc4388.robot.commands.climber.RunClimberWithTriggers;
import frc4388.robot.commands.climber.RunLevelerWithJoystick;
import frc4388.robot.commands.drive.DriveStraightToPositionMM;
import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.TurnDegrees;
import frc4388.robot.commands.intake.RunIntakeWithTriggers;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.ShootFireGroup;
import frc4388.robot.commands.shooter.ShootFullGroup;
import frc4388.robot.commands.shooter.ShootPrepGroup;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.storage.StorageIntake;
import frc4388.robot.commands.storage.StoragePrepIntake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.commands.TurnDegrees;
import frc4388.robot.commands.Wait;
import frc4388.robot.commands.StorageOutake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.XboxTriggerButton;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -100,6 +75,7 @@ public class RobotContainer {
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final ShooterAim m_robotShooterAim = new ShooterAim();
private final ShooterHood m_robotShooterHood = new ShooterHood();
private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
@@ -122,14 +98,20 @@ public class RobotContainer {
m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
m_robotLeveler.passRequiredSubsystem(m_robotClimber);
configureButtonBindings();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
//m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
// drives intake with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// runs the turret with joystick
@@ -139,10 +121,12 @@ public class RobotContainer {
// drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// drives the leveler with an axis input from the driver controller
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController()));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the storage not
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
@@ -157,11 +141,10 @@ public class RobotContainer {
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new TurnDegrees(m_robotDrive, 90));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new Wait(m_robotDrive, 0, 0));
@@ -169,7 +152,9 @@ public class RobotContainer {
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new InstantCommand());
/* Driver Buttons */
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -179,22 +164,32 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
// Disengages the rachet to allow for a climb
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whileHeld(new DisengageRachet(m_robotClimber));
/* Operator Buttons */
// shoots until released
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// shoots one ball
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
//.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
//.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
@@ -203,27 +198,13 @@ public class RobotContainer {
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
//Prepares storage for intaking
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
//Runs storage to outtake
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new StorageOutake(m_robotStorage));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
.whileHeld(new TrackTarget(m_robotShooterAim))
.whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
//.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage))
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
//.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
//.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
//.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
//Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
@@ -231,8 +212,23 @@ public class RobotContainer {
//Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
//Prepares storage for intaking
//new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
//.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
//Runs storage to outtake
//new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
//.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
//.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
//Run drum
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
.whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
}
/**
@@ -273,7 +269,7 @@ public class RobotContainer {
config);
// 10 = 20, 20 = 35, 30 = 53.5
// (0,10) = (8,22)
return exampleTrajectory;
}
@@ -281,11 +277,11 @@ public class RobotContainer {
RamseteCommand ramseteCommand = new RamseteCommand(
trajectory,
m_robotDrive::getPose,
new RamseteController(),
new RamseteController(),
DriveConstants.kDriveKinematics,
m_robotDrive::tankDriveVelocity,
m_robotDrive);
return ramseteCommand;
}
@@ -306,7 +302,7 @@ public class RobotContainer {
}
/**
*
*
*/
public void resetOdometry() {
m_robotDrive.resetGyroAngles();
@@ -320,7 +316,7 @@ public class RobotContainer {
public IHandController getDriverController() {
return m_driverXbox;
}
/**
* Used for analog inputs like triggers and axises.
* @return The IHandController interface for the Operator Controller.
@@ -329,7 +325,7 @@ public class RobotContainer {
{
return m_operatorXbox;
}
/**
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
@@ -339,7 +335,7 @@ public class RobotContainer {
{
return m_operatorXbox.getJoyStick();
}
/**
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox Controller.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
src/main/java/frc4388/robot/commands/HoldTarget.java
-117
View File
@@ -1,117 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.commands;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class HoldTarget extends CommandBase {
//Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
public double fireVel;
public double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
/**
* Uses the Limelight to track the target
* @param shooterSubsystem The Shooter subsystem
* @param aimSubsystem The ShooterAim subsystem
*/
public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
if (target == 1.0){ //If target in view
//Aiming Left/Right
turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone
//Deadzones
else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;}
else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
double xVel = (distance*VisionConstants.GRAV)/(yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}/*
else{
System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition());
double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1;
if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){
curveInput = -curveInput;
}
System.err.println("Curve Input: " + curveInput);
m_shooterAim.runShooterWithInput(curveInput);
}
*/
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java → src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java
+2 -1
View File
@@ -5,9 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinates;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java → src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java
+2 -1
View File
@@ -5,9 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinates;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/Wait.java → src/main/java/frc4388/robot/commands/auto/Wait.java
+3 -4
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -46,11 +45,11 @@ public class Wait extends CommandBase {
public void execute() {
if (counter == 0) {
SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
//SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
}
m_currentTime = System.currentTimeMillis();
SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
//SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
counter ++;
}
src/main/java/frc4388/robot/commands/climber/DisengageRachet.java
+47
View File
@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* 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.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
public class DisengageRachet extends CommandBase {
Climber m_climber;
/**
* Creates a new DisengageRachet command.
*/
public DisengageRachet(Climber subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_climber = subsystem;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_climber.climberSafety) {
m_climber.shiftServo(false);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return true;
}
}
src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java → src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java
+2 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
@@ -43,6 +43,7 @@ public class RunClimberWithTriggers extends CommandBase {
}
if (leftTrigger > rightTrigger) {
output = -leftTrigger;
m_climber.shiftServo(true);
}
} else {
output = rightTrigger;
src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java → src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java
+2 -2
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Leveler;
@@ -36,7 +36,7 @@ public class RunLevelerWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double input = m_controller.getLeftXAxis();
double input = m_controller.getRightXAxis();
m_leveler.runLeveler(input);
}
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java → src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java → src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java
+1 -2
View File
@@ -4,8 +4,7 @@
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java → src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java
+3 -3
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -61,7 +61,7 @@ public class DriveStraightToPositionMM extends CommandBase {
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
SmartDashboard.putBoolean("MM Run", true);
//SmartDashboard.putBoolean("MM Run", true);
i++;
}
@@ -74,7 +74,7 @@ public class DriveStraightToPositionMM extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
SmartDashboard.putBoolean("MM Run", false);
//SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java → src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java
+1 -2
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
src/main/java/frc4388/robot/commands/DriveWithJoystick.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
+2 -3
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
@@ -43,7 +42,7 @@ public class DriveWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = -m_controller.getLeftYAxis();
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java
+4 -6
View File
@@ -5,9 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
@@ -61,9 +59,9 @@ public class DriveWithJoystickAuxPID extends CommandBase {
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
System.err.println("Target: " + m_targetGyro);
System.err.println("Current: " + currentGyro);
System.err.println();
//System.err.println("Target: " + m_targetGyro);
//System.err.println("Current: " + currentGyro);
//System.err.println();
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java
+3 -6
View File
@@ -5,12 +5,9 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import java.security.PublicKey;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
@@ -109,12 +106,12 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
/**
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java → src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java
+4 -5
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
@@ -133,12 +132,12 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
@@ -159,7 +158,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
} else {
m_drive.driveWithInputAux(0, m_targetGyro);
}
System.out.println("Turning with Target: " + m_targetGyro);
//System.out.println("Turning with Target: " + m_targetGyro);
}
/**
src/main/java/frc4388/robot/commands/GotoCoordinates.java → src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java
+2 -2
View File
@@ -5,10 +5,10 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
src/main/java/frc4388/robot/commands/PlaySongDrive.java → src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
src/main/java/frc4388/robot/commands/TurnDegrees.java → src/main/java/frc4388/robot/commands/drive/TurnDegrees.java
+3 -3
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -50,8 +50,8 @@ public class TurnDegrees extends CommandBase {
m_drive.runTurningPID(m_targetAngleTicksOut);
SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
//SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
//SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
i++;
}
src/main/java/frc4388/robot/commands/RunExtenderOutIn.java → src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java
+1 -8
View File
@@ -5,17 +5,15 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.intake;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.IHandController;
public class RunExtenderOutIn extends CommandBase {
private Intake m_intake;
@@ -36,11 +34,6 @@ public class RunExtenderOutIn extends CommandBase {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
addRequirements(m_intake);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
}
// Called when the command is initially scheduled.
src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java → src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java
+5 -9
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.intake;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Intake;
@@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase {
double rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if (leftTrigger < .5) {
if(leftTrigger > rightTrigger) {
output = leftTrigger;
}
if (rightTrigger > leftTrigger) {
output = -leftTrigger;
}
} else {
if(leftTrigger >= rightTrigger) {
output = leftTrigger;
}
if (rightTrigger > leftTrigger) {
output = -rightTrigger;
}
m_intake.runIntake(output);
}
src/main/java/frc4388/robot/commands/CalibrateShooter.java → src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
+9 -11
View File
@@ -5,28 +5,31 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
/**
* Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
* @param shootSub The Shooter subsystem
* @param aimSub The ShooterAim subsystem
*/
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) {
m_shooter = shootSub;
m_shooterAim = aimSub;
addRequirements(m_shooter, m_shooterAim);
m_shooterHood = hoodSub;
addRequirements(m_shooter, m_shooterHood, m_shooterAim);
}
// Called when the command is initially scheduled.
@@ -37,10 +40,8 @@ public class CalibrateShooter extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooter.m_angleEncoder.setPosition(0);
m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterHood.m_angleEncoder.setPosition(0);
m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
@@ -51,9 +52,6 @@ public class CalibrateShooter extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
}
src/main/java/frc4388/robot/commands/HoodPositionPID.java → src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java
+18 -15
View File
@@ -5,22 +5,22 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterHood;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
ShooterHood m_shooterHood;
double firingAngle;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(Shooter subSystem) {
m_shooter = subSystem;
//addRequirements(m_shooter);
public HoodPositionPID(ShooterHood subSystem) {
m_shooterHood = subSystem;
addRequirements(m_shooterHood);
}
// Called when the command is initially scheduled.
@@ -31,12 +31,13 @@ public class HoodPositionPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
double b = ShooterConstants.HOOD_CONVERT_B;
firingAngle = (-slope*m_shooter.addFireAngle())+b;
SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooter.runAngleAdjustPID(firingAngle);
//double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
//double b = ShooterConstants.HOOD_CONVERT_B;
//firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
firingAngle = m_shooterHood.addFireAngle();
//SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
//SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooterHood.runAngleAdjustPID(firingAngle);
}
// Called once the command ends or is interrupted.
@@ -47,9 +48,11 @@ public class HoodPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return true;
m_shooterHood.m_isHoodReady = true;
} else {
m_shooterHood.m_isHoodReady = false;
}
return false;
}
src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
+75
View File
@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* 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.commands.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
public class PrepChecker extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
Storage m_storage;
boolean m_isDrumReady = false;
boolean m_isAimReady = false;
boolean m_isHoodReady = false;
boolean m_isStorageReady = false;
/**
* Creates a new PrepChecker.
* @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands.
* @param storage reads storage in a similar way to shooter. Not used as a requirement.
*/
public PrepChecker(Shooter shooter, Storage storage) {
m_shooter = shooter;
m_shooterAim = m_shooter.m_shooterAimSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
m_storage = storage;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isDrumReady = false;
m_isAimReady = false;
m_isHoodReady = false;
m_isStorageReady = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady);
m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady);
m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady);
m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_isDrumReady = false;
m_shooterAim.m_isAimReady = false;
m_shooterHood.m_isHoodReady = false;
m_storage.m_isStorageReadyToFire = false;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ShootFireGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+8 -6
View File
@@ -5,12 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.commands.storage.StorageRun;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -23,12 +25,12 @@ public class ShootFireGroup extends ParallelRaceGroup {
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
addCommands(
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
new HoldTarget(m_shooter, m_shooterAim),
new StorageRun(m_storage)
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
new TrackTarget(m_shooterAim)
//new StorageRun(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootFullGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
+5 -4
View File
@@ -5,11 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -22,10 +23,10 @@ public class ShootFullGroup extends SequentialCommandGroup {
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
addCommands(
new ShootPrepGroup(m_shooter, m_shooterAim, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootPrepGroup.java → src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+11 -9
View File
@@ -5,30 +5,32 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import frc4388.robot.commands.storage.StoragePrepAim;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootPrepGroup extends ParallelCommandGroup {
public class ShootPrepGroup extends ParallelDeadlineGroup {
/**
* Prepares the Shooter to be fired
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
addCommands(
new TrackTarget(m_shooter, m_shooterAim),
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
super(
new PrepChecker(m_shooter, m_storage),
new TrackTarget(m_shooterAim),
new ShooterVelocityControlPID(m_shooter),
new StoragePrepAim(m_storage),
new HoodPositionPID(m_shooter)
new HoodPositionPID(m_shooterHood),
new StoragePrepAim(m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java → src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+10 -10
View File
@@ -5,9 +5,8 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
@@ -16,6 +15,7 @@ public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
/**
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
@@ -34,10 +34,10 @@ 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(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
//m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
//SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
//SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
}
// Called once the command ends or is interrupted.
@@ -52,12 +52,12 @@ public class ShooterVelocityControlPID extends CommandBase {
double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
if (m_actualVel < upperBound && m_actualVel > lowerBound){
SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
return true;
m_shooter.m_isDrumReady = true;
}
else{
SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
return false;
m_shooter.m_isDrumReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/TrackTarget.java → src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+29 -32
View File
@@ -5,31 +5,25 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.commands.TrimShooter;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class TrackTarget extends CommandBase {
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
ShooterHood m_shooterHood;
NetworkTableEntry xEntry;
IHandController m_driverController;
// Aiming
double turnAmount = 0;
@@ -43,16 +37,15 @@ public class TrackTarget extends CommandBase {
public double m_hoodTrim;
public double m_turretTrim;
ShooterTables m_shooterTable;
/**
* Uses the Limelight to track the target
* @param shooterSubsystem The Shooter subsystem
* @param aimSubsystem The ShooterAim subsystem
*/
public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
public TrackTarget(ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
m_shooter = m_shooterAim.m_shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
@@ -63,7 +56,6 @@ public class TrackTarget extends CommandBase {
// Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
m_shooterTable = new ShooterTables();
}
// Called every time the scheduler runs while the command is scheduled.
@@ -85,7 +77,7 @@ public class TrackTarget extends CommandBase {
} else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
}
m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -95,18 +87,22 @@ public class TrackTarget extends CommandBase {
double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
double xVel = (distance * VisionConstants.GRAV) / (yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
//fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//END Equation Code
/*//START CSV Code
fireVel = m_shooterTable.getVelocity(distance);
fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
//END CSV Code*/
//START CSV Code
fireVel = m_shooter.m_shooterTable.getVelocity(distance);
fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
//fireVel = SmartDashboard.getNumber("Velocity Target", 0);
//fireAngle = SmartDashboard.getNumber("Angle Target", 3);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
@@ -121,10 +117,11 @@ public class TrackTarget extends CommandBase {
public boolean isFinished() {
if (xAngle < 1 && xAngle > -1 && target == 1)
{
SmartDashboard.putBoolean("TrackTarget Finished", true);
return true;
m_shooterAim.m_isAimReady = true;
} else {
m_shooterAim.m_isAimReady = false;
}
SmartDashboard.putBoolean("TrackTarget Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/TrimShooter.java → src/main/java/frc4388/robot/commands/shooter/TrimShooter.java
+2 -6
View File
@@ -5,16 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.Joystick;
import frc4388.utility.controller.XboxController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Trims;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.utility.controller.XboxController;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
src/main/java/frc4388/robot/commands/StorageIntake.java → src/main/java/frc4388/robot/commands/storage/StorageIntake.java
+3 -4
View File
@@ -5,10 +5,9 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
@@ -39,7 +38,7 @@ public class StorageIntake extends CommandBase {
@Override
public void execute() {
if (!m_storage.getBeam(0)){
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
@@ -56,7 +55,7 @@ public class StorageIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(0) && intook){
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){
return true;
}
return false;
src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java
+40
View File
@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* 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.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class StorageIntakeFinal extends CommandBase {
/**
* Creates a new StorageIntakeFinal.
*/
public StorageIntakeFinal() {
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/StorageOutake.java → src/main/java/frc4388/robot/commands/storage/StorageOutake.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
src/main/java/frc4388/robot/commands/StoragePositionPID.java → src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
src/main/java/frc4388/robot/commands/StoragePrepAim.java → src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java
+9 -7
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase {
@Override
public void execute() {
if (m_storage.getBeam(1)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
//m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
@@ -49,11 +49,13 @@ public class StoragePrepAim extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
SmartDashboard.putBoolean("StoragePrepAim Finished", true);
/*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
m_storage.m_isStorageReadyToFire = true;
return true;
}
SmartDashboard.putBoolean("StoragePrepAim Finished", false);
return false;
} else {
m_storage.m_isStorageReadyToFire = false;
return false;
}*/
return true;
}
}
src/main/java/frc4388/robot/commands/StoragePrepIntake.java → src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
+4 -4
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
@@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(0)){
if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
else{
@@ -54,8 +54,8 @@ public class StoragePrepIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return true;
if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return false;
}
return false;
}
src/main/java/frc4388/robot/commands/StorageRun.java → src/main/java/frc4388/robot/commands/storage/StorageRun.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
package frc4388.robot.commands.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
src/main/java/frc4388/robot/subsystems/Camera.java
-2
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/Climber.java
+20 -1
View File
@@ -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);
}
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+20 -23
View File
@@ -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 */
@@ -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()){
src/main/java/frc4388/robot/subsystems/Intake.java
+14 -9
View File
@@ -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);
}
}
}*/
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -50,6 +50,6 @@ public class LED extends SubsystemBase {
@Override
public void periodic(){
SmartDashboard.putNumber("LED", currentLED);
//SmartDashboard.putNumber("LED", currentLED);
}
}
src/main/java/frc4388/robot/subsystems/Leveler.java
+10 -7
View File
@@ -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;
}
}
src/main/java/frc4388/robot/subsystems/Pneumatics.java
-2
View File
@@ -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 {
src/main/java/frc4388/robot/subsystems/Shooter.java
+47 -82
View File
@@ -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
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+31 -19
View File
@@ -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();
}
}
src/main/java/frc4388/robot/subsystems/ShooterHood.java
+96
View File
@@ -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();
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+10 -17
View File
@@ -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);
}
src/main/java/frc4388/robot/Gains.java → src/main/java/frc4388/utility/Gains.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
package frc4388.utility;
/**
* Add your docs here.
src/main/java/frc4388/utility/ShooterTables.java
+6 -6
View File
@@ -85,12 +85,12 @@ public class ShooterTables {
} catch (IOException e) {
e.printStackTrace();
}
SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]);
SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]);
SmartDashboard.putNumber("m_distanceLength", m_distanceLength);
SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]);
SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]);
SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
//SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]);
//SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]);
//SmartDashboard.putNumber("m_distanceLength", m_distanceLength);
//SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]);
//SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]);
//SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
}
public double getHood(double distance) { //Rotations of motor
src/main/java/frc4388/robot/Trims.java → src/main/java/frc4388/utility/Trims.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
package frc4388.utility;
public class Trims{
public double m_turretTrim;
vendordeps/Phoenix.json
+15 -15
View File
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix.json",
"name": "CTRE-Phoenix",
"version": "5.18.1",
"version": "5.18.2",
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [
"http://devsite.ctr-electronics.com/maven/release/"
@@ -11,19 +11,19 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-java",
"version": "5.18.1"
"version": "5.18.2"
},
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-java",
"version": "5.18.1"
"version": "5.18.2"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.18.1",
"version": "5.18.2",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -35,7 +35,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.18.1",
"version": "5.18.2",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -47,7 +47,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.18.1",
"version": "5.18.2",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -58,7 +58,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.18.1",
"version": "5.18.2",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -69,7 +69,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.18.1",
"version": "5.18.2",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -83,7 +83,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-cpp",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_Phoenix_WPI",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -97,7 +97,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-cpp",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_Phoenix",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -111,7 +111,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_PhoenixCCI",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -125,7 +125,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_PhoenixDiagnostics",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -139,7 +139,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_PhoenixCanutils",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -152,7 +152,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_PhoenixPlatform",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -165,7 +165,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.18.1",
"version": "5.18.2",
"libName": "CTRE_PhoenixCore",
"headerClassifier": "headers",
"sharedLibrary": false,
vendordeps/REVRobotics.json
+5 -5
View File
@@ -1,7 +1,7 @@
{
"fileName": "REVRobotics.json",
"name": "REVRobotics",
"version": "1.5.1",
"version": "1.5.2",
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
"mavenUrls": [
"http://www.revrobotics.com/content/sw/max/sdk/maven/"
@@ -11,14 +11,14 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "SparkMax-java",
"version": "1.5.1"
"version": "1.5.2"
}
],
"jniDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "SparkMax-driver",
"version": "1.5.1",
"version": "1.5.2",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
@@ -35,7 +35,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "SparkMax-cpp",
"version": "1.5.1",
"version": "1.5.2",
"libName": "SparkMax",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -52,7 +52,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "SparkMax-driver",
"version": "1.5.1",
"version": "1.5.2",
"libName": "SparkMaxDriver",
"headerClassifier": "headers",
"sharedLibrary": false,
vendordeps/WPILibOldCommands.json
-37
View File
@@ -1,37 +0,0 @@
{
"fileName": "WPILibOldCommands.json",
"name": "WPILib-Old-Commands",
"version": "2020.0.0",
"uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
"mavenUrls": [],
"jsonUrl": "",
"javaDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-java",
"version": "wpilib"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-cpp",
"version": "wpilib",
"libName": "wpilibOldCommands",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"linuxathena",
"linuxraspbian",
"linuxaarch64bionic",
"windowsx86-64",
"windowsx86",
"linuxx86-64",
"osxx86-64"
]
}
]
}