From 1b31a6d5082c4ceb888a0dc9fe24b4fb79cf7a29 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:25:20 -0700 Subject: [PATCH 1/9] Cleanup Imports --- src/main/java/frc4388/robot/RobotContainer.java | 14 -------------- src/main/java/frc4388/robot/subsystems/Drive.java | 14 -------------- 2 files changed, 28 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97676e6..9bd371f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,7 +9,6 @@ package frc4388.robot; import java.util.List; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; @@ -17,7 +16,6 @@ import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; @@ -29,17 +27,11 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; -import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.DriveWithJoystickDriveStraight; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.commands.StorageIntakeGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -47,17 +39,11 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; -import frc4388.robot.commands.TurnDegrees; -import frc4388.robot.commands.Wait; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; -import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index b690162..fc5e310 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -8,15 +8,6 @@ package frc4388.robot.subsystems; import java.io.File; -import java.io.FilenameFilter; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.util.ArrayList; -import java.util.List; -import java.util.stream.Collectors; -import java.util.stream.Stream; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -26,7 +17,6 @@ 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.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.music.Orchestra; @@ -34,21 +24,17 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.DoubleSolenoid; 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.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; 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 edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Gains; From f7dfe25744ec65e2621f33badf412a48f5c4c235 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:25:37 -0700 Subject: [PATCH 2/9] Cleanup Robot Container --- .../java/frc4388/robot/RobotContainer.java | 131 ++++++++++-------- 1 file changed, 75 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9bd371f..8f0e772 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -102,18 +102,24 @@ public class RobotContainer { * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ - //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); - - // resets the yaw of the pigeon - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36)); - - // turn 45 degrees + /* Test Buttons */ + // A driver test button + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whenPressed(new InstantCommand()); + + // B driver test button + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(new InstantCommand()); + + // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive)); + .whenPressed(new InstantCommand()); + + // 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_robotDrive.setShiftState(false), m_robotDrive)); @@ -123,7 +129,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); /* Operator Buttons */ - //TODO: Shooter Buttons // shoots until released //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) @@ -159,7 +164,62 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) .whileHeld(new storageOutake(m_robotStorage)); } - + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Create config for trajectory + TrajectoryConfig config = getTrajectoryConfig(); + Trajectory trajectory = getTrajectory(config); + RamseteCommand ramseteCommand = getRamseteCommand(trajectory); + // Run path following command, then stop at the end. + //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + + // return new InstantCommand(); + return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + } + + 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); + } + + Trajectory getTrajectory(TrajectoryConfig config) { + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(10, 0) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(20, 20, new Rotation2d(0)), + // Pass config + config); + // 10 = 20, 20 = 35, 30 = 53.5 + // (0,10) = (8,22) + + return exampleTrajectory; + } + + RamseteCommand getRamseteCommand(Trajectory trajectory) { + RamseteCommand ramseteCommand = new RamseteCommand( + trajectory, + m_robotDrive::getPose, + new RamseteController(), + DriveConstants.kDriveKinematics, + m_robotDrive::tankDriveVelocity, + m_robotDrive); + + return ramseteCommand; + } + /** * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to @@ -176,55 +236,14 @@ public class RobotContainer { m_robotDrive.setShiftState(state); } - public void configDriveTrainSensors(FeedbackDevice type) { - m_robotDrive.configMotorSensor(type); - } - + /** + * + */ public void resetOdometry() { m_robotDrive.resetGyroAngles(); m_robotDrive.setOdometry(new Pose2d()); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - - // Create config for trajectory - /*TrajectoryConfig config = 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); - - Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), - // Pass through these two interior waypoints, making an 's' curve path - List.of( - new Translation2d(10, 0) - ), - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(20, 20, new Rotation2d(0)), - // Pass config - config); - // 10 = 20, 20 = 35, 30 = 53.5 - // (0,10) = (8,22) - RamseteCommand ramseteCommand = new RamseteCommand( - exampleTrajectory, - m_robotDrive::getPose, - new RamseteController(), - DriveConstants.kDriveKinematics, - m_robotDrive::tankDriveVelocity, - m_robotDrive); - - // Run path following command, then stop at the end. - return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/ - // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); - } - /** * Used for analog inputs like triggers and axises. * @return IHandController interface for the Driver Controller. From 1f1d5ed158782b3717a041bf78476011c3df358d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:27:53 -0700 Subject: [PATCH 3/9] Group like default commands together to improve readability --- src/main/java/frc4388/robot/RobotContainer.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8f0e772..3f40a5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -83,15 +83,14 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // drives climber with input from triggers on the opperator controller - m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); + // drives climber with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); - + // continually sends updates to the Blinkin LED controller to keep the lights on + m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } From f60e4a8bf9d2c40f11b2ac9bf63136ed42f7a6d0 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:39:18 -0700 Subject: [PATCH 4/9] Reorganise Drive Variables --- .../java/frc4388/robot/subsystems/Drive.java | 52 ++++++++++--------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fc5e310..0eee452 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -42,51 +42,56 @@ import frc4388.robot.Gains; * Add your docs here. */ public class Drive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - + /* Create Motors, Gyros, Solenoids, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); + public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1); + public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); + + /* Drive objects to manage Drive Train */ + public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public final DifferentialDriveOdometry m_odometry; public Orchestra m_orchestra = new Orchestra(); - public double m_rightFrontMotorPos; - - public double m_rightFrontMotorVel; - - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - - SendableChooser m_chooser = new SendableChooser(); + /* 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; - - public final DifferentialDriveOdometry m_odometry; - - public DoubleSolenoid m_speedShift; - public boolean m_isSpeedShiftHigh; - - public DoubleSolenoid m_coolFalcon; - - SendableChooser m_songChooser = new SendableChooser(); + /* Timey Whimey */ + public long m_lastTime; + public long m_deltaTime = 0; public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); - public long m_lastTime, m_deltaTime; //in milliseconds - public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle; + /* 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_lastLeftPosTicks = 0; public double m_lastRightPosTicks = 0; + + public double m_lastAngleYaw = 0; + public double m_currentAngleYaw = 0; + public double m_kinematicsTargetAngle = 0; + + /* Smart Dashboard Objects */ + SendableChooser m_songChooser = new SendableChooser(); + + /* Misc */ + public boolean m_isSpeedShiftHigh; /** * Add your docs here. @@ -101,10 +106,7 @@ public class Drive extends SubsystemBase { resetGyroYaw(); m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), - new Pose2d(0, 0, new Rotation2d()) ); - - m_speedShift = new DoubleSolenoid(7,0,1); - m_coolFalcon = new DoubleSolenoid(7,3,2); + new Pose2d(0, 0, new Rotation2d())); coolFalcon(false); From 5225a98c2c5d4ae496fe0ca6e24b7397a9eb8d06 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:18:17 -0700 Subject: [PATCH 5/9] Cleanup Drive Constructor Also move constants to Drive Constants --- src/main/java/frc4388/robot/Constants.java | 13 ++ .../java/frc4388/robot/subsystems/Drive.java | 152 ++++++++---------- 2 files changed, 79 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 50f0f9c..7e01b54 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,8 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import frc4388.utility.LEDPatterns; @@ -86,6 +88,17 @@ public final class Constants { public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; + + public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); + public static final boolean isRightMotorInverted = false; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; + + public static final int CLOSED_LOOP_TIME_MS = 1; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 0eee452..3976c47 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -8,6 +8,7 @@ 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; @@ -25,7 +26,6 @@ import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.DoubleSolenoid; - import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -35,12 +35,10 @@ 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.robot.Gains; -/** - * Add your docs here. - */ public class Drive extends SubsystemBase { /* Create Motors, Gyros, Solenoids, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); @@ -52,9 +50,9 @@ public class Drive extends SubsystemBase { public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); /* Drive objects to manage Drive Train */ - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public DifferentialDrive m_driveTrain; public final DifferentialDriveOdometry m_odometry; - public Orchestra m_orchestra = new Orchestra(); + public Orchestra m_orchestra; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -82,7 +80,7 @@ public class Drive extends SubsystemBase { public double m_lastLeftPosTicks = 0; public double m_lastRightPosTicks = 0; - + public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; public double m_kinematicsTargetAngle = 0; @@ -104,43 +102,36 @@ public class Drive extends SubsystemBase { m_rightBackMotor.configFactoryDefault(); m_pigeon.configFactoryDefault(); resetGyroYaw(); - - m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), - new Pose2d(0, 0, new Rotation2d())); - - coolFalcon(false); - - /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(false); - //m_driveTrain.setRightSideInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); + 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); - float rampRate = 0.1f; - m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + /* 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); - //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01); - //m_rightFrontMotor.configSupplyCurrentLimit(c); - //m_leftFrontMotor.configSupplyCurrentLimit(c); + /* 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); - /* deadbands */ - //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed - //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed + /* 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 */ - + setRightMotorGains(false); - /* PID for Back Motor control in Auto */ + /* 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_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); @@ -155,7 +146,7 @@ public class Drive extends SubsystemBase { 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); - /* Setup Sensors for WPI_TalonFXs */ + /* Reset Sensors for WPI_TalonFXs */ resetEncoders(); /* Configure the left Talon's selected sensor as local QuadEncoder */ @@ -175,50 +166,43 @@ public class Drive extends SubsystemBase { /* 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] + 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); + /* 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); - /* Setup Sum signal to be used for Distance */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Diff Signal */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - configMotorSensor(FeedbackDevice.SensorDifference); - - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - /* - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient - DriveConstants.PID_PRIMARY, // PID Slot of Source - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - */ - + /* Config Remote1 to be used for Aux PID Index */ m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - //m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - //m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, 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_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 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 @@ -240,37 +224,39 @@ 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. */ - int closedLoopTimeMs = 1; m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + m_rightBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, + DriveConstants.CLOSED_LOOP_TIME_MS, 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(false, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Set up Differential Drive */ + m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - m_lastTime = System.currentTimeMillis(); + /* 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(); + /* 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()); String[] songsStrings = songsDir.list(); @@ -278,6 +264,9 @@ public class Drive extends SubsystemBase { m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString); } Shuffleboard.getTab("Songs").add(m_songChooser); + + /* Start counting time */ + m_lastTime = System.currentTimeMillis(); } String currentSong = ""; @@ -583,15 +572,6 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - /** - * Selects the feedback device for the motors. - * @param feedbackDevice The feedback device to set it to, usually SensorDifference or - */ - public void configMotorSensor(FeedbackDevice type) { - m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); - } - /** * Returns the current yaw of the pigeon */ From a36a7f978ee325cb59b9fd5a5f7196e528e0e225 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:34:38 -0700 Subject: [PATCH 6/9] Organize Drive Periodic --- .../robot/commands/DrivePositionMPAux.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 70 ++++++++++--------- 2 files changed, 39 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 6883468..68d9538 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -63,7 +63,7 @@ public class DrivePositionMPAux extends CommandBase { m_currentPos = m_drive.m_rightFrontMotorPos; if (System.currentTimeMillis() - m_startTime < m_rampRate) { // Ramping - m_targetVel += m_rampAcc * m_drive.m_deltaTime; + m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs; m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); } else if (m_targetPos - m_currentPos > m_rampDist) { // Cruising diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3976c47..8caae27 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -67,9 +67,10 @@ public class Drive extends SubsystemBase { public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; /* Timey Whimey */ - public long m_lastTime; - public long m_deltaTime = 0; - public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); + 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; @@ -78,6 +79,8 @@ public class Drive extends SubsystemBase { 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; @@ -90,6 +93,7 @@ public class Drive extends SubsystemBase { /* Misc */ public boolean m_isSpeedShiftHigh; + String m_currentSong = ""; /** * Add your docs here. @@ -266,14 +270,34 @@ public class Drive extends SubsystemBase { Shuffleboard.getTab("Songs").add(m_songChooser); /* Start counting time */ - m_lastTime = System.currentTimeMillis(); - } + m_lastTimeMs = System.currentTimeMillis(); + } - String currentSong = ""; @Override public void periodic() { - m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); - SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis()); + m_lastTimeMs = m_currentTimeMs; + m_currentTimeMs = System.currentTimeMillis(); + m_currentTimeSec = m_currentTimeMs / 1000; + m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + + m_lastAngleYaw = m_currentAngleYaw; + m_currentAngleYaw = getGyroYaw(); + + 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); + + m_odometry.update(Rotation2d.fromDegrees( getHeading()), + getDistanceInches(m_leftFrontMotor), + -getDistanceInches(m_rightFrontMotor)); if (m_currentTimeSec % 30 == 0) { coolFalcon(true); @@ -282,21 +306,6 @@ public class Drive extends SubsystemBase { coolFalcon(false); SmartDashboard.putBoolean("Solenoid", false); } - - m_deltaTime = System.currentTimeMillis() - m_lastTime; - m_lastTime = System.currentTimeMillis(); - m_lastAngleYaw = m_currentAngleYaw; - m_currentAngleYaw = getGyroYaw(); - - m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); - m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); - - m_totalRightDistanceInches += ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastRightPosTicks); - m_totalLeftDistanceInches += ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastLeftPosTicks); - - m_odometry.update(Rotation2d.fromDegrees( getHeading()), - getDistanceInches(m_rightFrontMotor), - -getDistanceInches(m_rightFrontMotor)); try { //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); @@ -349,18 +358,15 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); //SmartDashboard.putNumber("Delta Time", m_deltaTime); - if (currentSong != m_songChooser.getSelected()){ - currentSong = m_songChooser.getSelected(); - selectSong(currentSong); - System.err.println(currentSong); + 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 in the Drive Subsystem"); + System.err.println("Error while using Drive SmartDashboard"); // e.printStackTrace(System.err); } - - m_lastRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition(); - m_lastLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(); } public void setRightMotorGains(boolean isHighGear) { @@ -639,7 +645,7 @@ public class Drive extends SubsystemBase { */ public double getTurnRate() { double deltaYaw = m_currentAngleYaw - m_lastAngleYaw; - double turnRate = 1000 * deltaYaw / m_deltaTime; + double turnRate = 1000 * deltaYaw / m_deltaTimeMs; return turnRate; } From dc64155c0d402af94af1d0126d1c8d2c7f08fc3c Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:47:19 -0700 Subject: [PATCH 7/9] Cleanup Drive Subsystem Methods --- .../java/frc4388/robot/subsystems/Drive.java | 458 +++++++++--------- 1 file changed, 234 insertions(+), 224 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 8caae27..a0e49ae 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -13,7 +13,6 @@ 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.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; @@ -279,7 +278,6 @@ public class Drive extends SubsystemBase { m_currentTimeMs = System.currentTimeMillis(); m_currentTimeSec = m_currentTimeMs / 1000; m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; - //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); m_lastAngleYaw = m_currentAngleYaw; m_currentAngleYaw = getGyroYaw(); @@ -299,152 +297,8 @@ public class Drive extends SubsystemBase { getDistanceInches(m_leftFrontMotor), -getDistanceInches(m_rightFrontMotor)); - if (m_currentTimeSec % 30 == 0) { - coolFalcon(true); - SmartDashboard.putBoolean("Solenoid", true); - } else if ((m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - SmartDashboard.putBoolean("Solenoid", false); - } - - try { - //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); - SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); - SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); - SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - - //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - - //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("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_deltaTime); - - 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); - } - } - - 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); + runFalconCooling(); + updateSmartDashboard(); } /** @@ -578,6 +432,104 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + m_speedShift.set(DoubleSolenoid.Value.kReverse); + } + if (state == false) { + m_speedShift.set(DoubleSolenoid.Value.kForward); + } + setRightMotorGains(state); + m_isSpeedShiftHigh = state; + } + + /** + * Set to open or close solenoid that cools the falcon, true = open, false = close + * @param state Chooses between open and close + */ + public void coolFalcon(boolean state) { + if (state == true) { + m_coolFalcon.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + } + } + + /** + * + */ + public void runFalconCooling() { + if (m_currentTimeSec % 30 == 0) { + coolFalcon(true); + SmartDashboard.putBoolean("Solenoid", true); + } else if ((m_currentTimeSec - 1) % 30 == 0) { + coolFalcon(false); + SmartDashboard.putBoolean("Solenoid", false); + } + } + + /** + * Selects a song to play! + * @param song The name of the song to be played + */ + 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(); + m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); + } + + /** + * Resets the yaw of the pigeon + */ + public void resetGyroYaw() { + m_pigeon.setYaw(0); + m_pigeon.setAccumZAngle(0); + resetGyroAngles(); + } + + /** + * Add docs here + */ + public void resetGyroAngles() { + m_lastAngleYaw = 0; + m_currentAngleYaw = 0; + m_kinematicsTargetAngle = 0; + } + /** * Returns the current yaw of the pigeon */ @@ -609,27 +561,11 @@ public class Drive extends SubsystemBase { return ypr[2]; } - /** - * Resets the yaw of the pigeon - */ - public void resetGyroYaw() { - m_pigeon.setYaw(0); - m_pigeon.setAccumZAngle(0); - resetGyroAngles(); - } - - /** - * Add docs here - */ - public void resetGyroAngles() { - m_lastAngleYaw = 0; - m_currentAngleYaw = 0; - m_kinematicsTargetAngle = 0; - } //lol //sko //ridge //brayden=bad coder + /** * Returns the heading of the robot * @return The robot's heading in degrees, from -180 to 180 @@ -666,29 +602,6 @@ public class Drive extends SubsystemBase { -getVelocityInchesPerSecond(m_rightBackMotor)); } - /** - * 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(); - m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); - } - /** * Gets the encoder value (position) of a motor * @param falcon The motor to get the position of @@ -750,48 +663,145 @@ public class Drive extends SubsystemBase { public double metersToInches(double meters) { return meters * DriveConstants.INCHES_PER_METER; } - - /* - * Plays Music! - */ - public void playSong() { - m_orchestra.play(); + + 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); + } } /** - * Selects a song to play! - * @param song The name of the song to be played + * Sets Motors to a NeutralMode. + * + * @param mode NeutralMode to set motors to */ - public void selectSong(String song) { - SmartDashboard.putString("Selected Song", song); - m_orchestra.loadMusic(song); - } - /** - * Set to high or low gear based on boolean state, true = high, false = low - * @param state Chooses between high or low gear - */ - public void setShiftState(boolean state) { - if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kReverse); - } - if (state == false) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - setRightMotorGains(state); - m_isSpeedShiftHigh = state; + public void setDriveTrainNeutralMode(NeutralMode mode) { + m_leftFrontMotor.setNeutralMode(mode); + m_rightFrontMotor.setNeutralMode(mode); + m_leftBackMotor.setNeutralMode(mode); + m_rightBackMotor.setNeutralMode(mode); } - /** - * Set to open or close solenoid that cools the falcon, true = open, false = close - * @param state Chooses between open and close - */ - public void coolFalcon(boolean state) { - if (state == true) { - m_coolFalcon.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { - m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + public void updateSmartDashboard() { + try { + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); + SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); + SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + + //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + + //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("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_deltaTime); + + 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); } } - } From 237f35171f9a0a6aca99a4642a6496a97ba1fa62 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:54:10 -0700 Subject: [PATCH 8/9] Cleanup Constants --- src/main/java/frc4388/robot/Constants.java | 24 ++++++++++--------- src/main/java/frc4388/robot/Robot.java | 1 - .../java/frc4388/robot/RobotContainer.java | 1 - 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7e01b54..3678f33 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,19 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; + /* Drive Inversions */ + public static final boolean isRightMotorInverted = false; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; + + /* Drive Configuration */ + public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); + public static final int CLOSED_LOOP_TIME_MS = 1; + /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); @@ -88,17 +101,6 @@ public final class Constants { public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; - - public static final double OPEN_LOOP_RAMP_RATE = 0.1; - public static final double NEUTRAL_DEADBAND = 0.04; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = - new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); - public static final boolean isRightMotorInverted = false; - public static final boolean isLeftMotorInverted = false; - public static final boolean isRightArcadeInverted = false; - public static final boolean isAuxPIDInverted = false; - - public static final int CLOSED_LOOP_TIME_MS = 1; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ec52eb2..1029754 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,7 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3f40a5d..2156bd0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; From 50854ddd14594c012f168238337f50df2523cbba Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:56:57 -0700 Subject: [PATCH 9/9] Remove unused variable --- src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a0e49ae..fca1e7b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -85,7 +85,6 @@ public class Drive extends SubsystemBase { public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; - public double m_kinematicsTargetAngle = 0; /* Smart Dashboard Objects */ SendableChooser m_songChooser = new SendableChooser(); @@ -527,7 +526,6 @@ public class Drive extends SubsystemBase { public void resetGyroAngles() { m_lastAngleYaw = 0; m_currentAngleYaw = 0; - m_kinematicsTargetAngle = 0; } /**