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 add-shooter-subsystem-PID-new

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-11 00:16:16 +00:00
committed by GitHub
parent 9d8f047bed df1468d6ae
commit 57cbc028b9
23 changed files with 922 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+55 -20
View File
@@ -19,6 +19,7 @@ import frc4388.utility.LEDPatterns;
*/
public final class Constants {
public static final class DriveConstants {
/* Drive Train IDs */
public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
@@ -26,31 +27,48 @@ public final class Constants {
public static final int PIGEON_ID = 6;
/* PID Constants Drive*/
public static final int DRIVE_SLOT_IDX = 0;
public static final int DRIVE_PID_LOOP_IDX = 0;
public static final int DRIVE_TIMEOUT_MS = 30;
public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY = 2000;
public static final int DRIVE_ACCELERATION = 1000;
/* Remote Sensors */
public final static int REMOTE_0 = 0;
public final static int REMOTE_1 = 1;
public static final double ENCODER_TICKS_PER_REV = 2048;
/* PID Indexes */
public final static int PID_PRIMARY = 0;
public final static int PID_TURN = 1;
/* PID SLOTS */
public final static int SLOT_DISTANCE = 0;
public final static int SLOT_VELOCITY = 1;
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*2;
public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5;
public static final double WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
/* Ratio Calculation */
public static final double TICK_TIME_TO_SECONDS = 0.1;
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
}
public static final class IntakeConstants {
public static final int INTAKE_SPARK_ID = 1;
}
public static final class ElevatorConstants {
public static final int TALON_1 = 7;
public static final int TALON_2 = 8;
/* PID Constants Elevator */
public static final int ELEVATOR_SLOT_IDX = 0;
public static final int ELEVATOR_PID_LOOP_IDX = 1;
public static final int ELEVATOR_TIMEOUT_MS = 30;
public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0);
public static final double ENCODER_TICKS_PER_REV = 2048;
}
public static final class ShooterConstants {
public static final int SHOOTER_FALCON_ID = 8;
@@ -62,10 +80,27 @@ public final class Constants {
public static final double ENCODER_TICKS_PER_REV = 2048;
}
public static final class ClimberConstants {
public static final int CLIMBER_SPARK_ID = 10;
}
public static final class LevelerConstants {
public static final int LEVELER_CAN_ID = 9;
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = -1;
public static final int BEAM_SENSOR_DIO_0 = 0;
public static final int BEAM_SENSOR_DIO_1 = 1;
public static final int BEAM_SENSOR_DIO_2 = 2;
public static final int BEAM_SENSOR_DIO_3 = 3;
public static final int BEAM_SENSOR_DIO_4 = 4;
public static final int BEAM_SENSOR_DIO_5 = 5;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
src/main/java/frc4388/robot/Gains.java
+22 -13
View File
@@ -11,20 +11,29 @@ package frc4388.robot;
* Add your docs here.
*/
public class Gains {
public double kP;
public double kI;
public double kD;
public double kF;
public int kIzone;
public double kPeakOutput;
public double m_kP;
public double m_kI;
public double m_kD;
public double m_kF;
public int m_kIzone;
public double m_kPeakOutput;
public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput)
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIzone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at.
*/
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
{
kP = _kP;
kI = _kI;
kD = _kD;
kF = _kF;
kIzone = _kIzone;
kPeakOutput = _kPeakOutput;
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kPeakOutput = kPeakOutput;
}
}
src/main/java/frc4388/robot/Robot.java
-2
View File
@@ -98,7 +98,6 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -113,7 +112,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+53 -19
View File
@@ -18,13 +18,22 @@ import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveAtVelocityPID;
import frc4388.robot.commands.DriveToDistanceMM;
import frc4388.robot.commands.DriveToDistancePID;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.ShooterVelocityControlPID;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
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.subsystems.Leveler;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
@@ -42,6 +51,9 @@ public class RobotContainer {
private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -58,9 +70,16 @@ public class RobotContainer {
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
// drives motor with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// 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));
// drives the leveler with an axis input from the driver controller
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));
}
/**
@@ -74,32 +93,41 @@ public class RobotContainer {
// test command to spin the robot while pressing A on the driver controller
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive);
//new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
// .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
/* 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(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveToDistancePID(m_robotDrive, 5000));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveToDistanceMM(m_robotDrive, 5000));
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000));
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
/* PID Test Command */
// runs velocity PID while driving straight
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
// resets the yaw of the pigeon
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
/*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(null, m_robotDrive));*/
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter));
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
// interrupts any running command
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
}
/**
* Sets Motors to a NeutralMode.
* @param mode NeutralMode to set motors to
@@ -119,14 +147,16 @@ public class RobotContainer {
}
/**
* Add your docs here.
* Used for analog inputs like triggers and axises.
* @return IHandController interface for the Driver Controller.
*/
public IHandController getDriverController() {
return m_driverXbox;
}
/**
* Add your docs here.
* Used for analog inputs like triggers and axises.
* @return The IHandController interface for the Operator Controller.
*/
public IHandController getOperatorController()
{
@@ -134,7 +164,9 @@ public class RobotContainer {
}
/**
* Add your docs here.
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
* @return The IHandController interface for the Operator Controller.
*/
public Joystick getOperatorJoystick()
{
@@ -142,7 +174,9 @@ public class RobotContainer {
}
/**
* Add your docs here.
* Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox Controller.
* Generic HIDs/Joysticks can be used to set up JoystickButtons.
* @return The IHandController interface for the Driver Controller.
*/
public Joystick getDriverJoystick()
{
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+53
View File
@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* 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.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightAtVelocityPID extends CommandBase {
Drive m_drive;
double m_targetVel;
double m_targetGyro;
/**
* Creates a new DriveStraightAtVelocityPID.
* @param subsystem The drive subsystem
* @param targetVel The target velocity for the motors in units
*/
public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
}
// 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/DriveStraightToPositionPID.java
+60
View File
@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* 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.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_targetPos;
double m_targetGyro;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
}
// 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 (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/commands/DriveToDistanceMM.java
+6 -3
View File
@@ -8,6 +8,7 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveToDistanceMM extends CommandBase {
@@ -18,11 +19,13 @@ public class DriveToDistanceMM extends CommandBase {
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param distance distance to travel in inches
*/
public DriveToDistanceMM(Drive subsystem, double distance) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance;
m_distance = distance * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
}
@@ -36,8 +39,8 @@ public class DriveToDistanceMM extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
//m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
//m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+6 -1
View File
@@ -16,7 +16,12 @@ public class DriveWithJoystick extends CommandBase {
private IHandController m_controller;
/**
* Creates a new DriveWithJoystick.
* Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* @param subsystem 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 DriveWithJoystick(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java
+63
View File
@@ -0,0 +1,63 @@
/*----------------------------------------------------------------------------*/
/* 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.Climber;
import frc4388.utility.controller.IHandController;
public class RunClimberWithTriggers extends CommandBase {
private Climber m_climber;
private IHandController m_controller;
/**
* Uses input from opperator triggers to control climber motor
* @param subsystem the climber subsystem
* @param controller the driver controller
*/
public RunClimberWithTriggers(Climber subsystem, IHandController controller) {
m_climber = subsystem;
m_controller = controller;
addRequirements(m_climber);
}
// 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 rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if (rightTrigger < .5) {
if(rightTrigger > leftTrigger) {
output = rightTrigger;
}
if (leftTrigger > rightTrigger) {
output = -leftTrigger;
}
} else {
output = rightTrigger;
}
m_climber.runClimber(output);
}
// 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/RunIntakeWithTriggers.java
+7 -4
View File
@@ -16,9 +16,12 @@ public class RunIntakeWithTriggers extends CommandBase {
private IHandController m_controller;
/**
* Uses input from opperator triggers to control intake motor
* @param subsystem the intake subsystem
* @param controller the operator controller
* Uses input from opperator triggers to control intake motor.
* The right trigger will run the intake in and the left trigger will run it out.
* @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunIntakeWithTriggers(Intake subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
@@ -43,7 +46,7 @@ public class RunIntakeWithTriggers extends CommandBase {
output = rightTrigger;
}
if (leftTrigger > rightTrigger) {
output = leftTrigger;
output = -leftTrigger;
}
} else {
output = rightTrigger;
src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java
+53
View File
@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* 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.Leveler;
import frc4388.utility.controller.IHandController;
public class RunLevelerWithJoystick extends CommandBase {
private Leveler m_leveler;
private IHandController m_controller;
/**
* Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
* @param subsystem 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 RunLevelerWithJoystick(Leveler subsystem, IHandController controller) {
m_leveler = subsystem;
m_controller = controller;
addRequirements(m_leveler);
}
// 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 input = m_controller.getLeftXAxis();
m_leveler.runLeveler(input);
}
// 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/subsystems/Climber.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.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_forwardLimit, m_reverseLimit;
/**
* Creates a new Climber.
*/
public Climber() {
m_climberMotor.restoreFactoryDefaults();
m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_forwardLimit.enableLimitSwitch(false);
m_reverseLimit.enableLimitSwitch(false);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Runs climber motor
* @param input the voltage to run motor at
*/
public void runClimber(double input) {
m_climberMotor.set(input);
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+294 -52
View File
@@ -7,13 +7,23 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
@@ -34,7 +44,13 @@ public class Drive extends SubsystemBase {
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
public static Gains m_driveGains = DriveConstants.DRIVE_GAINS;
SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
public DoubleSolenoid speedShift;
/**
* Add your docs here.
@@ -48,68 +64,167 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
speedShift = new DoubleSolenoid(7,0,1);
/* set back motors as followers */
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
/* set neutral mode */
setDriveTrainNeutralMode(NeutralMode.Brake);
setDriveTrainNeutralMode(NeutralMode.Coast);
/* deadbands */
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); //Ensures motors run at the same speed
m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
/* Motor Encoders */
//m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
//m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
/*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
setDriveTrainGains();
/* Setup Sensors for WPI_TalonFXs */
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure the left Talon's selected sensor as local QuadEncoder */
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
RemoteSensorSource.TalonSRX_SelectedSensor,
DriveConstants.REMOTE_0, // Source number [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(),
RemoteSensorSource.Pigeon_Yaw,
DriveConstants.REMOTE_1,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Setup Sum signal to be used for Distance */
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Diff Signal */
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
DriveConstants.PID_PRIMARY, // PID Slot of Source
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1,
DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Set status frame periods to ensure we don't have stale data */
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
/* Smart Dashboard Initial Values */
/* Set up Chooser */
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
//setDriveTrainGains("Distance PID", m_gainsDistance);
m_chooser.addOption("Velocity PID", m_gainsVelocity);
//setDriveTrainGains("Velocity PID", m_gainsVelocity);
m_chooser.addOption("Turning PID", m_gainsTurning);
//setDriveTrainGains("Turning PID", m_gainsTurning);
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
//setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
Shuffleboard.getTab("PID").add(m_chooser);
/* Gyro */
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
/* Sensor Values */
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
/* PID */
Gains gains = m_chooser.getSelected();
Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP);
Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI);
Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD);
Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF);
/**
* Max out the peak output (for all modes).
* However you can limit the output of a given PID object with configClosedLoopPeakOutput().
*/
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* 1ms per loop. PID loop can be slowed down if need be.
* For example,
* - if sensor updates are too slow
* - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
* - sensor movement is very slow causing the derivative error to be near zero.
*/
int closedLoopTimeMs = 1;
m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* configAuxPIDPolarity(boolean invert, int timeoutMs)
* false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
* true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
}
@Override
@@ -124,17 +239,24 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
setDriveTrainGains();
SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
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));
} catch (Exception e) {
System.err.println("The programming team has failed successfully in the Drive Subsystem.");
System.err.println("Error in the Drive Subsystem");
//e.printStackTrace(System.err);
}
}
@@ -145,8 +267,55 @@ public class Drive extends SubsystemBase {
public void setDriveTrainNeutralMode(NeutralMode mode) {
m_leftFrontMotor.setNeutralMode(mode);
m_rightFrontMotor.setNeutralMode(mode);
m_leftFrontMotor.setNeutralMode(mode);
m_rightFrontMotor.setNeutralMode(mode);
m_leftBackMotor.setNeutralMode(mode);
m_rightBackMotor.setNeutralMode(mode);
}
/**
* Initializes the drive train gains kP, kI, kD, and kF
* @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID"
* @param gains A gains object which is the gains that are set for the slot
*/
public void setDriveTrainGains(String slot, Gains gains){
/* Distance */
if (slot.equals("Distance PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Velocity */
if (slot.equals("Velocity PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Turning */
if (slot.equals("Turning PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Motion Magic */
if (slot.equals("Motion Magic PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
}
}
/**
@@ -173,38 +342,111 @@ public class Drive extends SubsystemBase {
m_driveTrain.arcadeDrive(move, steer);
}
public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) {
talon.set(TalonFXControlMode.Position, targetPos);
/**
* Runs a position PID while driving straight (has not been tested)
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetPos *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) {
talon.set(TalonFXControlMode.Velocity, targetVel);
/**
* Runs velocity PID while driving straight
* @param targetVel The velocity to drive at in units
* @param targetGyro The angle to drive at in units
*/
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetVel *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){
talon.set(TalonFXControlMode.MotionMagic, targetPos);
/**
* Runs motion magic PID while driving straight (has not been tested)
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void runMotionMagicPID(double targetPos, double targetGyro){
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
/**
* Runs a Turning PID to rotate a to a target angle
* @param targetAngle target angle in degrees
*/
public void runTurningPID(double targetAngle){
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
runDriveStraightVelocityPID(0, targetGyro);
}
/**
* Returns the current yaw of the pigeon
*/
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[0];
}
/**
* Returns the current pitch of the pigeon
*/
public double getGyroPitch() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[1];
}
/**
* Returns the current roll of the pigeon
*/
public double getGyroRoll() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[2];
}
/**
* Resets the yaw of the pigeon
*/
public void resetGyroYaw() {
m_pigeon.setYaw(0);
m_pigeon.setAccumZAngle(0);
}
/**
* 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) {
speedShift.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
speedShift.set(DoubleSolenoid.Value.kReverse);
}
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+2 -1
View File
@@ -18,6 +18,7 @@ public class Intake extends SubsystemBase {
* Creates a new Intake.
*/
public Intake() {
m_intakeMotor.setInverted(false);
}
@@ -28,7 +29,7 @@ public class Intake extends SubsystemBase {
/**
* Runs intake motor
* @param input the voltage to run motor at
* @param input the percent output to run motor at
*/
public void runIntake(double input) {
m_intakeMotor.set(input);
src/main/java/frc4388/robot/subsystems/LED.java
+7 -7
View File
@@ -14,17 +14,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
/**
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
* Driver
*/
public class LED extends SubsystemBase {
public static float currentLED;
public static Spark LEDController;
/**
* Add your docs here.
* Creates a new LED to run a 5v LED Strip using a Rev
* Robotics Blinkin LED Driver
*/
public LED(){
LEDController = new Spark(LEDConstants.LED_SPARK_ID);
@@ -34,14 +31,17 @@ public class LED extends SubsystemBase {
}
/**
* Add your docs here.
* Sends an update to the LED Driver with the current LED value.
* This method should be run continously to keep the lights on.
*/
public void updateLED(){
LEDController.set(currentLED);
}
/**
* Add your docs here.
* Sets the current LED pattern. This method should only be run
* whenever you want to change the current LED.
* @param pattern LEDPattern to set the Blinkin to.
*/
public void setPattern(LEDPatterns pattern){
currentLED = pattern.getValue();
src/main/java/frc4388/robot/subsystems/Leveler.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.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LevelerConstants;
public class Leveler extends SubsystemBase {
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
/**
* Creates a new Leveler.
*/
public Leveler() {
m_levelerMotor.restoreFactoryDefaults();
m_levelerMotor.setIdleMode(IdleMode.kCoast);
m_levelerMotor.setInverted(false);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Runs intake motor
* @param input the percent output to run motor at
*/
public void runLeveler(double input) {
m_levelerMotor.set(input);
}
}
src/main/java/frc4388/robot/subsystems/Storage.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.subsystems;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
/**
* Creates a new Storage.
*/
public Storage() {
m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3);
m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4);
m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5);
}
@Override
public void periodic() {
// NO
}
/**
* Runs storage motor
* @param input the voltage to run motor at
*/
public void runStorage(double input) {
m_storageMotor.set(input);
boolean beam_on = m_beamSensors[0].get();
if (beam_on) {
System.err.println("Beam on");
} else {
System.err.println("Beam off");
}
}
}
src/main/java/frc4388/utility/LEDPatterns.java
+2 -1
View File
@@ -1,7 +1,8 @@
package frc4388.utility;
/**
* Add your docs here.
* Enum to hold all the different patterns for a Blinkin LED Driver.
* Use in place of a double when setting the Blinkin output.
*/
public enum LEDPatterns {
/* PALLETTE PATTERNS */
src/main/java/frc4388/utility/controller/IHandController.java
+1 -1
View File
@@ -1,7 +1,7 @@
package frc4388.utility.controller;
/**
* Add your docs here.
* Interface for the {@link XboxController}.
*/
public interface IHandController {
src/main/java/frc4388/utility/controller/XboxController.java
+5 -4
View File
@@ -3,7 +3,7 @@ package frc4388.utility.controller;
import edu.wpi.first.wpilibj.Joystick;
/**
* This is a wrapper for the WPILib Joystick class that represents an XBox
* This is a wrapper for the WPILib Joystick class that represents an Xbox
* controller.
* @author frc1675
*/
@@ -53,14 +53,15 @@ public class XboxController implements IHandController
private Joystick m_stick;
/**
* Add your docs here.
*/
* Creates a new Xbox Controller
* @param portNumber ID of Xbox Controller
*/
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
/**
* Add your docs here.
* @return Joystick for Xbox Controller
*/
public Joystick getJoyStick() {
return m_stick;
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+1 -2
View File
@@ -5,7 +5,7 @@ import frc4388.utility.controller.XboxController;
/**
* Mapping for the Xbox controller triggers to allow triggers to be defined as
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
* buttons in {@link frc4388.robot.RobotContainer RobotContainer}. Checks to see if the given trigger
* exceeds a tolerance defined in {@link XboxController}.
*/
public class XboxTriggerButton extends Button {
@@ -31,7 +31,6 @@ public class XboxTriggerButton extends Button {
m_trigger = trigger;
}
/** {@inheritDoc} */
@Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {