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

Merge branch 'master' into auto-programming

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-02 23:44:54 +00:00
committed by GitHub
parent 4bf9873f85 a83304baf0
commit d8521c4514
42 changed files with 1429 additions and 358 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+66 -36
View File
@@ -32,31 +32,38 @@ public final class Constants {
public static final int PIGEON_ID = 6;
/* Drive Inversions */
public static final boolean isRightMotorInverted = false;
public static final boolean isRightMotorInverted = true;
public static final boolean isLeftMotorInverted = false;
public static final boolean isRightArcadeInverted = false;
public static final boolean isAuxPIDInverted = false;
/* Drive Configuration */
public static final double OPEN_LOOP_RAMP_RATE = 0.1;
public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
public static final double NEUTRAL_DEADBAND = 0.04;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
public static final int CLOSED_LOOP_TIME_MS = 1;
new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
/* Drive Train Characteristics */
public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
public static final double WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
public static final double TICKS_PER_MOTOR_REV = 2048;
/* PID Constants Drive*/
public static final int DRIVE_TIMEOUT_MS = 30;
public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY = 20000;
public static final int DRIVE_ACCELERATION = 7000;
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY = 30000;
public static final int DRIVE_ACCELERATION = 23000;
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
public static final int DRIVE_ACCELERATION_HIGH = 7000;
@@ -80,13 +87,6 @@ public final class Constants {
public final static int SLOT_TURNING = 2;
public final static int SLOT_MOTION_MAGIC = 3;
/* Drive Train Characteristics */
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
public static final double WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
/* Ratio Calculation */
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double TICK_TIME_TO_SECONDS = 0.1;
@@ -105,39 +105,60 @@ public final class Constants {
public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
}
public static final class IntakeConstants {
public static final int INTAKE_SPARK_ID = -9;
public static final int EXTENDER_SPARK_ID = -10;
public static final class IntakeConstants {;
public static final double EXTENDER_SPEED = 0.3;
public static final int INTAKE_SPARK_ID = 12;
public static final int EXTENDER_SPARK_ID = 13;
}
public static final class ShooterConstants {
/* Motor IDs */
public static final int SHOOTER_FALCON_ID = -1;
public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
public static final int SHOOTER_ROTATE_ID = 10;
public static final int SHOOTER_FALCON_ID = 8;
public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
public static final int SHOOTER_ROTATE_ID = 9;
/* PID Constants Shooter */
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 SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 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.2, 0.0, 0, 0.0453, 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 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 int TURRET_RIGHT_SOFT_LIMIT = -2;
public static final int TURRET_LEFT_SOFT_LIMIT = -55;
public static final double TURRET_SPEED_MULTIPLIER = 0.3;
public static final double TURRET_CALIBRATE_SPEED = 0.075;
public static final int HOOD_UP_SOFT_LIMIT = 33;
public static final int HOOD_DOWN_SOFT_LIMIT = 3;
public static final double HOOD_CONVERT_SLOPE = 0.47;
public static final double HOOD_CONVERT_B = 40.5;
public static final double HOOD_CALIBRATE_SPEED = 0.1;
public static final double DRUM_RAMP_LIMIT = 1000;
public static final double DRUM_VELOCITY_BOUND = 300;
}
public static final class ClimberConstants {
public static final int CLIMBER_SPARK_ID = 10;
public static final int CLIMBER_SPARK_ID = 14;
}
public static final class LevelerConstants {
public static final int LEVELER_CAN_ID = -1;
public static final int LEVELER_CAN_ID = 15;
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = -1;
public static final int STORAGE_CAN_ID = 11;
public static final double STORAGE_PARTIAL_BALL = 2;
public static final double STORAGE_FULL_BALL = 7;
public static final double STORAGE_SPEED = 0.5;
public static final double STORAGE_TIMEOUT = 2000;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
@@ -154,9 +175,18 @@ public final class Constants {
public static final int PID_PRIMARY = 0;
/* PID Gains */
public static final double STORAGE_MIN_OUTPUT = -1.0;
public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class PneumaticsConstants {
public static final int PCM_MODULE_ID = 7;
public static final int SPEED_SHIFT_FORWARD_ID = 0;
public static final int SPEED_SHIFT_REVERSE_ID = 1;
public static final int COOL_FALCON_FORWARD_ID = 3;
public static final int COOL_FALCON_REVERSE_ID = 2;
}
public static final class LEDConstants {
@@ -166,8 +196,8 @@ 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 = 82.75;
public static final double LIME_ANGLE = 18.7366;
public static final double TARGET_HEIGHT = 64;
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;
public static final double MOTOR_DEAD_ZONE = 0.3;
src/main/java/frc4388/robot/Robot.java
+2 -2
View File
@@ -77,7 +77,7 @@ public class Robot extends TimedRobot {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
//m_robotContainer.setDriveGearState(true);
m_robotContainer.setDriveGearState(false);
m_robotContainer.resetOdometry();
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
m_robotContainer.setDriveGearState(false);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
// This makes sure that the autonomous stops running when
src/main/java/frc4388/robot/RobotContainer.java
+109 -37
View File
@@ -16,6 +16,7 @@ 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;
@@ -28,24 +29,61 @@ 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.commands.StorageIntakeGroup;
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.TurnDegrees;
import frc4388.robot.commands.Wait;
import frc4388.robot.commands.storageOutake;
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.subsystems.Camera;
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.Storage;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
@@ -60,91 +98,103 @@ import frc4388.utility.controller.XboxController;
public class RobotContainer {
/* Subsystems */
private final Drive m_robotDrive = new Drive();
private final Pneumatics m_robotPneumatics = new Pneumatics();
private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final ShooterAim m_robotShooterAim = new ShooterAim();
private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
/* Cameras */
private final Camera m_robotCameraFront = new Camera("front",0,160,120,40);
private final Camera m_robotCameraBack = new Camera("back",1,160,120,40);
private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
private final LimeLight m_robotLime = new LimeLight();
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
/* Passing Drive and Pneumatics Subsystems */
m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
configureButtonBindings();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, 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 drum shooter in idle mode
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
// runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
// 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()));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new TurnDegrees(m_robotDrive, 90));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new Wait(m_robotDrive, 0, 0));
// 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)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
/* Operator Buttons */
//TODO: Shooter Buttons
// shoots until released
//new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
// shoots one ball
//new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
// aims the turret
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
//.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -154,17 +204,38 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
/* Storage Neo PID Test */
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
.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 StorageIntakeGroup(m_robotIntake, m_robotStorage));
.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
//Runs storage to outtake
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new storageOutake(m_robotStorage));
.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)));
//Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
.whenPressed(new TrimShooter(m_robotShooter));
//Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
}
/**
@@ -179,7 +250,9 @@ public class RobotContainer {
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
// Run path following command, then stop at the end.
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
//return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);
if (Constants.SELECTED_AUTO == 1) {
return new SequentialCommandGroup(new Wait(5, m_robotDrive),
new TurnDegrees(45, m_robotDrive),
@@ -191,7 +264,6 @@ public class RobotContainer {
return new InstantCommand();
// return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
}
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -243,7 +315,7 @@ public class RobotContainer {
* @param state the gearing of the gearbox (true is high, false is low)
*/
public void setDriveGearState(boolean state) {
m_robotDrive.setShiftState(state);
m_robotPneumatics.setShiftState(state);
}
/**
src/main/java/frc4388/robot/Trims.java
+18
View File
@@ -0,0 +1,18 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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;
public class Trims{
public double m_turretTrim;
public double m_hoodTrim;
public Trims(double turretTrim, double hoodTrim){
m_turretTrim = turretTrim;
m_hoodTrim = hoodTrim;
}
}
src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+45
View File
@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
// 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 AutoPath1FromCenter extends SequentialCommandGroup {
Drive m_drive;
Pneumatics m_pneumatics;
/**
* Creates a new AutoPath1FromCenter.
*/
public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
m_pneumatics = subsystem2;
addCommands( new Wait(m_drive, 0, 1),
//shoot pre-loaded 3 balls
new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90),
//Start Intake Ball 1
new GotoCoordinates(m_drive, m_pneumatics, 0, 12),
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 2
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 3
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
new Wait(m_drive, 0, 2)
//Shoot 3 Balls
);
}
}
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+46
View File
@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
// 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 AutoPath2FromRight extends SequentialCommandGroup {
Drive m_drive;
Pneumatics m_pneumatics;
/**
* Creates a new AutoPath2FromRight.
*/
public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
m_pneumatics = subsystem2;
addCommands( new Wait(m_drive, 0, 1),
new GotoCoordinates(m_drive, m_pneumatics, 0, 77),
//Start Intake Ball 1
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 2
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
//Shoot 5 Balls
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 1 (second round)
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
//Start Moving to 4th Ball
new GotoCoordinates(m_drive, m_pneumatics, 60, -50),
new Wait(m_drive, 0, 2)
);
}
}
src/main/java/frc4388/robot/commands/CalibrateShooter.java
+66
View File
@@ -0,0 +1,66 @@
/*----------------------------------------------------------------------------*/
/* 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 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;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
/**
* 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) {
m_shooter = shootSub;
m_shooterAim = aimSub;
addRequirements(m_shooter, m_shooterAim);
}
// 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() {
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_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooterAim.m_shooterRotateEncoder.setPosition(0);
m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED);
}
// 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);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+10 -7
View File
@@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase {
double m_rampAcc;
long m_startTime;
long m_rampRate;
int m_counter;
/**
* Creates a new DrivePositionMPAux.
*
* @param subsystem The drive subsystem
* @param cruiseVel The target velocity for the motors in units
* @param cruiseVel The target velocity for the motors in in/s
* @param rampDist The distance before cruise velocity is reached in inches
* @param rampRate The time to reach the cruise velocity in seconds
* @param targetPos The target position
*/
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase {
m_targetVel = m_currentVel;
m_startTime = System.currentTimeMillis();
m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
m_counter = 0;
}
// Called every m_isRamptime the scheduler runs while the command is scheduled.
@@ -64,14 +66,15 @@ public class DrivePositionMPAux extends CommandBase {
if (System.currentTimeMillis() - m_startTime < m_rampRate) {
// Ramping
m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
} else if (m_targetPos - m_currentPos > m_rampDist) {
// Cruising
m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
} else {
// Deramp PID
m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
}
m_counter ++;
}
// Called once the command ends or is interrupted.
@@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
return true;
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
//return true;
}
return false;
}
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+13 -2
View File
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
public class DriveStraightToPositionMM extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
@@ -25,10 +27,19 @@ public class DriveStraightToPositionMM extends CommandBase {
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
}
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+14 -3
View File
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
@@ -24,10 +26,19 @@ public class DriveStraightToPositionPID extends CommandBase {
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
}
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
@@ -58,7 +69,7 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
return true;
} else {
i++;
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+38 -2
View File
@@ -7,13 +7,16 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystick extends CommandBase {
private Drive m_drive;
private IHandController m_controller;
private Pneumatics m_pneumatics;
/**
* Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
@@ -23,9 +26,10 @@ public class DriveWithJoystick extends CommandBase {
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystick(Drive subsystem, IHandController controller) {
public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = subsystem2;
m_controller = controller;
addRequirements(m_drive);
}
@@ -48,8 +52,15 @@ public class DriveWithJoystick extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
double cosMultiplier = .55;
double cosMultiplier = 1.0;
double deadzone = .1;
if (m_pneumatics.m_isSpeedShiftHigh) {
cosMultiplier = 0.8;
} else {
cosMultiplier = 1.0;
}
if (steerInput > 0){
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
} else if (steerInput < 0) {
@@ -58,6 +69,31 @@ public class DriveWithJoystick extends CommandBase {
steerOutput = 0;
}
/*
double outputLimit = 0.8;
boolean isMoveOutputLimited = false;
boolean isSteerOutputLimited = false;
if (m_pneumatics.m_isSpeedShiftHigh) {
if (isMoveOutputLimited) {
if (moveOutput > outputLimit) {
moveOutput = outputLimit;
} else if(moveOutput < -outputLimit) {
moveOutput = -outputLimit;
}
}
if (isSteerOutputLimited) {
if (steerOutput > outputLimit) {
steerOutput = outputLimit;
} else if(steerOutput < -outputLimit) {
steerOutput = -outputLimit;
}
}
}
*/
m_drive.driveWithInput(moveOutput, steerOutput);
}
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+13 -6
View File
@@ -21,6 +21,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
boolean m_isInterrupted;
/**
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -54,6 +55,11 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
if (m_isInterrupted) {
resetGyroTarget();
m_isInterrupted = false;
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
@@ -78,14 +84,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double cosMultiplier = .7;
double steerOutput = 0;
double deadzone = .2;
double deadzone = .1;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
if (steer > 0) {
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
} else if (steer < 0) {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
@@ -108,6 +114,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_isInterrupted = interrupted;
}
// Returns true when the command should end.
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+11 -8
View File
@@ -12,10 +12,12 @@ 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;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
@@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_drive = subsystemDrive;
m_pneumatics = subsystemPneumatics;
m_controller = controller;
addRequirements(m_drive);
}
@@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
if (m_drive.m_isSpeedShiftHigh) {
if (m_pneumatics.m_isSpeedShiftHigh) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
@@ -119,14 +122,14 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runDriveWithInput(double move, double steerInput) {
double cosMultiplier = .55;
double cosMultiplier = .70;
double steerOutput = 0;
double deadzone = .2;
double deadzone = .1;
/* Curves the steer output to be similarily gradual */
if (steerInput > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
steerOutput = -(cosMultiplier - deadzone)*Math.cos(1.571*steerInput)+(cosMultiplier);
} else if (steerInput < 0) {
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
steerOutput = (cosMultiplier - deadzone)*Math.cos(1.571*steerInput)-(cosMultiplier);
}
m_drive.driveWithInput(move, steerOutput);
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+87
View File
@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
// 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 GotoCoordinates extends SequentialCommandGroup {
Drive m_drive;
Pneumatics m_pneumatics;
double m_xTarget;
double m_yTarget;
double m_currentAngle;
double m_hypotDist;
double m_endAngle;
/**
* Creates a new GotoPosition.
*/
public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
m_pneumatics = subsystem2;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = endAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
m_drive = subsystem;
m_pneumatics = subsystem2;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = m_currentAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
public boolean isQuadrantThree() {
if ((m_xTarget < 0) && (m_yTarget < 0)) {
return true;
} else {
return false;
}
}
public double calcAngle() {
if (isQuadrantThree()) {
return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
} else {
return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
}
}
}
src/main/java/frc4388/robot/commands/HoldTarget.java
+117
View File
@@ -0,0 +1,117 @@
/*----------------------------------------------------------------------------*/
/* 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/HoodPositionPID.java
+56
View File
@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
double firingAngle;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(Shooter subSystem) {
m_shooter = subSystem;
//addRequirements(m_shooter);
}
// 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() {
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);
}
// 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() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
+5 -5
View File
@@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase {
double rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if (rightTrigger < .5) {
if(rightTrigger > leftTrigger) {
output = rightTrigger;
if (leftTrigger < .5) {
if(leftTrigger > rightTrigger) {
output = leftTrigger;
}
if (leftTrigger > rightTrigger) {
if (rightTrigger > leftTrigger) {
output = -leftTrigger;
}
} else {
output = rightTrigger;
output = leftTrigger;
}
m_intake.runIntake(output);
}
src/main/java/frc4388/robot/commands/ShootFireGroup.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
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 ShootFireGroup extends ParallelRaceGroup {
/**
* Fires the shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, 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)
);
}
}
src/main/java/frc4388/robot/commands/StorageIntakeGroup.java → src/main/java/frc4388/robot/commands/ShootFullGroup.java
+11 -7
View File
@@ -8,20 +8,24 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
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 StorageIntakeGroup extends SequentialCommandGroup {
public class ShootFullGroup extends SequentialCommandGroup {
/**
* Creates a new StorageIntakeGroup.
* Preps and Fires the Shooter
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
addCommands(
new storagePrepIntake(m_intake, m_storage),
new storageIntake(m_intake, m_storage),
new StorageIntakeFinal(m_storage));
new ShootPrepGroup(m_shooter, m_shooterAim, m_storage),
new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
);
}
}
src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
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 {
/**
* 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),
new ShooterVelocityControlPID(m_shooter),
new StoragePrepAim(m_storage),
new HoodPositionPID(m_shooter)
);
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+22 -7
View File
@@ -7,32 +7,37 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
/**
* Creates a new ShooterVelocityControlPID.
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
*/
public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
public ShooterVelocityControlPID(Shooter subsystem) {
m_shooter = subsystem;
m_targetVel = targetVel;
addRequirements(m_shooter);
}
// 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() {
System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
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());
}
// Called once the command ends or is interrupted.
@@ -43,6 +48,16 @@ public class ShooterVelocityControlPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
//Tells whether the target velocity has been reached
double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
if (m_actualVel < upperBound && m_actualVel > lowerBound){
SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
return true;
}
else{
SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
return false;
}
}
}
src/main/java/frc4388/robot/commands/StorageIntake.java
+64
View File
@@ -0,0 +1,64 @@
/*----------------------------------------------------------------------------*/
/* 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 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;
public class StorageIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public boolean intook;
/**
* Runs the Storage in for intaking
* @param inSub The Intake subsystem
* @param storeSub The Storage subsytem
*/
public StorageIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
intook = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (!m_storage.getBeam(0)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
intook = true;
}
else{
m_storage.runStorage(0);
}
}
// 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() {
if (m_storage.getBeam(0) && intook){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storageOutake.java → src/main/java/frc4388/robot/commands/StorageOutake.java
+6 -4
View File
@@ -8,14 +8,16 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storageOutake extends CommandBase {
public class StorageOutake extends CommandBase {
Storage m_storage;
/**
* Creates a new storageOutake.
* Runs the Storage out for outaking
* @param storeSub The Storage subsystem
*/
public storageOutake(Storage storeSub) {
public StorageOutake(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
@@ -28,7 +30,7 @@ public class storageOutake extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_storage.runStorage(-0.5);
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/storagePrepIntake.java → src/main/java/frc4388/robot/commands/StoragePositionPID.java
+12 -16
View File
@@ -8,36 +8,31 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storagePrepIntake extends CommandBase {
public Intake m_intake;
public class StoragePositionPID extends CommandBase {
public Storage m_storage;
double startPos;
/**
* Creates a new storagePrepIntake.
* Moves the storage a number of rotations
* @param subsystem The Storage subsystem
*/
public storagePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
public StoragePositionPID(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
}
// 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_storage.getBeam(1) == false){
m_storage.runStorage(-0.5);
}
else{
m_storage.runStorage(0);
}
m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
}
// Called once the command ends or is interrupted.
@@ -48,9 +43,10 @@ public class storagePrepIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(1)){
/*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
{
return true;
}
}*/
return false;
}
}
src/main/java/frc4388/robot/commands/storagePrepAim.java → src/main/java/frc4388/robot/commands/StoragePrepAim.java
+13 -6
View File
@@ -7,15 +7,19 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class storagePrepAim extends CommandBase {
public class StoragePrepAim extends CommandBase {
Storage m_storage;
double startTime;
/**
* Creates a new storagePrepAim.
* Prepares the Storage for aiming
* @param storeSub The Storage subsystem
*/
public storagePrepAim(Storage storeSub) {
public StoragePrepAim(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
@@ -23,13 +27,14 @@ public class storagePrepAim extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(2) == false){
m_storage.runStorage(0.5);
if (m_storage.getBeam(1)){
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
else{
m_storage.runStorage(0);
@@ -44,9 +49,11 @@ public class storagePrepAim extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(2)){
if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
SmartDashboard.putBoolean("StoragePrepAim Finished", true);
return true;
}
SmartDashboard.putBoolean("StoragePrepAim Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/storageIntake.java → src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+12 -8
View File
@@ -8,16 +8,21 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Storage;
public class storageIntake extends CommandBase {
public class StoragePrepIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
public double startTime;
/**
* Creates a new storageIntake.
* Prepares the Storage for intaking
* @param inSub The Intake subsystem
* @param storeSub the Storage subsystem
*/
public storageIntake(Intake inSub, Storage storeSub) {
public StoragePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
@@ -27,18 +32,17 @@ public class storageIntake extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(0)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
m_intake.runExtender(-0.3);
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
}
else{
m_intake.runExtender(0.3);
m_storage.runStorage(0);
}
}
@@ -50,7 +54,7 @@ public class storageIntake extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(0)){
if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
return true;
}
return false;
src/main/java/frc4388/robot/commands/StorageIntakeFinal.java → src/main/java/frc4388/robot/commands/StorageRun.java
+8 -8
View File
@@ -8,16 +8,17 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StorageIntakeFinal extends CommandBase {
public class StorageRun extends CommandBase {
Storage m_storage;
/**
* Creates a new StorageIntakeFinal.
* Runs the Storage at a speed
* @param storageSub The Storage subsytem
*/
public StorageIntakeFinal(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
public StorageRun(Storage storageSub) {
m_storage = storageSub;
}
// Called when the command is initially scheduled.
@@ -28,14 +29,13 @@ public class StorageIntakeFinal extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_storage.getBeam(1)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
}
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storage.runStorage(0);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/commands/TrackTarget.java
+58 -25
View File
@@ -9,21 +9,29 @@ package frc4388.robot.commands;
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;
public class TrackTarget extends CommandBase {
//Setup
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
// Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
@@ -32,66 +40,91 @@ public class TrackTarget extends CommandBase {
public static double fireVel;
public static double fireAngle;
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) {
public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
addRequirements(m_shooter);
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
// 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.
@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_shooter.runShooterWithInput(turnAmount/5);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
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);
//START Equation Code
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);
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*/
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
//Drive Camera Mode
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() {
if (xAngle < 1 && xAngle > -1 && target == 1)
{
SmartDashboard.putBoolean("TrackTarget Finished", true);
return true;
}
SmartDashboard.putBoolean("TrackTarget Finished", false);
return false;
}
}
src/main/java/frc4388/robot/commands/TrimShooter.java
+69
View File
@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* 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 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;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
public double turretTrim = 0;
public double hoodTrim = 0;
public Shooter m_shooter;
/**
* Trims the shooter based on the D-Pad inputs
* @param shootSub The Shooter subsytems
*/
public TrimShooter(Shooter shootSub) {
m_shooter = shootSub;
}
// 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_operatorXbox.getDPadTop()){
hoodTrim += 1;
}
else if(m_operatorXbox.getDPadBottom()){
hoodTrim -= 1;
}
else if(m_operatorXbox.getDPadRight()){
turretTrim += 1;
}
else if(m_operatorXbox.getDPadLeft()){
turretTrim -= 1;
}
m_shooter.shooterTrims.m_turretTrim = turretTrim;
m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
}
// 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/TurnDegrees.java
+2 -2
View File
@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
/**
* Creates a new TurnDeg.
*/
public TurnDegrees(double targetAngle, Drive subsystem) {
public TurnDegrees(Drive subsystem, double targetAngle) {
// Use addRequirements() here to declare subsystem dependencies.
m_targetAngle = targetAngle;
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
return true;
}
return false;
src/main/java/frc4388/robot/commands/Wait.java
+12 -1
View File
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
long m_waitTime;
long m_currentTime;
SubsystemBase m_subsystem;
int m_waitNum;
int counter = 0;
/**
* Creates a new WaitCommand.
*/
public Wait(float seconds, SubsystemBase subsystem) {
public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
// Use addRequirements() here to declare subsystem dependencies.
m_waitTime = (long) (seconds * 1000);
m_subsystem = subsystem;
m_waitNum = waitNum;
addRequirements(m_subsystem);
}
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
}
m_currentTime = System.currentTimeMillis();
SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
counter ++;
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Camera.java
+2 -2
View File
@@ -11,6 +11,7 @@ import edu.wpi.cscore.MjpegServer;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -38,8 +39,7 @@ public class Camera extends SubsystemBase {
catch(Exception e){
System.err.println("Camera broken, pls nerf");
}
}
}
@Override
public void periodic() {
src/main/java/frc4388/robot/subsystems/Climber.java
+4 -4
View File
@@ -31,10 +31,10 @@ public class Climber extends SubsystemBase {
m_climberMotor.setIdleMode(IdleMode.kBrake);
m_climberMotor.setInverted(false);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberForwardLimit.enableLimitSwitch(false);
m_climberReverseLimit.enableLimitSwitch(false);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberForwardLimit.enableLimitSwitch(true);
m_climberReverseLimit.enableLimitSwitch(true);
}
@Override
src/main/java/frc4388/robot/subsystems/Drive.java
+63 -66
View File
@@ -36,23 +36,25 @@ 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;
public class Drive extends SubsystemBase {
/* Create Motors, Gyros, Solenoids, etc */
/* Create Motors, Gyros, etc */
public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
/* Drive objects to manage Drive Train */
public DifferentialDrive m_driveTrain;
public final DifferentialDriveOdometry m_odometry;
public Orchestra m_orchestra;
/* Pneumatics Subsystem */
Pneumatics m_pneumaticsSubsystem;
/* Low Gear Gains */
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
@@ -86,11 +88,11 @@ public class Drive extends SubsystemBase {
public double m_lastAngleYaw = 0;
public double m_currentAngleYaw = 0;
public double m_lastAngleGotoCoordinates;
/* Smart Dashboard Objects */
SendableChooser<String> m_songChooser = new SendableChooser<String>();
/* Misc */
public boolean m_isSpeedShiftHigh;
String m_currentSong = "";
/**
@@ -105,12 +107,6 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
/* Config Open Loop Ramp so we don't make sudden output changes */
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -118,11 +114,16 @@ public class Drive extends SubsystemBase {
m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
/* Config Supply Current Limit (Use only for debugging) */
m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
/* Config deadbands so that */
m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -131,7 +132,15 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
/* PID for Front Motor Control in Teleop */
setRightMotorGains(false);
try {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
setRightMotorGains(true);
} else {
setRightMotorGains(false);
}
} catch (Exception e) {
System.err.println("Error while trying to switch gains.");
}
/* PID for Back Motor Control in Tank Drive Vel */
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -251,6 +260,13 @@ public class Drive extends SubsystemBase {
/* Set up Orchestra */
m_orchestra = new Orchestra();
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
//m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
/* Set up music for drive train */
m_orchestra.addInstrument(m_leftBackMotor);
@@ -273,14 +289,33 @@ public class Drive extends SubsystemBase {
@Override
public void periodic() {
updateTime();
updateAngles();
updatePosition();
updateSmartDashboard();
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Pneumatics subsystem) {
m_pneumaticsSubsystem = subsystem;
}
public void updateTime() {
m_lastTimeMs = m_currentTimeMs;
m_currentTimeMs = System.currentTimeMillis();
m_currentTimeSec = m_currentTimeMs / 1000;
m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
}
public void updateAngles() {
m_lastAngleYaw = m_currentAngleYaw;
m_currentAngleYaw = getGyroYaw();
}
public void updatePosition() {
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
@@ -295,9 +330,6 @@ public class Drive extends SubsystemBase {
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
getDistanceInches(m_leftFrontMotor),
-getDistanceInches(m_rightFrontMotor));
runFalconCooling();
updateSmartDashboard();
}
/**
@@ -431,47 +463,6 @@ public class Drive extends SubsystemBase {
m_driveTrain.feedWatchdog();
}
/**
* Set to high or low gear based on boolean state, true = high, false = low
* @param state Chooses between high or low gear
*/
public void setShiftState(boolean state) {
if (state == true) {
m_speedShift.set(DoubleSolenoid.Value.kReverse);
}
if (state == false) {
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
setRightMotorGains(state);
m_isSpeedShiftHigh = state;
}
/**
* Set to open or close solenoid that cools the falcon, true = open, false = close
* @param state Chooses between open and close
*/
public void coolFalcon(boolean state) {
if (state == true) {
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
}
}
/**
*
*/
public void runFalconCooling() {
if (m_currentTimeSec % 30 == 0) {
coolFalcon(true);
SmartDashboard.putBoolean("Solenoid", true);
} else if ((m_currentTimeSec - 1) % 30 == 0) {
coolFalcon(false);
SmartDashboard.putBoolean("Solenoid", false);
}
}
/**
* Selects a song to play!
* @param song The name of the song to be played
@@ -624,7 +615,7 @@ public class Drive extends SubsystemBase {
* @return The converted value in inches
*/
public double ticksToInches(double ticks) {
if (m_isSpeedShiftHigh) {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
} else {
return ticks * DriveConstants.INCHES_PER_TICK_LOW;
@@ -637,7 +628,7 @@ public class Drive extends SubsystemBase {
* @return The converted value in ticks
*/
public double inchesToTicks(double inches) {
if (m_isSpeedShiftHigh) {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
return inches * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
return inches * DriveConstants.TICKS_PER_INCH_LOW;
@@ -747,8 +738,14 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
//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 Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
@@ -780,9 +777,9 @@ public class Drive extends SubsystemBase {
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));
@@ -790,7 +787,7 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
if (m_currentSong != m_songChooser.getSelected()){
m_currentSong = m_songChooser.getSelected();
src/main/java/frc4388/robot/subsystems/Intake.java
+3 -3
View File
@@ -41,9 +41,9 @@ public class Intake extends SubsystemBase {
m_extenderMotor.setInverted(false);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_extenderForwardLimit.enableLimitSwitch(true);
m_extenderReverseLimit.enableLimitSwitch(true);
}
@Override
src/main/java/frc4388/robot/subsystems/LimeLight.java
+35
View File
@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class LimeLight extends SubsystemBase {
/**
* Creates a new LimeLight.
*/
public LimeLight() {
}
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Pneumatics.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import 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 {
/* Create Solenoids */
public DoubleSolenoid m_speedShift = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID,
PneumaticsConstants.SPEED_SHIFT_FORWARD_ID,
PneumaticsConstants.SPEED_SHIFT_REVERSE_ID );
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID,
PneumaticsConstants.COOL_FALCON_FORWARD_ID,
PneumaticsConstants.COOL_FALCON_REVERSE_ID );
/* Get Drive Subsystem */
Drive m_driveSubsystem;
public boolean m_isSpeedShiftHigh;
/**
* Creates a new Pneumatics.
*/
public Pneumatics() {
}
@Override
public void periodic() {
// This method will be called once per scheduler run
runFalconCooling();
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Drive subsystem) {
m_driveSubsystem = subsystem;
}
/**
* Set to high or low gear based on boolean state, true = high, false = low
* @param state Chooses between high or low gear
*/
public void setShiftState(boolean state) {
if (state == true) {
m_speedShift.set(DoubleSolenoid.Value.kReverse);
}
if (state == false) {
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
m_driveSubsystem.setRightMotorGains(state);
m_isSpeedShiftHigh = state;
}
/**
* Set to open or close solenoid that cools the falcon, true = open, false = close
* @param state Chooses between open and close
*/
public void coolFalcon(boolean state) {
if (state == true) {
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
}
}
/**
* Runs coolFalcon every 30 seconds for 1 second.
*/
public void runFalconCooling() {
if (m_driveSubsystem.m_currentTimeSec % 30 == 0) {
coolFalcon(true);
} else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) {
coolFalcon(false);
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+59 -87
View File
@@ -13,9 +13,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Joystick;
@@ -23,6 +26,7 @@ import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Trims;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
@@ -31,22 +35,15 @@ public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public static Shooter m_shooter;
public static IHandController m_controller;
// Configure PID Controllers
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
double velP;
double input;
@@ -55,20 +52,23 @@ public class Shooter extends SubsystemBase {
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
public Trims shooterTrims;
/*
* Creates a new Shooter subsystem.
* Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
*/
public Shooter() {
//Testing purposes reseting gyros
resetGyroAngleAdj();
resetGyroShooterRotate();
//resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
m_shooterFalcon.configFactoryDefault();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(false);
m_shooterFalcon.setInverted(true);
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -90,6 +90,19 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override
@@ -105,6 +118,7 @@ public class Shooter extends SubsystemBase {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -118,89 +132,47 @@ public class Shooter extends SubsystemBase {
*/
public void setShooterGains() {
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* Runs drum shooter velocity PID.
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
velP = actualVel/targetVel; //Percent of target
double runSpeed = actualVel + (12000*velP); //Ramp up equation
SmartDashboard.putNumber("shoot", actualVel);
runSpeed = runSpeed/targetVel; //Convert to percent
if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
//Tells wether the target velocity has been reached
double upperBound = targetVel + 300;
double lowerBound = targetVel - 300;
if (actualVel < upperBound && actualVel > lowerBound)
{
velReached = true;
}
else{
velReached = false;
}
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double targetAngle)
{
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
/**
* Runs drum shooter velocity PID.
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
SmartDashboard.putNumber("shoot", actualVel);
if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
m_shooterFalcon.feed();
}
/* For Testing Purposes, reseting gyro for angle adjuster */
public void resetGyroAngleAdj()
{
m_angleEncoder.setPosition(0);
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
public double getAnglePosition(){
return m_angleAdjustMotor.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.ShooterConstants;
public class ShooterAim extends SubsystemBase {
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a subsytem for the turret aiming
*/
public ShooterAim() {
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit.enableLimitSwitch(true);
m_shooterLeftLimit.enableLimitSwitch(true);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
public double getShooterRotatePosition(){
return m_shooterRotateMotor.getEncoder().getPosition();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+9 -1
View File
@@ -20,12 +20,13 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.WaitUntilCommand;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -69,6 +70,11 @@ public class Storage extends SubsystemBase {
m_encoder.setPosition(0);
}
public void testBeams(){
SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
}
/* Storage PID Control */
public void runStoragePositionPID(double targetPos){
// Set PID Coefficients
@@ -79,6 +85,8 @@ public class Storage extends SubsystemBase {
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
SmartDashboard.putNumber("Storage Position PID Target", targetPos);
SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}