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

Merge pull request #45 from Team4388/fix-drive-train

Browse Source 
Fix Drive Train
This commit is contained in:
ryan123rudder
2020-02-27 19:20:02 -07:00
committed by GitHub
parent 03a9e063b9 82286cb180
commit 70a23eab1f
10 changed files with 224 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+22 -12
View File
@@ -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;
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -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
src/main/java/frc4388/robot/RobotContainer.java
+40 -8
View File
@@ -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);
}
/**
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+7 -4
View File
@@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase {
double m_rampAcc;
long m_startTime;
long m_rampRate;
int m_counter;
/**
* Creates a new DrivePositionMPAux.
*
* @param subsystem The drive subsystem
* @param cruiseVel The target velocity for the motors in units
* @param cruiseVel The target velocity for the motors in in/s
* @param rampDist The distance before cruise velocity is reached in inches
* @param rampRate The time to reach the cruise velocity in seconds
* @param targetPos The target position
*/
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase {
m_targetVel = m_currentVel;
m_startTime = System.currentTimeMillis();
m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
m_counter = 0;
}
// Called every m_isRamptime the scheduler runs while the command is scheduled.
@@ -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;
}
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+14 -12
View File
@@ -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);
}
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+7 -4
View File
@@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
@@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_drive = subsystemDrive;
m_pneumatics = subsystemPneumatics;
m_controller = controller;
addRequirements(m_drive);
}
@@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
if (m_drive.m_isSpeedShiftHigh) {
if (m_pneumatics.m_isSpeedShiftHigh) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
src/main/java/frc4388/robot/commands/TurnDegrees.java
+1 -1
View File
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
return true;
}
return false;
src/main/java/frc4388/robot/subsystems/Drive.java
+43 -67
View File
@@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.PneumaticsConstants;
import frc4388.robot.Gains;
public class Drive extends SubsystemBase {
/* Create Motors, Gyros, Solenoids, etc */
/* Create Motors, Gyros, etc */
public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
/* Drive objects to manage Drive Train */
public DifferentialDrive m_driveTrain;
public final DifferentialDriveOdometry m_odometry;
public Orchestra m_orchestra;
/* Pneumatics Subsystem */
Pneumatics m_pneumaticsSubsystem;
/* Low Gear Gains */
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
@@ -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();
src/main/java/frc4388/robot/subsystems/Pneumatics.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Constants.PneumaticsConstants;
public class Pneumatics extends SubsystemBase {
/* Create Solenoids */
public DoubleSolenoid m_speedShift = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID,
PneumaticsConstants.SPEED_SHIFT_FORWARD_ID,
PneumaticsConstants.SPEED_SHIFT_REVERSE_ID );
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID,
PneumaticsConstants.COOL_FALCON_FORWARD_ID,
PneumaticsConstants.COOL_FALCON_REVERSE_ID );
/* Get Drive Subsystem */
Drive m_driveSubsystem;
public boolean m_isSpeedShiftHigh;
/**
* Creates a new Pneumatics.
*/
public Pneumatics() {
}
@Override
public void periodic() {
// This method will be called once per scheduler run
runFalconCooling();
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Drive subsystem) {
m_driveSubsystem = subsystem;
}
/**
* Set to high or low gear based on boolean state, true = high, false = low
* @param state Chooses between high or low gear
*/
public void setShiftState(boolean state) {
if (state == true) {
m_speedShift.set(DoubleSolenoid.Value.kReverse);
}
if (state == false) {
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
m_driveSubsystem.setRightMotorGains(state);
m_isSpeedShiftHigh = state;
}
/**
* Set to open or close solenoid that cools the falcon, true = open, false = close
* @param state Chooses between open and close
*/
public void coolFalcon(boolean state) {
if (state == true) {
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
}
}
/**
* Runs coolFalcon every 30 seconds for 1 second.
*/
public void runFalconCooling() {
if (m_driveSubsystem.m_currentTimeSec % 30 == 0) {
coolFalcon(true);
} else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) {
coolFalcon(false);
}
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
-1
View File
@@ -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;