mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' of https://github.com/Team4388/RiseOfRidgebotics2020
This commit is contained in:
Aarav Shah
@@ -30,23 +30,30 @@ 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;
|
||||
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_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 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;
|
||||
@@ -78,13 +85,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;
|
||||
@@ -156,6 +156,16 @@ public final class Constants {
|
||||
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 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 {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
|
||||
@@ -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(true);
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
|
||||
@@ -21,12 +21,15 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RamseteCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.DrivePositionMPAux;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.RunExtenderOutIn;
|
||||
@@ -39,9 +42,12 @@ import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.commands.RunLevelerWithJoystick;
|
||||
import frc4388.robot.commands.TrackTarget;
|
||||
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;
|
||||
@@ -56,6 +62,7 @@ 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();
|
||||
@@ -75,6 +82,10 @@ public class RobotContainer {
|
||||
* 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 */
|
||||
@@ -103,11 +114,11 @@ public class RobotContainer {
|
||||
/* Test Buttons */
|
||||
// A driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
.whenPressed(new TurnDegrees(90, m_robotDrive));
|
||||
|
||||
// B driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
|
||||
|
||||
// Y driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
@@ -120,11 +131,11 @@ public class RobotContainer {
|
||||
/* 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(false), 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(true), m_robotDrive));
|
||||
|
||||
/* Operator Buttons */
|
||||
//TODO: Shooter Buttons
|
||||
@@ -175,11 +186,32 @@ public class RobotContainer {
|
||||
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
|
||||
// Run path following command, then stop at the end.
|
||||
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
|
||||
//Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
|
||||
//This assumes that we are positioned against the right wall with our shooter facing the target.
|
||||
return new SequentialCommandGroup(new Wait(2, m_robotDrive),
|
||||
//add aim command
|
||||
//add shooter command
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
|
||||
new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
|
||||
new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
//add aim command
|
||||
//add shooter command
|
||||
//Below this would be the picking up additional balls outside of those in the trench directly behind us
|
||||
|
||||
// return new InstantCommand();
|
||||
return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
|
||||
new TurnDegrees(-150, m_robotDrive),
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new TurnDegrees(75, m_robotDrive),
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
|
||||
new TurnDegrees(-45, m_robotDrive),
|
||||
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
|
||||
}
|
||||
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
return new TrajectoryConfig(
|
||||
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
@@ -231,7 +263,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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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.
|
||||
@@ -72,6 +74,7 @@ public class DrivePositionMPAux extends CommandBase {
|
||||
// Deramp PID
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
|
||||
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.utility.controller.IHandController;
|
||||
@@ -38,26 +39,27 @@ public class DriveWithJoystick extends CommandBase {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveInput = m_controller.getRightXAxis();
|
||||
double steerInput = -m_controller.getLeftYAxis();
|
||||
double moveOutput = 0;
|
||||
double steerOutput = 0;
|
||||
if (moveInput >= 0){
|
||||
moveOutput = -Math.cos(1.571*moveInput)+1;
|
||||
if (steerInput >= 0){
|
||||
steerOutput = -Math.cos(1.571*steerInput)+1;
|
||||
} else {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
steerOutput = Math.cos(1.571*steerInput)-1;
|
||||
}
|
||||
|
||||
double cosMultiplier = .55;
|
||||
double cosMultiplier = 1.0;
|
||||
double deadzone = .1;
|
||||
if (steerInput > 0){
|
||||
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
|
||||
} else if (steerInput < 0) {
|
||||
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
|
||||
if (moveInput > 0){
|
||||
moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
|
||||
} else if (moveInput < 0) {
|
||||
moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
|
||||
} else {
|
||||
steerOutput = 0;
|
||||
moveOutput = 0;
|
||||
}
|
||||
|
||||
|
||||
SmartDashboard.putNumber("Steer Output Test", moveOutput);
|
||||
m_drive.driveWithInput(moveOutput, steerOutput);
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
@@ -90,7 +92,6 @@ public class Drive extends SubsystemBase {
|
||||
SendableChooser<String> m_songChooser = new SendableChooser<String>();
|
||||
|
||||
/* Misc */
|
||||
public boolean m_isSpeedShiftHigh;
|
||||
String m_currentSong = "";
|
||||
|
||||
/**
|
||||
@@ -105,13 +106,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);
|
||||
m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
@@ -119,10 +113,10 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.configOpenloopRamp(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);
|
||||
@@ -251,6 +245,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 +274,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 +315,6 @@ public class Drive extends SubsystemBase {
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
getDistanceInches(m_leftFrontMotor),
|
||||
-getDistanceInches(m_rightFrontMotor));
|
||||
|
||||
runFalconCooling();
|
||||
updateSmartDashboard();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -305,7 +322,7 @@ public class Drive extends SubsystemBase {
|
||||
* using the Differential Drive class to manage the two inputs
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(steer, move);
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
}
|
||||
@@ -431,47 +448,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 +600,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 +613,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 +723,8 @@ 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());
|
||||
|
||||
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
@@ -780,9 +756,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 +766,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();
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
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.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
|
||||
Reference in New Issue
Block a user