From 46396a704f42183ee09bfa83fcd3b0348cd35240 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:07:53 -0600 Subject: [PATCH] qawsedrftgyhujikolp --- src/main/java/frc4388/robot/Constants.java | 18 +++---- src/main/java/frc4388/robot/Robot.java | 18 +++---- .../java/frc4388/robot/RobotContainer.java | 14 ++--- .../frc4388/robot/subsystems/Climber.java | 54 ++++++++++--------- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++-- 5 files changed, 63 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 76fdf67..1b73301 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -36,7 +36,7 @@ public final class Constants { public static final double TICKS_PER_ROTATION_FX = 2048; public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4.0; + public static final double ROTATION_SPEED = 3.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -203,18 +203,18 @@ public final class Constants { // PID Constants public static final int SHOULDER_POSITION_SLOT_IDX = 0; - public static final int SHOULDER_VELOCITY_SLOT_IDX = 1; - public static final int SHOULDER_PID_LOOP_IDX = 1; + public static final int SHOULDER_VELOCITY_SLOT_IDX = 0; + public static final int SHOULDER_PID_LOOP_IDX = 0; public static final int ELBOW_POSITION_SLOT_IDX = 0; - public static final int ELBOW_VELOCITY_SLOT_IDX = 1; - public static final int ELBOW_PID_LOOP_IDX = 1; + public static final int ELBOW_VELOCITY_SLOT_IDX = 0; + public static final int ELBOW_PID_LOOP_IDX = 0; - public static final Gains SHOULDER_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); - public static final Gains ELBOW_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains SHOULDER_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); + public static final Gains ELBOW_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); - public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); - public static final Gains ELBOW_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(0.2, 0.0, 5.0, 0.0, 0, 0.2); + public static final Gains ELBOW_VELOCITY_GAINS = new Gains(0.3, 0.005, 5.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 50; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index f01d45a..a0d5e5c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -143,15 +143,15 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - m_robotContainer.m_robotBoomBoom.getCurrent() + - m_robotContainer.m_robotClimber.getCurrent() + - m_robotContainer.m_robotHood.getCurrent() + - m_robotContainer.m_robotIntake.getCurrent() + - m_robotContainer.m_robotExtender.getCurrent() + - m_robotContainer.m_robotSerializer.getCurrent() + - m_robotContainer.m_robotStorage.getCurrent() + - m_robotContainer.m_robotSwerveDrive.getCurrent(); - m_robotContainer.m_robotTurret.getCurrent(); + // m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotClimber.getCurrent(); //+ + // m_robotContainer.m_robotHood.getCurrent() + + // m_robotContainer.m_robotIntake.getCurrent() + + // m_robotContainer.m_robotExtender.getCurrent() + + // m_robotContainer.m_robotSerializer.getCurrent() + + // m_robotContainer.m_robotStorage.getCurrent() + + // m_robotContainer.m_robotSwerveDrive.getCurrent(); + // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d3cc08..3273411 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -173,8 +173,8 @@ public class RobotContainer { // IK command m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), - getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -226,11 +226,11 @@ public class RobotContainer { // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } // }, m_robotHood)); - m_robotClimber.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), - getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} + // m_robotClimber.setDefaultCommand( + // // new RunCommand(() -> { + // // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + // /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), + // getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} // }, m_robotClimber)); // m_robotTurret.setDefaultCommand( diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d9b57f0..15d5a90 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -54,8 +54,9 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberPositionGains(); + // setClimberPositionGains(); setClimberVelocityGains(); + useVelocityGains(); tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; @@ -90,24 +91,24 @@ public class Climber extends SubsystemBase { this.elbowSpeedLimiter = 1.0; } - public void setClimberPositionGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + // public void setClimberPositionGains() { + // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + // m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } + // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + // m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + // } - public void usePositionGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - } + // public void usePositionGains() { + // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + // } public void setClimberVelocityGains() { m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); @@ -198,8 +199,8 @@ public class Climber extends SubsystemBase { // // shoulderPosition += m_shoulderOffset; // // elbowPosition += m_elbowOffset; - m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); - m_elbow.set(TalonFXControlMode.Position, elbowAngle); + // m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); + // m_elbow.set(TalonFXControlMode.Position, elbowAngle); } public void setJointSpeeds(double[] speeds) { @@ -212,15 +213,18 @@ public class Climber extends SubsystemBase { } boolean movingPrev = false; + boolean moving; public void controlWithInput(double xInput, double yInput) { - boolean moving = xInput != 0 && yInput != 0; + moving = xInput != 0 || yInput != 0; if(movingPrev != moving) { if(moving) { useVelocityGains(); + SmartDashboard.putString("Climber Gains", "Velocity"); } else { - usePositionGains(); + // usePositionGains(); tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + SmartDashboard.putString("Climber Gains", "Position"); } } @@ -242,13 +246,15 @@ public class Climber extends SubsystemBase { int pCount = 0; @Override public void periodic() { + SmartDashboard.putBoolean("Moving", moving); + SmartDashboard.putBoolean("MovingPrev", movingPrev); SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - if(pCount % 1 == 0) - setJointAngles(tJointAngles); + // if(pCount % 1 == 0) + // setJointAngles(tJointAngles); - pCount ++; + // pCount ++; // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 555ef83..24727e7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -116,7 +117,7 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = new Translation2d(leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, rightY); + rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); @@ -199,9 +200,12 @@ public class SwerveDrive extends SubsystemBase { * * @return Rotation2d object holding current gyro in radians */ - public Rotation2d getRegGyro() { - double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; - return new Rotation2d(regCur * Math.PI / 180); + public Rotation2d getRegGyro() { + // * test chassis regression + // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + // * new robot regression + double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743; + return new Rotation2d(Math.toRadians(regCur)); } /**