Skip to content

Commit

Permalink
test tele fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
mwitcpalek committed Apr 10, 2024
1 parent 6b7b41b commit 32ecc5e
Show file tree
Hide file tree
Showing 8 changed files with 41 additions and 32 deletions.
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
import frc.robot.commands.robotState.FeedCommand;
import frc.robot.commands.robotState.FullTrapClimbCommand;
import frc.robot.commands.robotState.IntakeCommand;
import frc.robot.commands.robotState.MovingVisionShootCommand;
import frc.robot.commands.robotState.OperatorRumbleCommand;
import frc.robot.commands.robotState.PodiumCommand;
import frc.robot.commands.robotState.PositionShootCommand;
Expand All @@ -76,7 +77,6 @@
import frc.robot.commands.robotState.TuningOffCommand;
import frc.robot.commands.robotState.TuningShootCommand;
import frc.robot.commands.robotState.UpdateElbowOffsetCommand;
import frc.robot.commands.robotState.VisionShootCommand;
import frc.robot.commands.wrist.ClosedLoopWristCommand;
import frc.robot.commands.wrist.OpenLoopWristCommand;
import frc.robot.commands.wrist.WriteWristToStowCommand;
Expand Down Expand Up @@ -860,7 +860,7 @@ private void configureOperatorBindings() {
new PodiumCommand(
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));

// SubWoofer
// Speed Up Pass
new JoystickButton(xboxController, XboxController.Button.kX.value)
.onTrue(new SpeedUpPassCommand(robotStateSubsystem, superStructure));

