mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' into add-shooter
This commit is contained in:
aarav18
@@ -30,26 +30,33 @@ public final class Constants {
|
||||
public static final int PIGEON_ID = 6;
|
||||
|
||||
/* Drive Inversions */
|
||||
public static final boolean isRightMotorInverted = false;
|
||||
public static final boolean isRightMotorInverted = true;
|
||||
public static final boolean isLeftMotorInverted = false;
|
||||
public static final boolean isRightArcadeInverted = false;
|
||||
public static final boolean isAuxPIDInverted = false;
|
||||
|
||||
/* Drive Configuration */
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.1;
|
||||
public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
|
||||
new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
|
||||
public static final int CLOSED_LOOP_TIME_MS = 1;
|
||||
new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
|
||||
public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
|
||||
|
||||
/* Drive Train Characteristics */
|
||||
public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
|
||||
public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 6;
|
||||
public static final double TICKS_PER_GYRO_REV = 8192;
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
|
||||
/* PID Constants Drive*/
|
||||
public static final int DRIVE_TIMEOUT_MS = 30;
|
||||
public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
|
||||
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
|
||||
public static final Gains DRIVE_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;
|
||||
public static final int DRIVE_CRUISE_VELOCITY = 25000;
|
||||
public static final int DRIVE_ACCELERATION = 21000;
|
||||
|
||||
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
@@ -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;
|
||||
@@ -103,10 +103,10 @@ public final class Constants {
|
||||
public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
|
||||
}
|
||||
|
||||
public static final class IntakeConstants {
|
||||
public static final int INTAKE_SPARK_ID = -9;
|
||||
public static final int EXTENDER_SPARK_ID = -10;
|
||||
public static final class IntakeConstants {;
|
||||
public static final double EXTENDER_SPEED = 0.3;
|
||||
public static final int INTAKE_SPARK_ID = 12;
|
||||
public static final int EXTENDER_SPARK_ID = 13;
|
||||
}
|
||||
|
||||
public static final class ShooterConstants {
|
||||
@@ -176,6 +176,16 @@ public final class Constants {
|
||||
public static final double STORAGE_MIN_OUTPUT = -1.0;
|
||||
public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
}
|
||||
|
||||
public static final class PneumaticsConstants {
|
||||
public static final int PCM_MODULE_ID = 7;
|
||||
|
||||
public static final int SPEED_SHIFT_FORWARD_ID = 0;
|
||||
public static final int SPEED_SHIFT_REVERSE_ID = 1;
|
||||
|
||||
public static final int COOL_FALCON_FORWARD_ID = 3;
|
||||
public static final int COOL_FALCON_REVERSE_ID = 2;
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
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
|
||||
|
||||
@@ -25,11 +25,15 @@ 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.CalibrateShooter;
|
||||
import frc4388.robot.commands.DrivePositionMPAux;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
@@ -42,6 +46,7 @@ import frc4388.robot.commands.RunExtenderOutIn;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
import frc4388.robot.commands.ShooterVelocityControlPID;
|
||||
import frc4388.robot.commands.StorageIntake;
|
||||
import frc4388.robot.commands.GotoCoordinates;
|
||||
import frc4388.robot.commands.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.RunExtenderOutIn;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
@@ -67,6 +72,12 @@ import frc4388.robot.commands.StorageRun;
|
||||
import frc4388.robot.subsystems.Camera;
|
||||
import frc4388.robot.subsystems.Leveler;
|
||||
import frc4388.robot.subsystems.LimeLight;
|
||||
import frc4388.robot.commands.TurnDegrees;
|
||||
import frc4388.robot.commands.Wait;
|
||||
import frc4388.robot.commands.storageOutake;
|
||||
import frc4388.robot.subsystems.Camera;
|
||||
import frc4388.robot.subsystems.Leveler;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
@@ -81,6 +92,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();
|
||||
@@ -103,6 +115,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 */
|
||||
@@ -134,15 +150,15 @@ public class RobotContainer {
|
||||
/* Test Buttons */
|
||||
// A driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192));
|
||||
|
||||
// B driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
.whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
|
||||
|
||||
// Y driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
.whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
|
||||
|
||||
// X driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
@@ -151,11 +167,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(true), m_robotDrive));
|
||||
|
||||
// sets solenoids into low gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
|
||||
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
|
||||
|
||||
|
||||
/* Operator Buttons */
|
||||
@@ -225,11 +241,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(0, m_robotDrive),
|
||||
//add aim command
|
||||
//add shooter command
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 48.0),
|
||||
/*new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
|
||||
new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DriveStraightToPositionMM(m_robotDrive, 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 GotoCoordinates(m_robotDrive, 36, 36),
|
||||
new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
|
||||
//new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
//new TurnDegrees(m_robotDrive, 75),
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 18.0),
|
||||
//new TurnDegrees(m_robotDrive, -45),
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 12.0));
|
||||
}
|
||||
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
return new TrajectoryConfig(
|
||||
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
@@ -281,7 +318,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.
|
||||
@@ -64,14 +66,15 @@ public class DrivePositionMPAux extends CommandBase {
|
||||
if (System.currentTimeMillis() - m_startTime < m_rampRate) {
|
||||
// Ramping
|
||||
m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
|
||||
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
|
||||
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
|
||||
} else if (m_targetPos - m_currentPos > m_rampDist) {
|
||||
// Cruising
|
||||
m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
|
||||
m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
|
||||
} else {
|
||||
// Deramp PID
|
||||
m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
|
||||
m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
|
||||
}
|
||||
m_counter ++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
|
||||
return true;
|
||||
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
|
||||
//return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
|
||||
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
|
||||
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
|
||||
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
|
||||
addRequirements(m_drive);
|
||||
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
|
||||
}
|
||||
|
||||
@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
|
||||
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
|
||||
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
|
||||
addRequirements(m_drive);
|
||||
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
|
||||
}
|
||||
@@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
|
||||
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
|
||||
return true;
|
||||
} else {
|
||||
i++;
|
||||
|
||||
@@ -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;
|
||||
@@ -48,7 +49,7 @@ public class DriveWithJoystick extends CommandBase {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
double cosMultiplier = .55;
|
||||
double cosMultiplier = 1.0;
|
||||
double deadzone = .1;
|
||||
if (steerInput > 0){
|
||||
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
|
||||
@@ -57,7 +58,24 @@ public class DriveWithJoystick extends CommandBase {
|
||||
} else {
|
||||
steerOutput = 0;
|
||||
}
|
||||
double tempOutputLimit = 0.8;
|
||||
|
||||
boolean isOutputLimited = false;
|
||||
|
||||
if (isOutputLimited) {
|
||||
if (moveOutput > tempOutputLimit) {
|
||||
moveOutput = tempOutputLimit;
|
||||
} else if(moveOutput < -tempOutputLimit) {
|
||||
moveOutput = -tempOutputLimit;
|
||||
}
|
||||
|
||||
if (steerOutput > tempOutputLimit) {
|
||||
steerOutput = tempOutputLimit;
|
||||
} else if(steerOutput < -tempOutputLimit) {
|
||||
steerOutput = -tempOutputLimit;
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
|
||||
public class GotoCoordinates extends SequentialCommandGroup {
|
||||
Drive m_drive;
|
||||
|
||||
double m_xTarget;
|
||||
double m_yTarget;
|
||||
double m_currentAngle;
|
||||
double m_hypotDist;
|
||||
double m_endAngle;
|
||||
|
||||
/**
|
||||
* Creates a new GotoPosition.
|
||||
*/
|
||||
public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
m_drive = subsystem;
|
||||
|
||||
m_xTarget = xTarget;
|
||||
m_yTarget = yTarget;
|
||||
|
||||
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
|
||||
|
||||
m_currentAngle = calcAngle();
|
||||
|
||||
SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle);
|
||||
|
||||
m_endAngle = endAngle;
|
||||
|
||||
|
||||
addCommands( new TurnDegrees(m_drive, m_currentAngle),
|
||||
new DriveStraightToPositionMM(m_drive, m_hypotDist),
|
||||
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
|
||||
}
|
||||
|
||||
public boolean isQuadrantThree() {
|
||||
if ((m_xTarget < 0) && (m_yTarget < 0)) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
public double calcAngle() {
|
||||
if (isQuadrantThree()) {
|
||||
return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
|
||||
} else {
|
||||
return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase {
|
||||
double rightTrigger = m_controller.getRightTriggerAxis();
|
||||
double leftTrigger = m_controller.getLeftTriggerAxis();
|
||||
double output = 0;
|
||||
if (rightTrigger < .5) {
|
||||
if(rightTrigger > leftTrigger) {
|
||||
output = rightTrigger;
|
||||
if (leftTrigger < .5) {
|
||||
if(leftTrigger > rightTrigger) {
|
||||
output = leftTrigger;
|
||||
}
|
||||
if (leftTrigger > rightTrigger) {
|
||||
if (rightTrigger > leftTrigger) {
|
||||
output = -leftTrigger;
|
||||
}
|
||||
} else {
|
||||
output = rightTrigger;
|
||||
output = leftTrigger;
|
||||
}
|
||||
m_intake.runIntake(output);
|
||||
}
|
||||
|
||||
@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
|
||||
/**
|
||||
* Creates a new TurnDeg.
|
||||
*/
|
||||
public TurnDegrees(double targetAngle, Drive subsystem) {
|
||||
public TurnDegrees(Drive subsystem, double targetAngle) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
|
||||
m_targetAngle = targetAngle;
|
||||
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
|
||||
if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Constants.PneumaticsConstants;
|
||||
import frc4388.robot.Gains;
|
||||
|
||||
public class Drive extends SubsystemBase {
|
||||
/* Create Motors, Gyros, Solenoids, etc */
|
||||
/* Create Motors, Gyros, etc */
|
||||
public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
|
||||
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
|
||||
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
|
||||
public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
|
||||
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
|
||||
|
||||
/* Drive objects to manage Drive Train */
|
||||
public DifferentialDrive m_driveTrain;
|
||||
public final DifferentialDriveOdometry m_odometry;
|
||||
public Orchestra m_orchestra;
|
||||
|
||||
/* Pneumatics Subsystem */
|
||||
Pneumatics m_pneumaticsSubsystem;
|
||||
|
||||
/* Low Gear Gains */
|
||||
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
|
||||
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
|
||||
@@ -86,11 +88,11 @@ public class Drive extends SubsystemBase {
|
||||
public double m_lastAngleYaw = 0;
|
||||
public double m_currentAngleYaw = 0;
|
||||
|
||||
public double m_lastAngleGotoCoordinates;
|
||||
/* Smart Dashboard Objects */
|
||||
SendableChooser<String> m_songChooser = new SendableChooser<String>();
|
||||
|
||||
/* Misc */
|
||||
public boolean m_isSpeedShiftHigh;
|
||||
String m_currentSong = "";
|
||||
|
||||
/**
|
||||
@@ -105,12 +107,6 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.configFactoryDefault();
|
||||
resetGyroYaw();
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
|
||||
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
|
||||
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
|
||||
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
|
||||
// m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
|
||||
|
||||
/* Config Open Loop Ramp so we don't make sudden output changes */
|
||||
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
@@ -119,10 +115,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 +247,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 +276,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 +317,6 @@ public class Drive extends SubsystemBase {
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
getDistanceInches(m_leftFrontMotor),
|
||||
-getDistanceInches(m_rightFrontMotor));
|
||||
|
||||
runFalconCooling();
|
||||
updateSmartDashboard();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -431,47 +450,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 +602,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 +615,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 +725,14 @@ public class Drive extends SubsystemBase {
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
|
||||
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
|
||||
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
//SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
//SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Left Motor RPM", leftRPM);
|
||||
SmartDashboard.putNumber("Right Motor RPM", rightRPM);
|
||||
|
||||
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
@@ -780,9 +764,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 +774,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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user