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

Monday Fixes

Browse Source
This commit is contained in:
Ryan Manley
2022-03-08 16:14:34 -07:00
parent 931c0cbd12
commit dce8ab320c
6 changed files with 90 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+22 -22
View File
@@ -31,7 +31,7 @@ import frc4388.utility.LEDPatterns;
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4;
public static final double ROTATION_SPEED = 0.5;
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;
@@ -40,18 +40,18 @@ public final class Constants {
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int LEFT_FRONT_STEER_CAN_ID = 2; //
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; //
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; //
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; //
public static final int LEFT_BACK_STEER_CAN_ID = 6; //
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; //
public static final int RIGHT_BACK_STEER_CAN_ID = 8; //
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; //
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; //
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; //
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;//
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; //
public static final int GYRO_ID = 14;
// offsets are in degrees
@@ -65,10 +65,10 @@ public final class Constants {
// public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;//
// 180-2.021484375;//0.0;//134.384765625
public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.;
public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90 ) % 360.;
public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.;
public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 90 - 180) % 360.;
public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 ) % 360.;
public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 ) % 360.;
public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50) % 360.;
public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180) % 360.;
// swerve PID constants
public static final int SWERVE_SLOT_IDX = 0;
@@ -176,10 +176,10 @@ public final class Constants {
// ID
public static final int TURRET_MOTOR_CAN_ID = 19;
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double TURRET_FORWARD_LIMIT = 61.7; // TODO: find
public static final double TURRET_REVERSE_LIMIT = -42.3; // TODO: find
public static final double TURRET_FORWARD_LIMIT = 55.0; // TODO: find
public static final double TURRET_REVERSE_LIMIT = -55.0; // TODO: find
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
@@ -187,8 +187,8 @@ public final class Constants {
public static final int SHOOTER_ANGLE_ADJUST_ID = 20;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find
public static final double HOOD_FORWARD_LIMIT = 48.69; // TODO: find
public static final double HOOD_REVERSE_LIMIT = -100; // TODO: find
public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find
public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find
}
src/main/java/frc4388/robot/RobotContainer.java
+36 -19
View File
@@ -133,8 +133,8 @@ public class RobotContainer {
getDriverController().getLeftY(),
//getDriverController().getRightX(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
//getDriverController().getRightY(),
false),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
m_robotIntake.setDefaultCommand(
@@ -147,9 +147,9 @@ public class RobotContainer {
new RunCommand(() -> m_robotStorage.manageStorage(),
m_robotStorage).withName("Storage manageStorage defaultCommand"));*/
// Serializer Management
// m_robotSerializer.setDefaultCommand(
// new RunCommand(() -> m_robotSerializer.setSerializer(0.8),//m_robotSerializer.setSerializerStateWithBeam(),
// m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
m_robotSerializer.setDefaultCommand(
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Turret Manual
m_robotTurret.setDefaultCommand(
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
@@ -194,11 +194,11 @@ public class RobotContainer {
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
.whenPressed(() -> m_robotMap.leftFront.reset())
.whenPressed(() -> m_robotMap.rightFront.reset())
.whenPressed(() -> m_robotMap.leftBack.reset())
.whenPressed(() -> m_robotMap.rightBack.reset());
// new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
// .whenPressed(() -> m_robotMap.leftFront.reset())
// .whenPressed(() -> m_robotMap.rightFront.reset())
// .whenPressed(() -> m_robotMap.leftBack.reset())
// .whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */
@@ -209,22 +209,39 @@ public class RobotContainer {
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(() -> m_robotIntake.runExtender(false));*/
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft
// .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0)));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0)));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.3)))
// .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025));
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425)))
.whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475)))
.whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0)));
// .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5)))
.whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))).whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.75), m_robotStorage))
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.75), m_robotStorage))
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+9 -1
View File
@@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
@@ -31,6 +32,7 @@ public class BoomBoom extends SubsystemBase {
public WPI_TalonFX m_shooterFalconRight;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static BoomBoom m_boomBoom;
double speed2;
double velP;
double input;
@@ -175,7 +177,8 @@ public class BoomBoom extends SubsystemBase {
* @param speed percent output form -1.0 to 1.0
*/
public void runDrumShooter(double speed) {
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
}
@@ -199,4 +202,9 @@ public class BoomBoom extends SubsystemBase {
// feedforward.calculate(targetVel));
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
public void increaseSpeed(double amount)
{
speed2 = speed2 + amount;
}
}
src/main/java/frc4388/robot/subsystems/Hood.java
+6 -6
View File
@@ -20,8 +20,8 @@ public class Hood extends SubsystemBase {
public BoomBoom m_shooterSubsystem;
public CANSparkMax m_angleAdjusterMotor;
public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
// public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
// public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
public static Gains m_angleAdjusterGains;
public RelativeEncoder m_angleEncoder;
@@ -41,10 +41,10 @@ public double m_fireAngle;
m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodUpLimitSwitch.enableLimitSwitch(true);
m_hoodDownLimitSwitch.enableLimitSwitch(true);
// m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_hoodUpLimitSwitch.enableLimitSwitch(true);
// m_hoodDownLimitSwitch.enableLimitSwitch(true);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -10
View File
@@ -38,13 +38,13 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Units.inchesToMeters(-halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
@@ -108,7 +108,7 @@ public class SwerveDrive extends SubsystemBase {
ignoreAngles = true;
else
ignoreAngles = false;
Translation2d speed = new Translation2d(-speedX, speedY);
Translation2d speed = new Translation2d(-speedY, -speedX);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
@@ -116,9 +116,9 @@ public class SwerveDrive extends SubsystemBase {
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED);
-rot * SwerveDriveConstants.ROTATION_SPEED * 8);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
@@ -128,17 +128,19 @@ 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).minus(new Rotation2d(0, 1));
rotTarget = new Rotation2d(rightX, -rightY).plus(new Rotation2d(0, 1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees()))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
}
/**
@@ -149,6 +151,7 @@ public class SwerveDrive extends SubsystemBase {
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
// int i = 2; {
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+1
View File
@@ -120,6 +120,7 @@ public class SwerveModule extends SubsystemBase {
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}