Expand Down Expand Up @@ -978,7 +978,7 @@ private void configureDriverBindings() {
// Vision Shoot
new JoystickButton(driveJoystick, Button.M_SWH.id)
.onTrue(
new VisionShootCommand(
new MovingVisionShootCommand(
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));

// Release Game Piece Command
Expand Down
33 changes: 16 additions & 17 deletions src/main/java/frc/robot/commands/auton/DriveDownLineCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,29 +8,28 @@
import frc.robot.subsystems.vision.DeadEyeSubsystem;

public class DriveDownLineCommand extends Command {
private DeadEyeSubsystem deadeye;
private DriveSubsystem driveSubsystem;
private ProfiledPIDController deadeyeYDrive;
public DriveDownLineCommand(DeadEyeSubsystem deadeye, DriveSubsystem driveSubsystem) {
addRequirements(driveSubsystem);
this.driveSubsystem = driveSubsystem;
this.deadeye = deadeye;
private DeadEyeSubsystem deadeye;
private DriveSubsystem driveSubsystem;
private ProfiledPIDController deadeyeYDrive;

public DriveDownLineCommand(DeadEyeSubsystem deadeye, DriveSubsystem driveSubsystem) {
addRequirements(driveSubsystem);
this.driveSubsystem = driveSubsystem;
this.deadeye = deadeye;

deadeyeYDrive =
new ProfiledPIDController(
AutonConstants.kPDeadEyeYDrive,
AutonConstants.kIDeadEyeYDrive,
AutonConstants.kDDeadEyeYDrive,
new Constraints(
AutonConstants.kMaxVelDeadeyeDrive, AutonConstants.kMaxAccelDeadeyeDrive));
}
}

@Override
public void initialize() {
double ySpeed = deadeyeYDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
driveSubsystem.recordYVel(ySpeed);
driveSubsystem.move(AutonConstants.kForwardVel, ySpeed, 0.0, false);
}

@Override
public void initialize() {
double ySpeed = deadeyeYDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
driveSubsystem.recordYVel(ySpeed);
driveSubsystem.move(AutonConstants.kForwardVel, ySpeed, 0.0, false);
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/DriveConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,14 @@ public final class DriveConstants {

public static final double kSpeedStillFeedThreshold = 0.5;
public static final double kSpeedStillThreshold = 0.1; // meters per second
public static final double kGyroRateStillThreshold = 5.0; // degrees per second
public static final double kGyroRateStillThreshold = 25.0; // 5 degrees per second
public static final double kDegreesCloseEnough = 3;

// Move and shoot thresholds
public static final double kMaxStableAccel = 0.1;
public static final int kVelocityStableCounts = 5;
public static final double kMaxMoveShootVelocity = 2.0;
public static final double kMaxMoveGyroRateThreshold = 10.0;
public static final double kMaxMoveGyroRateThreshold = 25.0; // 10
public static final double kMoveShootVelDetune = 0.2;
public static final double kMoveShootTeleMaxVelX = 0.5;
public static final double kMaxSpeakerDist = 8.0;
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/ElbowConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -73,9 +73,9 @@ public static TalonFXConfiguration getFxConfiguration() {
config.HardwareLimitSwitch.ForwardLimitEnable = false;

config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.14691; // 0.151
config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.3; // 0.14691
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -0.26409; // -0.26
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -0.001; // -0.26409

config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
config.Feedback.FeedbackRemoteSensorID = ElbowConstants.kHighResCANcoderID;
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/constants/MagazineConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,17 @@ public final class MagazineConstants {
public static final double kShootCloseEnough = 10;
// public static final double kFeedingSpeed = 0.5;
public static final int kMinBeamBreaks = 0; // 3
public static final double kIntakingSpeed = -65; // -46
public static final double kFastIntakingSpeed = -65; // -90
public static final double kIntakingSpeed = -50; // -46
public static final double kFastIntakingSpeed = -50; // -90
public static final double kEmptyingSpeed = -70; // -90
public static final double kReversingSpeed = 4.8; // TODO do testing to determine correct speed
public static final double kReversingSpeed = 9.6; // 4.8

public static final double kReleaseTime = 0.75;
public static final double kPodiumPrepareSpeed = -10;
public static final double kPodiumPrepareSpeed = -20; // -10
public static final double kPodiumShootSpeed = 105; // 90
public static final double kTrapReleaseSpeed = -20.0; // -30.0
public static final double kTrapReleaseSpeed = -40.0; // -20.0
public static final double kTrapReleaseTime = 0.75;
public static final double kAmpReleaseSpeed = 20.0;
public static final double kAmpReleaseSpeed = 40.0; // 20.0
public static final double kPodiumRumbleSpeed = 80;

public static final TalonFXConfiguration getMagazineConfig() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/constants/RobotConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public static class CompConstants {
public static final double kElbowSetpointOffset = 0.00378; // 0.00488

// Wrist
public static final double kWristZero = 2243.0; // 2223
public static final double kWristZero = 2287.0; // 2243

// Climb
public static final double kLeftTrapBarExtend = 0.5;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -582,6 +582,8 @@ public void periodic() {
ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed();
superStructure.fixedFeeding(
FastMath.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond));
} else {
superStructure.stopShoot();
}

switch (curState) {
Expand All @@ -607,6 +609,10 @@ public void periodic() {
if (magazineHasNote()
&& driveSubsystem.getDistanceToSpeaker() < RobotStateConstants.kLookupMaxDistance) {
superStructure.spinUp();
} else if (speedUpPass) {
ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed();
superStructure.fixedFeeding(
FastMath.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond));
} else {
superStructure.stopShoot();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -321,7 +321,11 @@ public void fixedFeeding(double wheelSpeed) {
shooterSubsystem.setLeftSpeed(leftShooterSpeed);
shooterSubsystem.setRightSpeed(rightShooterSpeed);
wristSubsystem.setPosition(wristSetpoint);
elbowSubsystem.setPosition(SuperStructureConstants.kElbowMinToMoveWrist);
if (elbowSubsystem.getPosition() < SuperStructureConstants.kElbowMinToMoveWrist) {
elbowSubsystem.setPosition(elbowSetpoint);
} else {
elbowSubsystem.setPosition(SuperStructureConstants.kElbowMinToMoveWrist);
}

logger.info("{} -> TRANSFER(FEEDING)", curState);
isPrecise = false;
Expand Down

0 comments on commit 32ecc5e

Please sign in to comment.