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 branch 'master' into shooter-and-limelight-tuning

Browse Source
This commit is contained in:
ryan123rudder
2020-03-07 17:38:42 -07:00
committed by GitHub
parent 7f9b8d9c9f e9754feabd
commit a45f94a570
10 changed files with 266 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -34,11 +34,11 @@ public final class Constants {
public static final boolean isRightMotorInverted = true;
public static final boolean isLeftMotorInverted = false;
public static final boolean isRightArcadeInverted = false;
public static final boolean isAuxPIDInverted = false;
public static final boolean isAuxPIDInverted = true;
/* Drive Configuration */
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 OPEN_LOOP_RAMP_RATE = 0.2; // 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, 60, 40, 2);
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -106,6 +106,8 @@ public class Robot extends TimedRobot {
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(false);
m_robotContainer.shiftClimberRachet(false);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
// This makes sure that the autonomous stops running when
src/main/java/frc4388/robot/RobotContainer.java
+9 -1
View File
@@ -165,7 +165,7 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
// Disengages the rachet to allow for a climb
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
.whileHeld(new DisengageRachet(m_robotClimber));
/* Operator Buttons */
@@ -187,6 +187,7 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
@@ -301,6 +302,13 @@ public class RobotContainer {
m_robotPneumatics.setShiftState(state);
}
/**
*
*/
public void shiftClimberRachet(boolean state) {
m_robotClimber.shiftServo(state);
}
/**
*
*/
src/main/java/frc4388/robot/commands/climber/DisengageRachet.java
+2 -1
View File
@@ -29,8 +29,9 @@ public class DisengageRachet extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_climber.climberSafety) {
if (m_climber.m_climberSafety) {
m_climber.shiftServo(false);
System.err.println("Disengage Rachet");
}
}
src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java
+1
View File
@@ -44,6 +44,7 @@ public class RunClimberWithTriggers extends CommandBase {
if (leftTrigger > rightTrigger) {
output = -leftTrigger;
m_climber.shiftServo(true);
System.err.println("Engage Rachet");
}
} else {
output = rightTrigger;
src/main/java/frc4388/robot/subsystems/Climber.java
+11 -6
View File
@@ -15,6 +15,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClimberConstants;
@@ -25,7 +26,8 @@ public class Climber extends SubsystemBase {
Servo m_servo;
//Spark m_spark = new Spark(4);
public boolean climberSafety = false;
public boolean m_climberSafety = false;
public boolean m_isRachetEngaged = true;
/**
* Creates a new Climber.
@@ -47,6 +49,8 @@ public class Climber extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putBoolean("Climber Safety", m_climberSafety);
SmartDashboard.putBoolean("Rachet", m_isRachetEngaged);
}
/**
@@ -54,7 +58,7 @@ public class Climber extends SubsystemBase {
* @param input the voltage to run motor at
*/
public void runClimber(double input) {
if(climberSafety){
if(m_climberSafety){
input *= 1.0;
m_climberMotor.set(input);
}
@@ -66,13 +70,13 @@ public class Climber extends SubsystemBase {
/* Safety Button for Climber */
public void setSafetyPressed()
{
climberSafety = true;
m_climberSafety = true;
}
/* Safety Button for Climber set back to false */
public void setSafetyNotPressed()
{
climberSafety = false;
m_climberSafety = false;
}
/**
@@ -80,9 +84,10 @@ public class Climber extends SubsystemBase {
*/
public void shiftServo(boolean shift) {
if (shift) {
m_servo.setPosition(0.5);
m_servo.setPosition(0.48);
} else {
m_servo.setPosition(0.56);
}
}
m_isRachetEngaged = shift;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+231 -130
View File
@@ -24,9 +24,11 @@ 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;
@@ -43,7 +45,8 @@ public class Drive extends SubsystemBase {
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;
/* Drive objects to manage Drive Train */
public DifferentialDrive m_driveTrain;
public final DifferentialDriveOdometry m_odometry;
@@ -88,7 +91,7 @@ public class Drive extends SubsystemBase {
public double m_lastAngleGotoCoordinates;
/* Smart Dashboard Objects */
SendableChooser<String> m_songChooser = new SendableChooser<String>();
/* Misc */
String m_currentSong = "";
@@ -104,6 +107,7 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
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);
@@ -117,12 +121,12 @@ public class Drive extends SubsystemBase {
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);
// 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 */
/* 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);
@@ -145,56 +149,62 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.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
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
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
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
/*
* 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*/
/* 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);
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.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, 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);
/*
* 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);
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/**
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
@@ -232,28 +242,24 @@ public class Drive extends SubsystemBase {
* 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_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_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_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);
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()));
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()),
new Pose2d(0, 0, new Rotation2d()));
/* Set up Orchestra */
m_orchestra = new Orchestra();
@@ -263,8 +269,8 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
//m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
/* Set up music for drive train */
m_orchestra.addInstrument(m_leftBackMotor);
m_orchestra.addInstrument(m_rightFrontMotor);
@@ -275,7 +281,7 @@ public class Drive extends SubsystemBase {
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
String[] songsStrings = songsDir.list();
for (String songString : songsStrings){
for (String songString : songsStrings) {
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
@@ -294,6 +300,7 @@ public class Drive extends SubsystemBase {
/**
* Passes subsystem needed.
*
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Pneumatics subsystem) {
@@ -324,9 +331,8 @@ public class Drive extends SubsystemBase {
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
getDistanceInches(m_leftFrontMotor),
-getDistanceInches(m_rightFrontMotor));
m_odometry.update(Rotation2d.fromDegrees(getHeading()), getDistanceInches(m_leftFrontMotor),
-getDistanceInches(m_rightFrontMotor));
}
/**
@@ -334,14 +340,16 @@ 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);
}
/**
* Runs percent output control on the drive train while using an AUX PID for rotation
* @param targetPos The position to drive to in units
* 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) {
@@ -356,8 +364,9 @@ public class Drive extends SubsystemBase {
}
/**
* Runs position PID.
* Position is absolute and displacement should be handled on the command side.
* 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
*/
@@ -393,7 +402,8 @@ public class Drive extends SubsystemBase {
/**
* Runs motion magic PID while driving straight
* @param targetPos The position to drive to in units
*
* @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) {
@@ -416,7 +426,7 @@ public class Drive extends SubsystemBase {
* @param targetAngle target angle in degrees
*/
public void runTurningPID(double targetAngle) {
//double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
// double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
runDriveVelocityPID(0, targetAngle);
}
@@ -428,24 +438,27 @@ public class Drive extends SubsystemBase {
* @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);
// 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;
// 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);
// SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
// SmartDashboard.putNumber("Move Vel Right", moveVelRight);
//runDriveVelocityPID(moveVel*2, targetGyro);
// runDriveVelocityPID(moveVel*2, targetGyro);
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -462,13 +475,14 @@ public class Drive extends SubsystemBase {
/**
* 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!
*/
@@ -525,7 +539,7 @@ public class Drive extends SubsystemBase {
m_pigeon.getYawPitchRoll(ypr);
return ypr[0];
}
}
/**
* Returns the current pitch of the pigeon
@@ -547,13 +561,47 @@ public class Drive extends SubsystemBase {
return ypr[2];
}
//lol
//sko
//ridge
//brayden=bad coder
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();
}
@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() {
@@ -573,6 +621,7 @@ public class Drive extends SubsystemBase {
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
@@ -581,15 +630,17 @@ public class Drive extends SubsystemBase {
/**
* Returns current wheel speeds of robot.
*
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds( getVelocityInchesPerSecond(m_leftBackMotor),
-getVelocityInchesPerSecond(m_rightBackMotor));
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
*/
@@ -599,19 +650,22 @@ public class Drive extends SubsystemBase {
/**
* 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);
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) {
public double ticksToInches(double ticks) {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
} else {
@@ -621,6 +675,7 @@ public class Drive extends SubsystemBase {
/**
* Converts a value in inches to ticks.
*
* @param inches The value in inches to convert
* @return The converted value in ticks
*/
@@ -634,6 +689,7 @@ public class Drive extends SubsystemBase {
/**
* Converts a value in inches to meters.
*
* @param inches The value in inches to convert
* @return The converted value in meters
*/
@@ -643,6 +699,7 @@ public class Drive extends SubsystemBase {
/**
* Converts a value in meters to inches.
*
* @param meters The value in meters to convert
* @return The converted value in inches
*/
@@ -653,65 +710,104 @@ public class Drive extends SubsystemBase {
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.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.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.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.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.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.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.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.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);
}
}
@@ -730,9 +826,14 @@ public class Drive extends SubsystemBase {
public void updateSmartDashboard() {
try {
//SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
// 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 Front Output", m_leftFrontMotor.get());
//SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
//SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
src/main/java/frc4388/robot/subsystems/Leveler.java
+1 -1
View File
@@ -39,7 +39,7 @@ public class Leveler extends SubsystemBase {
* @param input the percent output to run motor at
*/
public void runLeveler(double input) {
if(m_climberSubsystem.climberSafety){
if(m_climberSubsystem.m_climberSafety){
m_levelerMotor.set(input);
}
else{
src/main/java/frc4388/robot/subsystems/Pneumatics.java
+3
View File
@@ -8,6 +8,7 @@
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;
@@ -36,6 +37,8 @@ public class Pneumatics extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
runFalconCooling();
SmartDashboard.putBoolean("Gear Shift", m_isSpeedShiftHigh);
}
/**
src/main/java/frc4388/robot/subsystems/Storage.java
+4 -1
View File
@@ -46,7 +46,10 @@ public class Storage extends SubsystemBase {
@Override
public void periodic() {
// NO
//SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
//SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
//SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
//SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
}
/**