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

deleted more stuff lol

Browse Source
This commit is contained in:
aarav18
2022-02-18 20:58:10 -07:00
parent cb1e06b72f
commit 58b70f7e63
16 changed files with 48 additions and 1956 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -12,7 +12,7 @@ import java.util.function.Consumer;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
// import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
@@ -113,7 +113,7 @@ public final class Constants {
public static final double MAX_SPEED_METERS_PER_SECOND = 1.0;
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0;
public static final double TRACK_WIDTH_METERS = 0.648;
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
// public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
/* Remote Sensors */
public static final int REMOTE_0 = 0;
src/main/java/frc4388/robot/Robot.java
+4 -4
View File
@@ -73,7 +73,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void disabledInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
/* Builds Autos */
m_robotContainer.buildAutos();
SmartDashboard.putString("Is Auto Start?", "NAH");
@@ -82,7 +82,7 @@ public class Robot extends TimedRobot {
@Override
public void disabledPeriodic() {
m_robotContainer.resetOdometry(new Pose2d());
// m_robotContainer.resetOdometry(new Pose2d());
// m_robotContainer.idenPath();
if (m_modeChooser.getSelected() != Mode.get())
Mode.set(m_modeChooser.getSelected());
@@ -96,7 +96,7 @@ public class Robot extends TimedRobot {
//m_robotContainer.buildAutos();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// m_robotContainer.setDriveGearState(true);
m_initialTime = System.currentTimeMillis();
@@ -134,7 +134,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
// m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
// m_robotContainer.setDriveGearState(true);
// m_robotContainer.shiftClimberRatchet(false);
src/main/java/frc4388/robot/RobotContainer.java
+33 -33
View File
@@ -47,8 +47,8 @@ import frc4388.robot.Constants.OIConstants;
// import frc4388.robot.commands.climber.DisengageRatchet;
// import frc4388.robot.commands.climber.RunClimberWithTriggers;
// import frc4388.robot.commands.climber.RunLevelerWithJoystick;
import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.PlaySongDrive;
// import frc4388.robot.commands.drive.DriveWithJoystick;
// import frc4388.robot.commands.drive.PlaySongDrive;
// import frc4388.robot.commands.drive.SetShooterToOdo;
import frc4388.robot.commands.drive.VisionUpdateOdometry;
// import frc4388.robot.commands.intake.RunIntakeWithTriggers;
@@ -62,12 +62,12 @@ import frc4388.robot.commands.drive.VisionUpdateOdometry;
// import frc4388.robot.commands.storage.ManageStorage;
import frc4388.robot.subsystems.Camera;
// import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Drive;
// import frc4388.robot.subsystems.Drive;
// import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
// import frc4388.robot.subsystems.Leveler;
// import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Pneumatics;
// import frc4388.robot.subsystems.Pneumatics;
// import frc4388.robot.subsystems.Shooter_1;
// import frc4388.robot.subsystems.ShooterAim_1;
// import frc4388.robot.subsystems.ShooterHood_1;
@@ -86,7 +86,7 @@ import frc4388.utility.controller.XboxController;
*/
public class RobotContainer {
/* Subsystems */
private final Drive m_robotDrive = new Drive();
// 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();
@@ -194,11 +194,11 @@ public class RobotContainer {
// A driver test button
// new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON).whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand());
// new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
// // Y driver test button
// new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON).whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
// // X driver test button
// new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand());
/* Driver Buttons */
// sets solenoids into high gear
// new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
@@ -230,8 +230,8 @@ public class RobotContainer {
// new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
// .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
// new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
// .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whenPressed(new VisionUpdateOdometry(m_robotVision));//, m_robotShooterAim, m_robotDrive));
// new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
@@ -381,15 +381,15 @@ public class RobotContainer {
}
//TODO
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
}
// TrajectoryConfig getTrajectoryConfig() {
// return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
// // Add kinematics to ensure max speed is actually obeyed
// .setKinematics(DriveConstants.kDriveKinematics);
// }
public RamseteCommand getRamseteCommand(Trajectory trajectory) {
return new RamseteCommand(trajectory, m_robotDrive::getPose, new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive);
}
// public RamseteCommand getRamseteCommand(Trajectory trajectory) {
// return new RamseteCommand(trajectory, m_robotDrive::getPose, new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive);
// }
public RamseteCommand[] buildPaths(String[] paths) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
@@ -407,8 +407,8 @@ public class RobotContainer {
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
// RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
// ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();
}
@@ -417,8 +417,8 @@ public class RobotContainer {
String trajectoryJSON = "paths/" + path + ".wpilib.json";
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[i] = ramseteCommand;
// RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
// ramseteCommands[i] = ramseteCommand;
times[i] = trajectory.getTotalTimeSeconds();
}
} catch (Exception e) {
@@ -435,9 +435,9 @@ public class RobotContainer {
* Sets Motors to a NeutralMode.
* @param mode NeutralMode to set motors to
*/
public void setDriveNeutralMode(NeutralMode mode) {
m_robotDrive.setDriveTrainNeutralMode(mode);
}
// public void setDriveNeutralMode(NeutralMode mode) {
// m_robotDrive.setDriveTrainNeutralMode(mode);
// }
/**
* Sets the gear of the drivetrain
@@ -451,13 +451,13 @@ public class RobotContainer {
// m_robotClimber.shiftServo(state);
// }
public void resetOdometry(Pose2d pose) {
m_robotDrive.setOdometry(pose);
}
// public void resetOdometry(Pose2d pose) {
// m_robotDrive.setOdometry(pose);
// }
public void resetGyroYawRobotContainer(double angle) {
m_robotDrive.resetGyroYaw(angle);
}
// public void resetGyroYawRobotContainer(double angle) {
// m_robotDrive.resetGyroYaw(angle);
// }
/**
* Used for analog inputs like triggers and axises.
src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java
-93
View File
@@ -1,93 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DrivePositionMPAux extends CommandBase {
Drive m_drive;
double m_cruiseVel;
double m_rampDist;
double m_targetPos;
double m_currentVel;
double m_currentPos;
double m_targetGyro;
double m_targetVel;
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 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) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
m_rampRate = (long) rampRate * 1000;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
//m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currentVel = m_drive.m_rightFrontMotorVel;
m_currentPos = m_drive.m_rightFrontMotorPos;
m_targetPos = m_targetPos + m_currentPos;
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.
@Override
public void execute() {
m_currentVel = m_drive.m_rightFrontMotorVel;
m_currentPos = m_drive.m_rightFrontMotorPos;
if (System.currentTimeMillis() - m_startTime < m_rampRate) {
// Ramping
m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
} else if (m_targetPos - m_currentPos > m_rampDist) {
// Cruising
m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
} else {
// Deramp PID
m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
}
m_counter ++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
//return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightAtVelocityPID extends CommandBase {
Drive m_drive;
double m_targetVel;
double m_targetGyro;
/**
* Creates a new DriveStraightAtVelocityPID.
* @param subsystem The drive subsystem
* @param targetVel The target velocity for the motors in units
*/
public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java
-86
View File
@@ -1,86 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
public class DriveStraightToPositionMM extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
int i;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
}
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
//SmartDashboard.putBoolean("MM Run", true);
i++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
//SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
isGoneFast = true;
}
return false;
}
}
}
src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java
-79
View File
@@ -1,79 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
int i;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = subsystem2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
}
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
return true;
} else {
i++;
//System.err.println(i);
return false;
}
}
}
src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
-64
View File
@@ -1,64 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystick extends CommandBase {
private Drive m_drive;
private IHandController m_controller;
private Pneumatics m_pneumatics;
/**
* Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = subsystem2;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = m_controller.getLeftYAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
double steerInput = m_controller.getRightXAxis() * DriveConstants.STEER_WITH_JOYSTICK_FACTOR;
double moveOutput = Math.copySign(1 - Math.cos(Math.PI * moveInput / 2), moveInput);
double cosMultiplier = m_pneumatics.m_isSpeedShiftHigh ? DriveConstants.COS_MULTIPLIER_HIGH : DriveConstants.COS_MULTIPLIER_LOW;
double deadzone = 0.1;
double steerOutput = Math.copySign(Math.cos(Math.PI * steerInput / 2) * (deadzone - cosMultiplier) + cosMultiplier, steerInput);
m_drive.driveWithInput(moveOutput, steerOutput * 0.8);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java
-77
View File
@@ -1,77 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
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.utility.controller.IHandController;
public class DriveWithJoystickAuxPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
IHandController m_controller;
/**
* Creates a new DriveWithJoystickAuxPID.
*/
public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
lastTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
if (moveInput >= 0){
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp(m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
//System.err.println("Target: " + m_targetGyro);
//System.err.println("Current: " + currentGyro);
//System.err.println();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java
-142
View File
@@ -1,142 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickDriveStraight extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
boolean m_isInterrupted;
double highGearMultiplier = 1;
double lowGearMultiplier = 1;
/**
* Creates a new DriveWithJoystickDriveStraight 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 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 DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_pneumatics = m_drive.m_pneumaticsSubsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
if (m_isInterrupted) {
resetGyroTarget();
m_isInterrupted = false;
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If steer stick has not been used for 1 sec */
else {
runDriveStraight(moveOutput);
}
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier;
double steerOutput = 0;
double deadzone = .1;
if (m_pneumatics.m_isSpeedShiftHigh) {
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
} else {
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
}
/* Curves the steer output to be similarily gradual */
if (steer > 0) {
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
} else if (steer < 0) {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
}
m_drive.driveWithInput(move, steerOutput);
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
//m_targetGyro = m_currentGyro;
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetGyro = m_currentGyro
+ highGearMultiplier * m_drive.getTurnRate();
} else {
m_targetGyro = m_currentGyro
+ lowGearMultiplier * m_drive.getTurnRate();
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_isInterrupted = interrupted;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java
-194
View File
@@ -1,194 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
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;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
boolean m_isInterrupted;
/* Deadassist Constants */
final float stopPosVelCoefLow = 1;
final float stopPosVelCoefHigh = 3;
final float cosMultiplierLow = 0.55f;
final float cosMultiplierHigh = 0.35f;
final float targetAngleCoefLow = 5;
final float targetAngleCoefHigh = 5;
final float gyroVelCoefLow = 1;
final float gyroVelCoefHigh = 3;
/* Deadassist Coeficients */
final float stopPosVelCoef = 1;
final float cosMultiplier = 0.55f;
final float targetAngleCoef = 5;
final float gyroVelCoef = 1;
/**
* 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 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 subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystemDrive;
m_pneumatics = subsystemPneumatics;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currTime = System.currentTimeMillis();
resetGyroTarget();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
if (m_isInterrupted) {
resetGyroTarget();
m_isInterrupted = false;
}
/* If move stick is being used */
if (moveInput != 0) {
m_deadTimeMove = m_currTime;
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
if (m_pneumatics.m_isSpeedShiftHigh) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If move stick has been pressed within 1 sec */
else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If steer stick has not been used for 1 sec */
else {
runDriveStraight(moveOutput);
}
}
/* If the move stick has not been used for 1 sec */
else {
runStoppedTurn(steerInput);
}
}
private void runDriveWithInput(double move, double steerInput) {
double cosMultiplier = .70;
double steerOutput = 0;
double deadzone = .1;
/* Curves the steer output to be similarily gradual */
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);
}
m_drive.driveWithInput(move, steerOutput);
//System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
//System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
double cosMultiplier = 0.55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steer > 0) {
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else if (steer < 0) {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
}
updateGyroTarget(steerOutput);
double currentPos = m_drive.m_rightFrontMotorPos;
if (Math.abs(currentPos - m_stopPos) > 200) {
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
} else {
m_drive.driveWithInputAux(0, m_targetGyro);
}
//System.out.println("Turning with Target: " + m_targetGyro);
}
/**
* If AuxPID is enabled, then update using the steer input
*/
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_isInterrupted = interrupted;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java
-28
View File
@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
// 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 GotoCoordinatesFieldRelative extends SequentialCommandGroup {
/**
* Creates a new GotoCoordinatesFieldRelative.
*/
public GotoCoordinatesFieldRelative(Drive susbsytem, Pneumatics subsystem2, double xTarget, double yTarget) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
);
}
}
src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
public class PlaySongDrive extends CommandBase {
private Drive m_drive;
/**
* Creates a new PlaySongDrive.
*/
public PlaySongDrive(Drive subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.m_rightFrontMotor.set(0);
m_drive.m_leftFrontMotor.set(0);
m_drive.m_rightBackMotor.set(0);
m_drive.m_leftBackMotor.set(0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.playSong();
//System.err.println("Playing " + m_drive.m_orchestra.isPlaying());
//m_drive.m_driveTrain.feedWatchdog();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
+9 -9
View File
@@ -23,14 +23,14 @@ import edu.wpi.first.wpiutil.math.numbers.N5;
import edu.wpi.first.wpiutil.math.numbers.N6;
import frc4388.robot.Constants.VOPConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
// import frc4388.robot.subsystems.Drive;
// import frc4388.robot.subsystems.ShooterAim_1;
import frc4388.robot.subsystems.Vision;
public class VisionUpdateOdometry extends CommandBase {
private Vision m_limeLight;
// private ShooterAim_1 m_shooterAim;
private Drive m_driveTrain;
// private Drive m_driveTrain;
private double xPos;
private double yPos;
@@ -44,11 +44,11 @@ public class VisionUpdateOdometry extends CommandBase {
* @param shooterAim replace with Turret subsystem for integration with 2022
* @param driveTrain replace with Swerve subsystem for integration with 2022
*/
public VisionUpdateOdometry(Vision limeLight, Drive driveTrain) {
public VisionUpdateOdometry(Vision limeLight) {
m_limeLight = limeLight;
// m_shooterAim = shooterAim;
m_driveTrain = driveTrain;
addRequirements(m_limeLight, m_driveTrain);
// m_driveTrain = driveTrain;
addRequirements(m_limeLight);//, m_driveTrain);
// // Turn camera on but leave LEDs off
// NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
@@ -90,16 +90,16 @@ public class VisionUpdateOdometry extends CommandBase {
System.out.println("Never gonna give you up: " + distance);
double[] ypr = new double[3];
Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022
double relativeAngle = Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]);
rotation = new Rotation2d(ypr[0]);
// Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022
double relativeAngle = 0;//Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]);
rotation = new Rotation2d(0);
xPos = Math.cos(relativeAngle) * distance;
yPos = Math.sin(relativeAngle) * distance;
translate = new Translation2d(xPos, yPos);
Pose2d pose = new Pose2d(translate, rotation);
m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter
// m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter
SmartDashboard.putNumber("x:", pose.getX());
SmartDashboard.putNumber("y:", pose.getY());
m_limeLight.setLEDs(false);
src/main/java/frc4388/robot/subsystems/Drive.java
-951
View File
@@ -1,951 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 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 java.io.File;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.Gains;
public class Drive extends SubsystemBase {
/* 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 static GyroBase m_pigeonGyro;
public boolean m_isReversed;
/* Drive objects to manage Drive Train */
public DifferentialDrive m_driveTrain;
public final DifferentialDriveOdometry m_odometry;
public Orchestra m_orchestra;
/* Pneumatics Subsystem */
public Pneumatics m_pneumaticsSubsystem;
// Shooter_1 m_shooter;
/* Low Gear Gains */
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW;
public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW;
/* High Gear Gains */
public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
/* Back Motor Gains */
public static Gains m_gainsVelocityBack = DriveConstants.DRIVE_VELOCITY_GAINS_BACK;
/* Timey Whimey */
public long m_currentTimeMs = System.currentTimeMillis();
public long m_lastTimeMs = m_currentTimeMs;
public long m_deltaTimeMs = 0;
public long m_currentTimeSec = m_currentTimeMs / 1000;
/* Position Tracking */
public double m_rightFrontMotorPos = 0;
public double m_rightFrontMotorVel = 0;
public double m_totalLeftDistanceInches = 0;
public double m_totalRightDistanceInches = 0;
public double m_currentLeftPosTicks = 0;
public double m_currentRightPosTicks = 0;
public double m_lastLeftPosTicks = 0;
public double m_lastRightPosTicks = 0;
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 String m_currentSong = "";
public String[] songsStrings;
/**
* Add your docs here.
*/
public Drive() {
/* factory default values */
m_leftFrontMotor.configFactoryDefault();
m_rightFrontMotor.configFactoryDefault();
m_leftBackMotor.configFactoryDefault();
m_rightBackMotor.configFactoryDefault();
m_pigeon.configFactoryDefault();
resetGyroYaw(0);
m_pigeonGyro = getGyroInterface();
/* 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);
m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedloopRamp(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);
/* Config deadbands so that */
m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
/* PID for Front Motor Control in Teleop */
try {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
setRightMotorGains(true);
} else {
setRightMotorGains(false);
}
} catch (Exception e) {
System.err.println("Error while trying to switch gains.");
}
/* PID for Back Motor Control in Tank Drive Vel */
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
/* Reset Sensors for WPI_TalonFXs */
resetEncoders();
/* Configure the left Talon's selected sensor as local QuadEncoder */
m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Configure the left back Talon's selected sensor as local QuadEncoder */
m_leftBackMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Configure the right back Talon's selected sensor as local QuadEncoder */
m_rightBackMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/*
* Configure the Remote Talon's selected sensor as a remote sensor for the right
* Talon
*/
m_rightFrontMotor.configRemoteFeedbackFilter(m_leftFrontMotor.getDeviceID(), // Device ID of Source
RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Diff Signal signal to be used for Distance */
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure Diff [Sum of both QuadEncoders] to be used for Primary PID Index */
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSelectedFeedbackCoefficient(0.5, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
/*
* Configure the Pigeon IMU to the other Remote Slot available on the right
* Talon
*/
m_rightFrontMotor.configRemoteFeedbackFilter(m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
/* Config Remote1 to be used for Aux PID Index */
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/**
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
* output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
m_rightFrontMotor.configAuxPIDPolarity(DriveConstants.isAuxPIDInverted, DriveConstants.DRIVE_TIMEOUT_MS);
/* Set status frame periods to ensure we don't have stale data */
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* Max out the peak output (for all modes). However you can limit the output of
* a given PID object with configClosedLoopPeakOutput().
*/
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* 1ms per loop. PID loop can be slowed down if need be. For example, - if
* sensor updates are too slow - sensor deltas are very small per update, so
* derivative error never gets large enough to be useful. - sensor movement is
* very slow causing the derivative error to be near zero.
*/
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, DriveConstants.CLOSED_LOOP_TIME_MS,
DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Set up Differential Drive */
m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
/* Set up Differential Drive Odometry. */
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()),
new Pose2d(0, 0, new Rotation2d()));
/* 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);
m_orchestra.addInstrument(m_rightFrontMotor);
m_orchestra.addInstrument(m_rightBackMotor);
m_orchestra.addInstrument(m_leftFrontMotor);
/* Create chooser to choose song to play */
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
songsStrings = songsDir.list();
for (String songString : songsStrings) {
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
selectSong(songsStrings[0]);
/* Start counting time */
m_lastTimeMs = System.currentTimeMillis();
}
@Override
public void periodic() {
updateTime();
updateAngles();
updatePosition();
updateSmartDashboard();
}
/**
* Passes subsystem needed.
*
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Pneumatics subsystem) {
m_pneumaticsSubsystem = subsystem;
// m_shooter = shooter;
// m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
}
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();
m_lastRightPosTicks = m_currentRightPosTicks;
m_lastLeftPosTicks = m_currentLeftPosTicks;
m_currentRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition();
m_currentLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition();
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
updateOdometry(m_isReversed);
}
public void updateOdometry(boolean reversed){
if (reversed){
m_odometry.update(Rotation2d.fromDegrees( -getGyroYaw()-180),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)),
inchesToMeters(getDistanceInches(m_leftFrontMotor)));
}
else
{
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
}
public void switchReversed(boolean reversed)
{
m_isReversed = reversed;
}
/**
* Runs percent output control on the moving and steering of the drive train,
* using the Differential Drive class to manage the two inputs
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(-move, steer);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
}
/**
* Runs percent output control on the drive train while using an AUX PID for
* rotation
*
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void driveWithInputAux(double move, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs position PID. Position is absolute and displacement should be handled on
* the command side.
*
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void runDrivePositionPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs velocity PID
*
* @param targetVel The velocity to drive at in units
* @param targetGyro The angle to drive at in units
*/
public void runDriveVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs motion magic PID while driving straight
*
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void runMotionMagicPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs a Turning PID to rotate a to a target angle
*
* @param targetAngle target angle in degrees
*/
public void runTurningPID(double targetAngle) {
// double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
runDriveVelocityPID(0, targetAngle);
}
/**
* Controls the left and right sides of the drive with velocity targets.
*
* @param leftSpeed the commanded left speed
* @param rightSpeed the commanded right speed
*/
public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
// DifferentialDriveWheelSpeeds wheelSpeeds = new
// DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
// ChassisSpeeds chassisSpeeds =
// DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
// double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
// double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
// double angleVelDeg = Math.toDegrees(angleVelRad);
// m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
// m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
// m_currentAngleYaw-(360),
// m_currentAngleYaw+(360));
// double targetGyro = (m_kinematicsTargetAngle / 360) *
// DriveConstants.TICKS_PER_GYRO_REV;
double moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
double moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
// SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
// SmartDashboard.putNumber("Move Vel Right", moveVelRight);
// runDriveVelocityPID(moveVel*2, targetGyro);
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
m_leftFrontMotor.follow(m_leftBackMotor);
m_rightFrontMotor.follow(m_rightBackMotor);
m_driveTrain.feedWatchdog();
}
/**
* Selects a song to play!
*
* @param song The name of the song to be played
*/
public void selectSong(String song) {
SmartDashboard.putString("Selected Song", song);
m_orchestra.loadMusic(song);
}
/*
* Plays Music!
*/
public void playSong() {
m_orchestra.play();
}
/**
* Resets the encoders for both motors.
*/
public void resetEncoders() {
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_totalLeftDistanceInches = 0;
m_totalRightDistanceInches = 0;
}
/**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
public void setOdometry(Pose2d pose) {
resetEncoders();
resetGyroYaw(pose.getRotation().getDegrees());
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
* Resets the yaw of the pigeon
*/
public void resetGyroYaw(double angle) {
m_pigeon.setYaw(angle);
m_pigeon.setAccumZAngle(angle);
resetGyroAngles(angle);
}
/**
* Add docs here
*/
public void resetGyroAngles(double angle) {
m_lastAngleYaw = angle;
m_currentAngleYaw = angle;
}
/**
* Returns the current yaw of the pigeon
*/
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[0];
}
/**
* Returns the current pitch of the pigeon
*/
public double getGyroPitch() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[1];
}
/**
* Returns the current roll of the pigeon
*/
public double getGyroRoll() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[2];
}
public GyroBase getGyroInterface() {
return new GyroBase(){
@Override
public void close() throws Exception {
// TODO Auto-generated method stub
}
@Override
public void reset() {
// TODO Auto-generated method stub
resetGyroYaw(0);
}
@Override
public double getRate() {
// TODO Auto-generated method stub
return getTurnRate();
}
@Override
public double getAngle() {
// TODO Auto-generated method stub
return getGyroYaw();
}
@Override
public void calibrate() {
// TODO Auto-generated method stub
}
};
}
// lol
// sko
// ridge
// brayden=bad coder
/**
* Returns the heading of the robot
*
* @return The robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(getGyroYaw(), 360);
}
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
double turnRate = 1000 * deltaYaw / m_deltaTimeMs;
return turnRate;
}
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* Returns current wheel speeds of robot.
*
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(getVelocityInchesPerSecond(m_leftBackMotor),
-getVelocityInchesPerSecond(m_rightBackMotor));
}
/**
* Gets the encoder value (position) of a motor
*
* @param falcon The motor to get the position of
* @return The position of the motor in inches
*/
public double getDistanceInches(WPI_TalonFX falcon) {
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
}
/**
* Gets the encoder value (velocity) of a motor
*
* @param falcon The motor to get the velocity of
* @return The velocity of the motor in inches per second
*/
public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
return ticksToInches(
falcon.getSensorCollection().getIntegratedSensorPosition() / DriveConstants.TICK_TIME_TO_SECONDS);
}
/**
* Converts a value in ticks to inches.
*
* @param ticks The value in ticks to convert
* @return The converted value in inches
*/
public double ticksToInches(double ticks) {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
} else {
return ticks * DriveConstants.INCHES_PER_TICK_LOW;
}
}
/**
* Converts a value in inches to ticks.
*
* @param inches The value in inches to convert
* @return The converted value in ticks
*/
public double inchesToTicks(double inches) {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
return inches * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
return inches * DriveConstants.TICKS_PER_INCH_LOW;
}
}
/**
* Converts a value in inches to meters.
*
* @param inches The value in inches to convert
* @return The converted value in meters
*/
public double inchesToMeters(double inches) {
return inches * DriveConstants.METERS_PER_INCH;
}
/**
* Converts a value in meters to inches.
*
* @param meters The value in meters to convert
* @return The converted value in inches
*/
public double metersToInches(double meters) {
return meters * DriveConstants.INCHES_PER_METER;
}
/**
* Converts a value in inches per second to miles per hour
* @param ips The value in inches per second to convert
* @return The value in miles per hour
*/
public double inchesPerSecondToMilesPerHour(double ips) {
double mph = ips * (3600) * (1/63360);
return mph;
}
public void setRightMotorGains(boolean isHighGear) {
if (!isHighGear) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_MOTION_MAGIC,
m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
} else {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_MOTION_MAGIC,
m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
}
}
/**
* Sets Motors to a NeutralMode.
*
* @param mode NeutralMode to set motors to
*/
public void setDriveTrainNeutralMode(NeutralMode mode) {
m_leftFrontMotor.setNeutralMode(mode);
m_rightFrontMotor.setNeutralMode(mode);
m_leftBackMotor.setNeutralMode(mode);
m_rightBackMotor.setNeutralMode(mode);
}
public void updateSmartDashboard() {
try {
// SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
// SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
// SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro);
SmartDashboard.putData("Drive Train", m_driveTrain);
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition());
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
SmartDashboard.putNumber("Average Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
double avgSpeedMPH = inchesPerSecondToMilesPerHour(10 * ticksToInches(m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)));
SmartDashboard.putNumber("Avg Speed MPH", avgSpeedMPH);
//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());
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());
//SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
SmartDashboard.putNumber("Left Motor Pos Meters", inchesToMeters(getDistanceInches(m_rightFrontMotor)));
SmartDashboard.putNumber("Right Motor Pos Meters", inchesToMeters(getDistanceInches(m_leftFrontMotor)));
/*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
*/
//SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
//SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
//SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
//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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
//SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
if (m_currentSong != m_songChooser.getSelected()){
m_currentSong = m_songChooser.getSelected();
selectSong(m_currentSong);
//System.err.println(m_currentSong);
}
} catch (Exception e) {
System.err.println("Error while using Drive SmartDashboard");
// e.printStackTrace(System.err);
}
}
}
src/main/java/frc4388/robot/subsystems/Pneumatics.java
-90
View File
@@ -1,90 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.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();
SmartDashboard.putBoolean("Gear Shift", m_isSpeedShiftHigh);
}
/**
* 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);
}
}
}