Merge branch 'master' into add-ramsete-controller-and-trajectories

This commit is contained in:
Keenan D. Buckley
2020-02-21 23:41:01 +00:00
committed by GitHub
36 changed files with 479 additions and 70 deletions
+22 -19
View File
@@ -1,21 +1,24 @@
MIT License
Copyright (c) 2009-2018 FIRST
All rights reserved.
Copyright (c) 2019 HFocus
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the FIRST nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+23 -6
View File
@@ -75,20 +75,26 @@ public final class Constants {
}
public static final class IntakeConstants {
public static final int INTAKE_SPARK_ID = 9;
public static final int EXTENDER_SPARK_ID = 10;
public static final int INTAKE_SPARK_ID = -9;
public static final int EXTENDER_SPARK_ID = -10;
}
public static final class ShooterConstants {
public static final int SHOOTER_FALCON_ID = 8;
/* 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;
/* 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 SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 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 SHOOTER_TURRET_GAINS = new Gains(0.2, 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 class ClimberConstants {
@@ -100,7 +106,7 @@ public final class Constants {
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = 10;
public static final int STORAGE_CAN_ID = -1;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
@@ -130,6 +136,17 @@ public final class Constants {
public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
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 TURN_P_VALUE = 0.65;
public static final double X_ANGLE_ERROR = 1.3;
public static final double MOTOR_DEAD_ZONE = 0.3;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
@@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystick;
@@ -44,6 +45,11 @@ import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.LEDPatterns;
@@ -67,6 +73,10 @@ public class RobotContainer {
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);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -87,11 +97,12 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the drum shooter in idle mode
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter));
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
// drives the leveler with an axis input from the driver controller
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
// m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
// runs storage motor at 50 percent
m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
}
/**
@@ -107,12 +118,15 @@ public class RobotContainer {
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
//new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
// .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
// .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -134,6 +148,7 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
@@ -146,9 +161,14 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
/* Storage Neo PID Test */
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1)));
.whileHeld(new TrackTarget(m_robotShooter));
}
/**
@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems.Drive;
public class PlaySongDrive extends CommandBase {
private Drive m_drive;
/**
* Creates a new PlaySongDrive.
*/
public PlaySongDrive(Drive subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.m_rightFrontMotor.set(0);
m_drive.m_leftFrontMotor.set(0);
m_drive.m_rightBackMotor.set(0);
m_drive.m_leftBackMotor.set(0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.playSong();
//System.err.println("Playing " + m_drive.m_orchestra.isPlaying());
//m_drive.m_driveTrain.feedWatchdog();
}
// 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;
}
}
@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* 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.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
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 TrackTarget extends CommandBase {
//Setup
NetworkTableEntry xEntry;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
/**
* Uses the Limelight to track the target
*/
public TrackTarget(Shooter shooterSubsystem) {
m_shooter = shooterSubsystem;
addRequirements(m_shooter);
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_shooter.runShooterWithInput(turnAmount/5);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
}
}
// 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() {
return false;
}
}
@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.cscore.MjpegServer;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Camera extends SubsystemBase {
CameraServer camServ = CameraServer.getInstance();
/**
* Creates a new Camera.
* Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
* @param name Name of the Camera in Shuffle Board.
* @param id USB Id of the Camera.
* @param width Resolution width.
* @param height Resolution height.
* @param brightness Percent brightness of the stream.
*/
public Camera(String name, int id, int width, int height, int brightness) {
try{
UsbCamera cam = new UsbCamera(name, id);
cam.setResolution(width, height);
cam.setBrightness(brightness);
cam.setFPS(10);
VideoSource camera = cam;
camServ.startAutomaticCapture(camera);
}
catch(Exception e){
System.err.println("Camera broken, pls nerf");
}
}
@Override
public void periodic() {
}
}
@@ -19,6 +19,9 @@ import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
public boolean climberSafety = false;
/**
* Creates a new Climber.
*/
@@ -44,6 +47,23 @@ public class Climber extends SubsystemBase {
* @param input the voltage to run motor at
*/
public void runClimber(double input) {
m_climberMotor.set(input);
if(climberSafety){
m_climberMotor.set(input);
}
else{
m_climberMotor.set(0);
}
}
/* Safety Button for Climber */
public void setSafetyPressed()
{
climberSafety = true;
}
/* Safety Button for Climber set back to false */
public void setSafetyNotPressed()
{
climberSafety = false;
}
}
@@ -7,6 +7,16 @@
package frc4388.robot.subsystems;
import java.io.File;
import java.io.FilenameFilter;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import java.util.stream.Stream;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -18,16 +28,20 @@ import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -48,6 +62,7 @@ public class Drive extends SubsystemBase {
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 Orchestra m_orchestra = new Orchestra();
public double m_rightFrontMotorPos;
@@ -67,7 +82,6 @@ public class Drive extends SubsystemBase {
public DoubleSolenoid m_coolFalcon;
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
public long m_lastTime, m_deltaTime; //in milliseconds
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
@@ -99,7 +113,7 @@ public class Drive extends SubsystemBase {
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_driveTrain.setRightSideInverted(false);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
@@ -268,8 +282,22 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
m_lastTime = System.currentTimeMillis();
m_orchestra.addInstrument(m_leftBackMotor);
m_orchestra.addInstrument(m_rightFrontMotor);
m_orchestra.addInstrument(m_rightBackMotor);
m_orchestra.addInstrument(m_leftFrontMotor);
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
String[] songsStrings = songsDir.list();
for (String songString : songsStrings){
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
}
String currentSong = "";
@Override
public void periodic() {
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
@@ -326,6 +354,11 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
if (currentSong != m_songChooser.getSelected()){
currentSong = m_songChooser.getSelected();
selectSong(currentSong);
System.err.println(currentSong);
}
} catch (Exception e) {
System.err.println("Error in the Drive Subsystem");
// e.printStackTrace(System.err);
@@ -389,7 +422,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
@@ -407,7 +440,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
@@ -425,6 +458,7 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
@@ -648,6 +682,21 @@ public class Drive extends SubsystemBase {
}
/*
* Plays Music!
*/
public void playSong() {
m_orchestra.play();
}
/**
* Selects a song to play!
* @param song The name of the song to be played
*/
public void selectSong(String song) {
SmartDashboard.putString("Selected Song", song);
m_orchestra.loadMusic(song);
}
/**
* Set to high or low gear based on boolean state, true = high, false = low
* @param state Chooses between high or low gear
*/
@@ -17,10 +17,13 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LevelerConstants;
import frc4388.robot.subsystems.*;
public class Leveler extends SubsystemBase {
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
private final Climber m_robotClimber = new Climber();
/**
* Creates a new Leveler.
*/
@@ -41,6 +44,11 @@ public class Leveler extends SubsystemBase {
* @param input the percent output to run motor at
*/
public void runLeveler(double input) {
m_levelerMotor.set(input);
if(m_robotClimber.climberSafety){
m_levelerMotor.set(input);
}
else{
m_levelerMotor.set(0);
}
}
}
@@ -11,25 +11,54 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.ControlType;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_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;
/*
* Creates a new Shooter subsystem.
*/
public Shooter() {
m_shooterFalcon.configFactoryDefault();
//Testing purposes reseting gyros
resetGyroAngleAdj();
resetGyroShooterRotate();
m_shooterFalcon.configFactoryDefault();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(false);
@@ -63,10 +92,10 @@ 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_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
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.
@@ -86,4 +115,55 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input);
}
/* 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.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);
}
/* For Testing Purposes, reseting gyro for angle adjuster */
public void resetGyroAngleAdj()
{
m_angleEncoder.setPosition(0);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
}
@@ -15,7 +15,6 @@ import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.SparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -57,13 +56,6 @@ public class Storage extends SubsystemBase {
public void runStorage(final double input) {
m_storageMotor.set(input);
final boolean beam_on = m_beamSensors[0].get();
if (beam_on) {
//System.err.println("Beam on");
} else {
//System.err.println("Beam off");
}
}
public void resetEncoder()
+15 -15
View File
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix.json",
"name": "CTRE-Phoenix",
"version": "5.17.4",
"version": "5.18.1",
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [
"http://devsite.ctr-electronics.com/maven/release/"
@@ -11,19 +11,19 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-java",
"version": "5.17.4"
"version": "5.18.1"
},
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-java",
"version": "5.17.4"
"version": "5.18.1"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.17.4",
"version": "5.18.1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -35,7 +35,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.17.4",
"version": "5.18.1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -47,7 +47,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.17.4",
"version": "5.18.1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -58,7 +58,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.17.4",
"version": "5.18.1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -69,7 +69,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.17.4",
"version": "5.18.1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -83,7 +83,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-cpp",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_Phoenix_WPI",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -97,7 +97,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-cpp",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_Phoenix",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -111,7 +111,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_PhoenixCCI",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -125,7 +125,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_PhoenixDiagnostics",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -139,7 +139,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_PhoenixCanutils",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -152,7 +152,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_PhoenixPlatform",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -165,7 +165,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.17.4",
"version": "5.18.1",
"libName": "CTRE_PhoenixCore",
"headerClassifier": "headers",
"sharedLibrary": false,
+37
View File
@@ -0,0 +1,37 @@
{
"fileName": "WPILibOldCommands.json",
"name": "WPILib-Old-Commands",
"version": "2020.0.0",
"uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
"mavenUrls": [],
"jsonUrl": "",
"javaDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-java",
"version": "wpilib"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "edu.wpi.first.wpilibOldCommands",
"artifactId": "wpilibOldCommands-cpp",
"version": "wpilib",
"libName": "wpilibOldCommands",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"linuxathena",
"linuxraspbian",
"linuxaarch64bionic",
"windowsx86-64",
"windowsx86",
"linuxx86-64",
"osxx86-64"
]
}
]
}