Skip to content

Commit 830af9a

Browse files
Eddie-EdwardEddie-Edward
committed
roggle stuff idfk
1 parent 7747fe5 commit 830af9a

File tree

5 files changed

+153
-23
lines changed

5 files changed

+153
-23
lines changed

‎src/main/java/frc/robot/commands/RobotCommands.java‎

Lines changed: 28 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -24,17 +24,38 @@ public static Command coralPrime(RushinatorPivot.State armState, ElevatorSubsyst
2424
}
2525

2626
public static Command toggleWristState() {
27-
if(RushinatorPivot.kLastState == RushinatorPivot.State.kScore) {
28-
RushinatorWrist.State toggledWristState = (RushinatorWrist.kLastState == State.kScoreLeftWrist) ? State.kScoreRightWrist : State.kScoreLeftWrist;
29-
return coralPrime(RushinatorPivot.kLastState, ElevatorSubsystem.kLastState, toggledWristState);
27+
System.out.println("Last State in Toggle Wrist" + RushinatorWrist.kLastState.name());
28+
if(RushinatorWrist.kLastState == State.kScoreLeftWrist) {
29+
System.out.println("First If");
30+
return new SetWristState(RushinatorWrist.State.kScoreRightWrist);
3031
}
31-
else if(RushinatorPivot.kLastState == RushinatorPivot.State.kStowTravel) {
32-
RushinatorWrist.State toggledWristState = (RushinatorWrist.kLastState == State.kTravelLeft) ? State.kTravelRight : State.kTravelLeft;
33-
return coralPrime(RushinatorPivot.kLastState, ElevatorSubsystem.kLastState, toggledWristState);
32+
else if(RushinatorWrist.kLastState == State.kScoreRightWrist) {
33+
System.out.println("2nd If");
34+
return new SetWristState(RushinatorWrist.State.kScoreLeftWrist);
35+
}
36+
else if(RushinatorWrist.kLastState == State.kTravelLeft) {
37+
System.out.println("3rd If");
38+
return new SetWristState(RushinatorWrist.State.kTravelRight);
39+
}
40+
else if(RushinatorWrist.kLastState == State.kTravelRight) {
41+
System.out.println("Fourth If");
42+
return new SetWristState(RushinatorWrist.State.kTravelLeft);
3443
}
3544
else {
36-
return coralPrime(RushinatorPivot.kLastState, ElevatorSubsystem.kLastState, RushinatorWrist.kLastState);
45+
System.out.println("Else");
46+
return new SetWristState(RushinatorWrist.kLastState);
3747
}
48+
// if(RushinatorPivot.kLastState == RushinatorPivot.State.kScore) {
49+
// RushinatorWrist.State toggledWristState = (RushinatorWrist.kLastState == State.kScoreLeftWrist) ? State.kScoreRightWrist : State.kScoreLeftWrist;
50+
// return new SetWristState(toggledWristState);
51+
// }
52+
// else if(RushinatorPivot.kLastState == RushinatorPivot.State.kStowTravel) {
53+
// RushinatorWrist.State toggledWristState = (RushinatorWrist.kLastState == State.kTravelLeft) ? State.kTravelRight : State.kTravelLeft;
54+
// return new SetWristState(toggledWristState);
55+
// }
56+
// else {
57+
// return new SetWristState(RushinatorWrist.kLastState);
58+
// }
3859
}
3960

4061
public static Command algaePrime(AlgaeSubsystem.State algaeState, ElevatorSubsystem.State eleState) {

‎src/main/java/frc/robot/driver/DriverXbox.java‎

Lines changed: 69 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.driver;
22

33
import edu.wpi.first.math.geometry.Translation2d;
4+
import edu.wpi.first.wpilibj2.command.Command;
45
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
56
import edu.wpi.first.wpilibj2.command.InstantCommand;
67
import frc.crevolib.util.ExpCurve;
@@ -23,6 +24,7 @@
2324
import frc.robot.rushinator.RushinatorWrist;
2425
import frc.robot.rushinator.commands.SetArmState;
2526
import frc.robot.rushinator.commands.SetWristState;
27+
import frc.robot.rushinator.commands.ToggleWristState;
2628
import frc.robot.rushinator.commands.SetRollersVoltage;
2729
import frc.robot.vision.commands.LineupCommand;
2830

@@ -68,6 +70,11 @@ public void setupTeleopButtons() {
6870

6971
//Score Coral
7072
// controller.a().whileTrue(new SetRollersVoltage(-1.0));
73+
// controller.x().onTrue(new ConditionalCommand(
74+
// RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreLeftWrist),
75+
// RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreRightWrist),
76+
// () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelLeft)
77+
// );
7178

7279
// Pulse Alage
7380
// controller.x().whileTrue(new AlgaeRoller.IntakeCommand());
@@ -93,6 +100,19 @@ public void setupTeleopButtons() {
93100
// controller.leftTrigger().whileTrue(new SetElevatorState(ElevatorSubsystem.State.kAlgaeIntake));
94101
// ELevator Setting Position ^^^^
95102

103+
// Toggle Wrist Left and Right
104+
// controller.povLeft().onTrue(new ConditionalCommand(
105+
// new SetWristState(RushinatorWrist.State.kTravelLeft),
106+
// new SetWristState(RushinatorWrist.State.kGroundMid),
107+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
108+
// );
109+
110+
// controller.povRight().onTrue(new ConditionalCommand(
111+
// new SetWristState(RushinatorWrist.State.kTravelRight),
112+
// new SetWristState(RushinatorWrist.State.kGroundMid),
113+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
114+
// );
115+
96116
// Zero Elevator
97117
// controller.leftBumper().onTrue(null);
98118

@@ -130,24 +150,58 @@ public void setupTeleopButtons() {
130150
/*Coral Arm Pivot TEsting */
131151
// controller.y().onTrue(new SetArmState(RushinatorPivot.State.kTestPos));
132152
// controller.a().onTrue(new SetArmState(RushinatorPivot.State.kTestPos2));
133-
controller.y().whileTrue(RobotCommands.coralPrime(
134-
RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kTravelLeft)
135-
);
136153

137-
controller.x().onTrue(new ConditionalCommand(
138-
RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreLeftWrist),
139-
RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreRightWrist),
140-
() -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelLeft)
141-
);
154+
// controller.x().onTrue(RobotCommands.coralPrime(
155+
// RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kTravelLeft)
156+
// );
157+
158+
// controller.b().onTrue(RobotCommands.coralPrime(
159+
// RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL3, RushinatorWrist.State.kTravelLeft)
160+
// );
142161

143-
controller.b().onTrue(RobotCommands.toggleWristState());
144-
145-
controller.a().whileTrue(RobotCommands.coralPrime(
146-
RushinatorPivot.State.kFloorIntake, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kGroundMid)
147-
);
148-
controller.a().whileTrue(new SetRollersVoltage(4.0));
162+
// controller.y().onTrue(RobotCommands.coralPrime(
163+
// RushinatorPivot.State.kStowTravel, ElevatorSubsystem.State.kCoralL4, RushinatorWrist.State.kTravelLeft)
164+
// );
165+
166+
// controller.povLeft().onTrue(new ConditionalCommand(
167+
// new SetWristState(RushinatorWrist.State.kTravelLeft),
168+
// new SetWristState(RushinatorWrist.State.kGroundMid),
169+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
170+
// );
149171

150-
controller.leftBumper().whileTrue(new SetRollersVoltage(-1.5));
172+
// controller.povRight().onTrue(new ConditionalCommand(
173+
// new SetWristState(RushinatorWrist.State.kTravelRight),
174+
// new SetWristState(RushinatorWrist.State.kGroundMid),
175+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
176+
// );
177+
178+
// controller.rightBumper().onTrue(new ConditionalCommand(
179+
// RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreLeftWrist),
180+
// RobotCommands.coralPrime(RushinatorPivot.State.kScore, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kScoreRightWrist),
181+
// () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelLeft)
182+
// );
183+
// controller.rightBumper().whileTrue(new SetRollersVoltage(-2.0));
184+
185+
// controller.a().whileTrue(RobotCommands.coralPrime(
186+
// RushinatorPivot.State.kFloorIntake, ElevatorSubsystem.State.kZero, RushinatorWrist.State.kGroundMid)
187+
// );
188+
// controller.a().whileTrue(new SetRollersVoltage(3.0));
189+
190+
// controller.leftBumper().whileTrue(new SetRollersVoltage(-1.5));
191+
192+
// controller.b().onTrue(new ToggleWristState());
193+
// controller.povLeft().onTrue(new ConditionalCommand(
194+
// new SetWristState(RushinatorWrist.State.kTravelLeft),
195+
// new SetWristState(RushinatorWrist.State.kGroundMid),
196+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
197+
// );
198+
199+
// controller.povRight().onTrue(new ConditionalCommand(
200+
// new SetWristState(RushinatorWrist.State.kTravelRight),
201+
// new SetWristState(RushinatorWrist.State.kGroundMid),
202+
// () -> RushinatorPivot.kLastState != RushinatorPivot.State.kFloorIntake)
203+
// );
204+
151205
/*Wrist TEsting*/
152206
// controller.povLeft().onTrue(new SetWristState(RushinatorWrist.State.kScoreLeftWrist));
153207
// controller.povUp().onTrue(new SetWristState(RushinatorWrist.State.kPickUp));

‎src/main/java/frc/robot/elevator/ElevatorSubsystem.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import edu.wpi.first.math.controller.ProfiledPIDController;
1010
import edu.wpi.first.math.geometry.Rotation2d;
1111
import edu.wpi.first.math.trajectory.TrapezoidProfile;
12+
import edu.wpi.first.wpilibj.DigitalInput;
1213
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1314
import edu.wpi.first.wpilibj2.command.Command;
1415
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -48,6 +49,7 @@ public static class Settings {
4849
private TalonFX mTalonLeft, mTalonRight;
4950
private final ElevatorFeedforward mFFLowController, mFFHighController;
5051
private final ProfiledPIDController mPPIDController;
52+
private DigitalInput mLowerLimitSwitch;
5153

5254
private Supplier<Double> mVelocitySupplier;
5355

@@ -85,6 +87,8 @@ private ElevatorSubsystem() {
8587
);
8688
mTalonRight.setPosition(0);
8789

90+
mLowerLimitSwitch = new DigitalInput(0);
91+
8892
mFFLowController = new ElevatorFeedforward(Settings.kS, Settings.kGLow, Settings.kV, Settings.kA);
8993
mFFHighController = new ElevatorFeedforward(Settings.kS, Settings.kGHigh, Settings.kV, Settings.kA);
9094
mPPIDController = new ProfiledPIDController(Settings.kP, Settings.kI, Settings.kD, new TrapezoidProfile.Constraints(

‎src/main/java/frc/robot/rushinator/RushinatorRollers.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ public DefaultCommand() {
6060

6161
@Override
6262
public void initialize() {
63-
subsystem.setFlywheelVoltage(0);
63+
subsystem.setFlywheelVoltage(0.5);
6464
}
6565
}
6666
}

‎src/main/java/frc/robot/rushinator/commands/ToggleWristState.java‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package frc.robot.rushinator.commands;
2+
3+
import java.net.CookieManager;
4+
5+
import edu.wpi.first.math.geometry.Rotation2d;
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
import frc.robot.rushinator.RushinatorPivot;
8+
import frc.robot.rushinator.RushinatorWrist;
9+
import frc.robot.rushinator.RushinatorWrist.State;
10+
11+
public class ToggleWristState extends Command{
12+
RushinatorWrist mRushinatorWrist;
13+
Rotation2d targetAngle;
14+
RushinatorWrist.State kTargetState;
15+
16+
public ToggleWristState() {
17+
mRushinatorWrist = RushinatorWrist.getInstance();
18+
// targetAngle = targetState.pos;
19+
addRequirements(mRushinatorWrist);
20+
}
21+
22+
@Override
23+
public void initialize() {
24+
System.out.println("Last State in Toggle Wrist" + RushinatorWrist.kLastState.name());
25+
if(RushinatorWrist.kLastState == State.kScoreLeftWrist) {
26+
System.out.println("First If");
27+
mRushinatorWrist.setTargetState(RushinatorWrist.State.kScoreRightWrist);
28+
}
29+
else if(RushinatorWrist.kLastState == State.kScoreRightWrist) {
30+
System.out.println("2nd If");
31+
mRushinatorWrist.setTargetState(RushinatorWrist.State.kScoreLeftWrist);
32+
}
33+
else if(RushinatorWrist.kLastState == State.kTravelLeft) {
34+
System.out.println("3rd If");
35+
mRushinatorWrist.setTargetState(RushinatorWrist.State.kTravelRight);
36+
}
37+
else if(RushinatorWrist.kLastState == State.kTravelRight) {
38+
System.out.println("Fourth If");
39+
mRushinatorWrist.setTargetState(RushinatorWrist.State.kTravelLeft);
40+
}
41+
else {
42+
System.out.println("Else");
43+
mRushinatorWrist.setTargetState(RushinatorWrist.kLastState);
44+
}
45+
}
46+
47+
@Override
48+
public void end(boolean interrupted) {
49+
50+
}
51+
}

0 commit comments

Comments
 (0)