Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

qawsedrftgyhujikolp

Browse Source
This commit is contained in:
aarav18
2022-03-18 23:07:53 -06:00
parent e1ef2f8aff
commit 46396a704f
5 changed files with 63 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -9
View File
@@ -36,7 +36,7 @@ public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048; public static final double TICKS_PER_ROTATION_FX = 2048;
public static final class SwerveDriveConstants { 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 WIDTH = 23.5;
public static final double HEIGHT = 23.5; public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -203,18 +203,18 @@ public final class Constants {
// PID Constants // PID Constants
public static final int SHOULDER_POSITION_SLOT_IDX = 0; public static final int SHOULDER_POSITION_SLOT_IDX = 0;
public static final int SHOULDER_VELOCITY_SLOT_IDX = 1; public static final int SHOULDER_VELOCITY_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 1; public static final int SHOULDER_PID_LOOP_IDX = 0;
public static final int ELBOW_POSITION_SLOT_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_VELOCITY_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 1; 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 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(1.0, 0.0, 0.1, 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 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(1.0, 0.0, 0.1, 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; public static final int CLIMBER_TIMEOUT_MS = 50;
src/main/java/frc4388/robot/Robot.java
+9 -9
View File
@@ -143,15 +143,15 @@ public class Robot extends TimedRobot {
// SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition());
// SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition());
current = current =
m_robotContainer.m_robotBoomBoom.getCurrent() + // m_robotContainer.m_robotBoomBoom.getCurrent() +
m_robotContainer.m_robotClimber.getCurrent() + m_robotContainer.m_robotClimber.getCurrent(); //+
m_robotContainer.m_robotHood.getCurrent() + // m_robotContainer.m_robotHood.getCurrent() +
m_robotContainer.m_robotIntake.getCurrent() + // m_robotContainer.m_robotIntake.getCurrent() +
m_robotContainer.m_robotExtender.getCurrent() + // m_robotContainer.m_robotExtender.getCurrent() +
m_robotContainer.m_robotSerializer.getCurrent() + // m_robotContainer.m_robotSerializer.getCurrent() +
m_robotContainer.m_robotStorage.getCurrent() + // m_robotContainer.m_robotStorage.getCurrent() +
m_robotContainer.m_robotSwerveDrive.getCurrent(); // m_robotContainer.m_robotSwerveDrive.getCurrent();
m_robotContainer.m_robotTurret.getCurrent(); // m_robotContainer.m_robotTurret.getCurrent();
SmartDashboard.putNumber("Total Robot Current Draw", current); SmartDashboard.putNumber("Total Robot Current Draw", current);
SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
src/main/java/frc4388/robot/RobotContainer.java
+7 -7
View File
@@ -173,8 +173,8 @@ public class RobotContainer {
// IK command // IK command
m_robotClimber.setDefaultCommand( m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input // Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand( m_robotSwerveDrive.setDefaultCommand(
@@ -226,11 +226,11 @@ public class RobotContainer {
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
// }, m_robotHood)); // }, m_robotHood));
m_robotClimber.setDefaultCommand( // m_robotClimber.setDefaultCommand(
// new RunCommand(() -> { // // new RunCommand(() -> {
// if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } // // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
/*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), // /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(),
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} // getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //}
// }, m_robotClimber)); // }, m_robotClimber));
// m_robotTurret.setDefaultCommand( // m_robotTurret.setDefaultCommand(
src/main/java/frc4388/robot/subsystems/Climber.java
+30 -24
View File
@@ -54,8 +54,9 @@ public class Climber extends SubsystemBase {
m_elbow.configFactoryDefault(); m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake); m_elbow.setNeutralMode(NeutralMode.Brake);
setClimberPositionGains(); // setClimberPositionGains();
setClimberVelocityGains(); setClimberVelocityGains();
useVelocityGains();
tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
@@ -90,24 +91,24 @@ public class Climber extends SubsystemBase {
this.elbowSpeedLimiter = 1.0; this.elbowSpeedLimiter = 1.0;
} }
public void setClimberPositionGains() { // public void setClimberPositionGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); // 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_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_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_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_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.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_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_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_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.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
} // }
public void usePositionGains() { // public void usePositionGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); // 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); // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
} // }
public void setClimberVelocityGains() { public void setClimberVelocityGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); 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; // // shoulderPosition += m_shoulderOffset;
// // elbowPosition += m_elbowOffset; // // elbowPosition += m_elbowOffset;
m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); // m_shoulder.set(TalonFXControlMode.Position, shoulderAngle);
m_elbow.set(TalonFXControlMode.Position, elbowAngle); // m_elbow.set(TalonFXControlMode.Position, elbowAngle);
} }
public void setJointSpeeds(double[] speeds) { public void setJointSpeeds(double[] speeds) {
@@ -212,15 +213,18 @@ public class Climber extends SubsystemBase {
} }
boolean movingPrev = false; boolean movingPrev = false;
boolean moving;
public void controlWithInput(double xInput, double yInput) { public void controlWithInput(double xInput, double yInput) {
boolean moving = xInput != 0 && yInput != 0; moving = xInput != 0 || yInput != 0;
if(movingPrev != moving) { if(movingPrev != moving) {
if(moving) { if(moving) {
useVelocityGains(); useVelocityGains();
SmartDashboard.putString("Climber Gains", "Velocity");
} else { } else {
usePositionGains(); // usePositionGains();
tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; 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; int pCount = 0;
@Override @Override
public void periodic() { public void periodic() {
SmartDashboard.putBoolean("Moving", moving);
SmartDashboard.putBoolean("MovingPrev", movingPrev);
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
if(pCount % 1 == 0) // if(pCount % 1 == 0)
setJointAngles(tJointAngles); // 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). // * 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(); double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8 -4
View File
@@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units; 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.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -116,7 +117,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d speed = new Translation2d(leftX, leftY); Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust); speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) 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 rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = speed.getX(); double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY(); double ySpeedMetersPerSecond = speed.getY();
@@ -199,9 +200,12 @@ public class SwerveDrive extends SubsystemBase {
* *
* @return Rotation2d object holding current gyro in radians * @return Rotation2d object holding current gyro in radians
*/ */
public Rotation2d getRegGyro() { public Rotation2d getRegGyro() {
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; // * test chassis regression
return new Rotation2d(regCur * Math.PI / 180); // 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));
} }
/** /**