Skip to content

Commit fec5163

Browse files
ShmayaRShmayaR
committed
dang bro that's 11 files changed. not what you want to see after first qual :hurtrealbad: 
1 parent ff18479 commit fec5163

File tree

11 files changed

+37
-16
lines changed

11 files changed

+37
-16
lines changed

‎src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,10 @@ public class IntakeAssistCommand extends ParallelCommandGroup {
2323
static final ProfiledPIDController
2424
X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
2525
new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
26-
new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
26+
new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
2727
Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
2828
new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
29-
new ProfiledPIDController(0.2, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)),
29+
new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)),
3030
THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
3131
new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
3232
new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));

‎src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() {
4949
return new SequentialCommandGroup(
5050
GeneralCommands.getResetFlipArmOverrideCommand(),
5151
CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
52-
getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))),
52+
getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.15))),
5353
new InstantCommand(() -> {
5454
IS_HOLDING_ALGAE = true;
5555
SHOULD_COLLECT_FROM_LOLLIPOP = false;
@@ -64,7 +64,7 @@ public static Command getReefAlgaeCollectionCommand() {
6464
return new SequentialCommandGroup(
6565
GeneralCommands.getResetFlipArmOverrideCommand(),
6666
CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
67-
getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece),
67+
getInitiateReefAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))),
6868
new InstantCommand(() -> IS_HOLDING_ALGAE = true),
6969
GeneralCommands.getResetFlipArmOverrideCommand(),
7070
getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand())

‎src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -153,11 +153,11 @@ public static boolean isPrepareArmAngleAboveCurrentArmAngle() {
153153
final Rotation2d targetAngle = targetState.prepareState == null
154154
? targetState.targetAngle
155155
: targetState.prepareState.targetAngle;
156-
return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle);
156+
return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle);
157157
}
158158

159159
private static boolean isReadyToScore(boolean shouldScoreRight) {
160-
return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore())
160+
return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore(), 4)
161161
&& RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight));
162162
}
163163

‎src/main/java/frc/trigon/robot/constants/AutonomousConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,8 @@
2727
public class AutonomousConstants {
2828
public static final String DEFAULT_AUTO_NAME = "DefaultAutoName";
2929
public static final RobotConfig ROBOT_CONFIG = getRobotConfig();
30-
public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate
31-
public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900));
30+
public static final double FEEDFORWARD_SCALAR = 0.6;//TODO: Calibrate
31+
public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900));
3232
public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2;
3333
public static final double
3434
REEF_RELATIVE_X_TOLERANCE_METERS = 0.085,

‎src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,13 +143,25 @@ public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolea
143143
&& elevatorAtPosition(targetState.targetPositionMeters);
144144
}
145145

146+
public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed, double tol) {
147+
if (targetState == null)
148+
return false;
149+
return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle, tol)
150+
&& elevatorAtPosition(targetState.targetPositionMeters);
151+
}
152+
146153
public boolean armAtAngle(Rotation2d targetAngle) {
147154
final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees());
148155
return currentToTargetAngleDifferenceDegrees < ArmElevatorConstants.ANGLE_TOLERANCE.getDegrees();
149156
}
150157

158+
public boolean armAtAngle(Rotation2d targetAngle, double tol) {
159+
final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees());
160+
return currentToTargetAngleDifferenceDegrees < tol;
161+
}
162+
151163
public boolean armAboveAngle(Rotation2d targetAngle) {
152-
return targetAngle.getDegrees() < getCurrentArmAngle().getDegrees();
164+
return targetAngle.getDegrees() - getCurrentArmAngle().getDegrees() < 4;
153165
}
154166

155167
public boolean elevatorAtPosition(double positionMeters) {
@@ -201,7 +213,7 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i
201213

202214
void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) {
203215
scaleArmPositionRequestSpeed(targetState.speedScalar);
204-
if (isStateReversed) {
216+
if (isStateReversed && !(targetState == ArmElevatorConstants.ArmElevatorState.SCORE_L1 || targetState == ArmElevatorConstants.ArmElevatorState.PREPARE_SCORE_L1)) {
205217
setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints);
206218
return;
207219
}

‎src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ public static Command resetElevatorPositionCommand() {
4545
RobotContainer.ARM_ELEVATOR
4646
).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)),
4747
new ExecuteEndCommand(
48-
() -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2),
48+
() -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
4949
() -> {},
5050
RobotContainer.ARM_ELEVATOR
5151
)).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition);

‎src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ public static Command getDebuggingCommand() {
2424

2525
public static Command resetClimberPositionCommand() {
2626
return new ExecuteEndCommand(
27-
() -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 3),
27+
() -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3),
2828
RobotContainer.CLIMBER::resetClimberPosition,
2929
RobotContainer.CLIMBER
3030
).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0));

‎src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.trigon.robot.subsystems.endeffector;
22

3+
import com.ctre.phoenix6.controls.TorqueCurrentFOC;
34
import com.ctre.phoenix6.controls.VoltageOut;
45
import edu.wpi.first.math.geometry.Translation3d;
56
import frc.trigon.robot.RobotContainer;
@@ -61,13 +62,21 @@ public boolean isEjecting() {
6162
}
6263

6364
void setTargetState(EndEffectorConstants.EndEffectorState targetState) {
64-
setEndEffectorTargetVoltage(targetState.targetVoltage);
65+
if (targetState == EndEffectorConstants.EndEffectorState.HOLD_ALGAE) {
66+
setCur(targetState.targetVoltage);
67+
} else {
68+
setEndEffectorTargetVoltage(targetState.targetVoltage);
69+
}
6570
}
6671

6772
void setTargetState(double targetVoltage) {
6873
setEndEffectorTargetVoltage(targetVoltage);
6974
}
7075

76+
private void setCur(double cur) {
77+
endEffectorMotor.setControl(new TorqueCurrentFOC(cur).withMaxAbsDutyCycle(0.6));
78+
}
79+
7180
private void setEndEffectorTargetVoltage(double targetVoltage) {
7281
EndEffectorConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage);
7382
endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));

‎src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ public enum EndEffectorState {
9898
COLLECT_ALGAE(-11),
9999
SCORE_ALGAE(10),
100100
HOLD_CORAL(-0.5),
101-
HOLD_ALGAE(-7.5);
101+
HOLD_ALGAE(-50);
102102

103103
public final double targetVoltage;
104104

‎src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ public static Command getDebuggingCommand() {
2626

2727
public static Command resetIntakePositionCommand() {
2828
return new ExecuteEndCommand(
29-
() -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2),
29+
() -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
3030
RobotContainer.INTAKE::resetIntakePosition,
3131
RobotContainer.INTAKE
3232
).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0));

0 commit comments

Comments
 (0)