From b741417e0b8f423d630b059201a1b5c543c519d1 Mon Sep 17 00:00:00 2001 From: Elijah Price Date: Tue, 25 Feb 2020 17:12:56 -0700 Subject: [PATCH 1/5] initial testing --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 7 +- .../robot/commands/DriveWithJoystick.java | 6 +- .../java/frc4388/robot/subsystems/Drive.java | 69 ++++++++++++------- 4 files changed, 56 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5ada949..ba82993 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -106,7 +106,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f14c5d9..f46ff66 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,11 +117,14 @@ public class RobotContainer { // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 5387e8d..42aedfc 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = .45; + double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); - } else { + } else if (steerInput < 0) { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } else { + steerOutput = 0; } m_drive.driveWithInput(moveOutput, steerOutput); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 1902e0d..d0e7601 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -26,6 +26,7 @@ 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; @@ -33,6 +34,7 @@ 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; @@ -69,6 +71,7 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -109,21 +112,31 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + //m_leftBackMotor.follow(m_leftFrontMotor); + //m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(true); + m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); + //m_leftBackMotor.setInverted(InvertType.FollowMaster); + //m_rightBackMotor.setInverted(InvertType.FollowMaster); + + 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); + + SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); + m_rightFrontMotor.configSupplyCurrentLimit(c); + m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); /* 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_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 @@ -322,26 +335,36 @@ public class Drive extends SubsystemBase { m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); try { - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //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 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("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 Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent()); + + + 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)); @@ -350,10 +373,10 @@ public class Drive extends SubsystemBase { //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.putString("Odometry Values Meters", getPose().toString()); + //SmartDashboard.putNumber("Odometry Heading", getHeading()); - SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); //SmartDashboard.putNumber("Delta Time", m_deltaTime); if (currentSong != m_songChooser.getSelected()){ @@ -389,8 +412,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); + m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); } /** From 7339658077b70d486beb1edcb2d10d6197660de8 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 19:15:15 -0700 Subject: [PATCH 2/5] Fix watchdog feeds and steer output curving --- .../java/frc4388/robot/commands/DriveWithJoystick.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 42aedfc..fe1c8e5 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -51,9 +51,9 @@ public class DriveWithJoystick extends CommandBase { double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { - steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier; } else { steerOutput = 0; } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d0e7601..9f2639b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -119,8 +119,8 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - //m_leftBackMotor.setInverted(InvertType.FollowMaster); - //m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); float rampRate = 0.1f; m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); @@ -128,9 +128,9 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); - m_rightFrontMotor.configSupplyCurrentLimit(c); - m_leftFrontMotor.configSupplyCurrentLimit(c); + //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01); + //m_rightFrontMotor.configSupplyCurrentLimit(c); + //m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); From ab6cc48c461052da84743ffeefa1871695ee52bb Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:45:29 -0700 Subject: [PATCH 3/5] Reduce deadband for greater range of motion --- src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index fe1c8e5..f51621a 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -49,7 +49,7 @@ public class DriveWithJoystick extends CommandBase { } double cosMultiplier = .55; - double deadzone = .2; + double deadzone = .1; if (steerInput > 0){ steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { From f7dc572a4463d88d58f47f5ebf5358dd83b87953 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:52:20 -0700 Subject: [PATCH 4/5] Minor Cleanup Removed set up of Neutral Mode in the constructor because Neutral Mode is controlled in Robot.java --- src/main/java/frc4388/robot/Robot.java | 1 - src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ba82993..feab056 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,7 +64,6 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); - //m_robotContainer.setDriveGearState(true); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 9f2639b..74b9ef9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -132,8 +132,6 @@ public class Drive extends SubsystemBase { //m_rightFrontMotor.configSupplyCurrentLimit(c); //m_leftFrontMotor.configSupplyCurrentLimit(c); - setDriveTrainNeutralMode(NeutralMode.Coast); - /* 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 From 3e829b4db8e2b13ce0cd94bda172ed84b1b7829b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 21:12:02 -0700 Subject: [PATCH 5/5] Remove extraneous code --- src/main/java/frc4388/robot/RobotContainer.java | 6 +----- src/main/java/frc4388/robot/subsystems/Drive.java | 9 ++++----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f46ff66..f57b496 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -123,13 +123,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); - - /* Operator Buttons */ - //TODO: Shooter Buttons + //TODO: Shooter Buttons // shoots until released //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 74b9ef9..0dae7c8 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -71,7 +71,6 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -112,8 +111,8 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - //m_leftBackMotor.follow(m_leftFrontMotor); - //m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); @@ -410,8 +409,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); - m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); } /**