Skip to content

Commit 82745e9

Browse files
peterpdepeterpde
committed
sequences
1 parent b2793de commit 82745e9

File tree

6 files changed

+87
-80
lines changed

6 files changed

+87
-80
lines changed

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -282,23 +282,27 @@ public static final class IntakeConstants {
282282

283283
public static final class ExtensionMechanism{
284284

285-
public static int extensionMotorId = 23;
285+
public static int extensionMotorId = 24;
286286

287287

288-
public static final double extendedPosition = 100.0;
289-
public static final double retractedPosition = 0.0;
288+
//public static final double extendedPosition = 100.0;
289+
public static final double extendedPositionEncoderValue = .969;
290+
//public static final double retractedPosition = 0.0;
291+
public static final double retractedPositionEncoderValue = .2799;
290292
public static final double extensionTolerance = 5.0;
291-
public static final double half_stowPosition = 90.0;
293+
//public static final double half_stowPosition = 90.0;
294+
public static final double half_stowPostionEncoderValue = .2457;
295+
public static final double climbPositionEncoderValue = .2009;
292296

293297
public static final double maxExtensionVoltage = 1.0;
294298

295-
public static final double maxExtensionVelocity = 0;
296-
public static final double maxExtensionAcceleration = 0;
299+
public static final double maxExtensionVelocity = .1;
300+
public static final double maxExtensionAcceleration = .1;
297301

298-
public static final double extensionSoftLowerBound = 0;
299-
public static final double extensionSoftUpperBound = 200;
302+
public static final double extensionSoftLowerBound = .0;
303+
public static final double extensionSoftUpperBound = .27990;
300304

301-
public static final double kpIntakeExtension = 0.0;
305+
public static final double kpIntakeExtension = 0.1;
302306
public static final double kiIntakeExtension = 0.0;
303307
public static final double kdIntakeExtension = 0.0;
304308

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 28 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -74,13 +74,13 @@ public class RobotContainer {
7474
private final ledSubsystem leds;
7575
private final Climb climb;
7676

77-
private final boolean compititionBindings = true;
77+
private final boolean compititionBindings = false;
7878
private final boolean driveEnabled = true;
79-
private final boolean wristEnabled = true;
80-
private final boolean manipulatorEnabled = true;
81-
private final boolean intakeEnabled = false;
79+
private final boolean wristEnabled = false;
80+
private final boolean manipulatorEnabled = false;
81+
private final boolean intakeEnabled = true;
8282
private final boolean passthroughEnabled = false;
83-
private final boolean elevatorEnabled = true;
83+
private final boolean elevatorEnabled = false;
8484
private final boolean servoEnabled = false;
8585
private final boolean ledEnabled = false;
8686
private final boolean climbEnabled = false;
@@ -167,33 +167,27 @@ private Command getOutakeCommand(ElevatorHeight height, Trigger trigger){
167167
}
168168

169169
command = command.until(() -> trigger.getAsBoolean() == false)
170-
.finallyDo(() -> {
171-
new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3)
170+
.andThen(new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3))
172171
.withInterruptBehavior(InterruptionBehavior.kCancelSelf)
173-
.schedule();
174-
});
172+
.andThen(new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.PASSTHROUGH))
173+
.withInterruptBehavior(InterruptionBehavior.kCancelSelf);
175174

176175
return command;
177176
}
178177

179-
public Command getFunnelIntakeCommand(Trigger trigger)
178+
public Command getFunnelIntakeCommand()
180179
{
181180
// Note need to make sure to set the intake at half stow position here
182181

183182
return new SequentialCommandGroup(
184183
new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3),
185184
new ElevatorGoToPositionPositionCommand(elevator, ElevatorHeight.FUNNEL),
186-
new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3),
185+
new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.FUNNEL_ANGLE),
187186
new IntakeCoralCommand(manipulator, passthrough))
188-
.withInterruptBehavior(InterruptionBehavior.kCancelSelf)
189-
.until(
190-
() -> manipulator.isDetectingCoral() || trigger.getAsBoolean() == false)
191-
.finallyDo(() -> {
192-
new SequentialCommandGroup(new InstantCommand( () -> manipulator.stopOuttake()),
193-
new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3))
187+
.andThen(
188+
new SequentialCommandGroup(new InstantCommand( () -> manipulator.stopOuttake())))
189+
.andThen(new WristGoToPositionCommand(wrist, Constants.WristConstants.WristAngleRad.L2L3))
194190
.withInterruptBehavior(InterruptionBehavior.kCancelSelf)
195-
.schedule();
196-
})
197191
.unless(() -> manipulator.isDetectingCoral());
198192
}
199193

@@ -209,7 +203,8 @@ private void configureButtonBindings() {
209203

210204
// Funnel Intake
211205
Trigger bPressedTrigger = operatorController.b();
212-
bPressedTrigger.onTrue(this.getFunnelIntakeCommand(bPressedTrigger));
206+
//bPressedTrigger.whileTrue(this.getFunnelIntakeCommand());
207+
bPressedTrigger.onTrue(this.getFunnelIntakeCommand());
213208

214209
// L1 and L2 Binding
215210
Trigger aPressedTrigger = operatorController.a();
@@ -230,7 +225,7 @@ private void configureButtonBindings() {
230225
DoubleSupplier horizontalAxis = () -> operatorController.getRightX();
231226
climb.setDefaultCommand(new ManualClimbCommand(climb, horizontalAxis));
232227

233-
// Start button is meant to prepare for the climb. Drops the funnel and moves the
228+
// Start button is meant to prepare for the climb. Drops the funnel and moves the
234229
// floor intake back
235230
Trigger startButton = operatorController.start();
236231

@@ -289,36 +284,6 @@ public Command getAutonomousCommand() {
289284
}
290285
}
291286

292-
293-
294-
295-
296-
297-
298-
299-
300-
301-
302-
303-
304-
305-
306-
307-
308-
309-
310-
311-
312-
313-
314-
315-
316-
317-
318-
319-
320-
321-
322287
// ============= Instantiation =============
323288

324289

@@ -466,13 +431,24 @@ private void configureIntakeBindings() {
466431
Trigger mainTrigger = new JoystickButton(flightStick, 1);
467432
mainTrigger.whileTrue(Commands.runOnce(() -> {intake.goToExtendedPosition();}, intake));
468433
mainTrigger.whileFalse(Commands.runOnce(() -> {intake.goToRetractedPosition();}, intake));
434+
435+
Trigger aButton = operatorController.a();
436+
aButton.onTrue(new IntakeCommand(intake, INTAKE_POSITION.HALF_STOW));
437+
438+
Trigger xButton = operatorController.x();
439+
xButton.onTrue(new IntakeCommand(intake, INTAKE_POSITION.STOW));
440+
441+
Trigger yButton = operatorController.y();
442+
yButton.onTrue(new IntakeCommand(intake, INTAKE_POSITION.CLIMB));
443+
469444
}
470445

471446

472447
public enum INTAKE_POSITION{
473448
INTAKE,
474449
HALF_STOW,
475-
STOW
450+
STOW,
451+
CLIMB
476452
}
477453

478454
public enum CLIMB_POSITION{

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

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,14 +23,18 @@ public IntakeCommand(Intake intakeSub, INTAKE_POSITION position) {
2323
public void initialize() {
2424
switch (position) {
2525
case INTAKE:
26-
intakeSub.goToPosition(Constants.IntakeConstants.ExtensionMechanism.extendedPosition);
26+
//intakeSub.goToPosition(Constants.IntakeConstants.ExtensionMechanism.extendedPosition);
27+
// temporary just going to stow half stow position
28+
intakeSub.goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.half_stowPostionEncoderValue);
2729
break;
2830
case HALF_STOW:
29-
intakeSub.goToPosition(Constants.IntakeConstants.ExtensionMechanism.half_stowPosition);
31+
intakeSub.goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.half_stowPostionEncoderValue);
3032
break;
3133
case STOW:
32-
intakeSub.goToPosition(Constants.IntakeConstants.ExtensionMechanism.retractedPosition);
34+
intakeSub.goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.retractedPositionEncoderValue);
3335
break;
36+
case CLIMB:
37+
intakeSub.goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.climbPositionEncoderValue);
3438
default:
3539
end(true);
3640
break;
@@ -47,6 +51,6 @@ public void end(boolean interrupted) {}
4751

4852
@Override
4953
public boolean isFinished() {
50-
return true;
54+
return intakeSub.hasReachedGoal();
5155
}
5256
}

‎src/main/java/frc/robot/subsystems/Intake/Intake.java‎

Lines changed: 22 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -64,20 +64,33 @@ public void periodic() {
6464
Constants.IntakeConstants.ExtensionMechanism.maxExtensionVoltage
6565
);
6666

67-
intakeIO.setExtensionMotorVoltage(voltage);
67+
//intakeIO.setExtensionMotorVoltage(voltage);
6868

6969
}
7070

71-
public void goToPosition(double degrees){
72-
extensionPID.setGoal(new State(degrees, 0));
71+
// public void goToPosition(double degrees){
72+
// extensionPID.setGoal(new State(degrees, 0));
73+
// }
74+
75+
public void goToPositionDirect(double encoderValue){
76+
extensionPID.setGoal(new State(encoderValue, 0));
7377
}
7478

7579
public void goToExtendedPosition() {
76-
goToPosition(Constants.IntakeConstants.ExtensionMechanism.extendedPosition);
80+
//goToPosition(Constants.IntakeConstants.ExtensionMechanism.extendedPosition);
81+
goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.extendedPositionEncoderValue);
7782
}
7883

7984
public void goToRetractedPosition() {
80-
goToPosition(Constants.IntakeConstants.ExtensionMechanism.retractedPosition);
85+
goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.retractedPositionEncoderValue);
86+
}
87+
88+
public void goToHalfStowPosition() {
89+
goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.half_stowPostionEncoderValue);
90+
}
91+
92+
public void goToClimbPosition() {
93+
goToPositionDirect(Constants.IntakeConstants.ExtensionMechanism.climbPositionEncoderValue);
8194
}
8295

8396
public void runIntake(){
@@ -87,4 +100,8 @@ public void runIntake(){
87100
public void stopIntake(){
88101
intakeIO.setIntakeMotorVoltage(0);
89102
}
103+
104+
public boolean hasReachedGoal() {
105+
return extensionPID.atGoal();
106+
}
90107
}

‎src/main/java/frc/robot/subsystems/Intake/intakeIOReal.java‎

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
package frc.robot.subsystems.Intake;
22

3+
import com.revrobotics.spark.SparkFlex;
34
import com.revrobotics.spark.SparkMax;
45
import com.revrobotics.spark.SparkBase.PersistMode;
56
import com.revrobotics.spark.config.SparkMaxConfig;
67

78
import frc.robot.Constants;
89

910
import com.revrobotics.spark.config.SoftLimitConfig;
11+
import com.revrobotics.spark.config.SparkFlexConfig;
1012
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
1113
import com.revrobotics.AbsoluteEncoder;
1214
import com.revrobotics.RelativeEncoder;
@@ -18,7 +20,7 @@ public class IntakeIOReal implements IntakeIO{
1820
private final SparkMax intakeMotor;
1921
private final RelativeEncoder intakeEncoder;
2022

21-
private final SparkMax intakeExtensionMotor;
23+
private final SparkFlex intakeExtensionMotor;
2224
private final AbsoluteEncoder extensionEncoder;
2325

2426

@@ -27,25 +29,28 @@ public IntakeIOReal(int intakeMotorID_1, int intakeExtensionMotorID){
2729

2830
intakeMotor = new SparkMax(intakeMotorID_1, MotorType.kBrushless);
2931

30-
intakeExtensionMotor = new SparkMax(intakeExtensionMotorID, MotorType.kBrushless);
32+
intakeExtensionMotor = new SparkFlex(intakeExtensionMotorID, MotorType.kBrushless);
3133

3234
extensionEncoder = intakeExtensionMotor.getAbsoluteEncoder();
3335
intakeEncoder = intakeMotor.getEncoder();
3436

3537

3638
SparkMaxConfig intakeMotor1Config = new SparkMaxConfig();
37-
intakeMotor1Config.idleMode(IdleMode.kBrake);
39+
intakeMotor1Config.idleMode(IdleMode.kCoast);
3840

39-
SparkMaxConfig config_3 = new SparkMaxConfig();
40-
config_3.idleMode(IdleMode.kBrake);
41+
42+
SparkFlexConfig intakeExtensionMotorConfig = new SparkFlexConfig();
43+
intakeExtensionMotorConfig.idleMode(IdleMode.kBrake);
44+
45+
intakeExtensionMotorConfig.closedLoop.feedbackSensor(com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor.kAbsoluteEncoder);
4146

4247
SoftLimitConfig limitConfig = new SoftLimitConfig();
4348
limitConfig.forwardSoftLimit(Constants.IntakeConstants.ExtensionMechanism.extensionSoftUpperBound);
4449
limitConfig.reverseSoftLimit(Constants.IntakeConstants.ExtensionMechanism.extensionSoftLowerBound);
45-
config_3.softLimit.apply(limitConfig);
50+
intakeExtensionMotorConfig.softLimit.apply(limitConfig);
4651

4752
intakeMotor.configure(intakeMotor1Config, null, PersistMode.kPersistParameters);
48-
intakeExtensionMotor.configure(config_3, null, PersistMode.kPersistParameters);
53+
intakeExtensionMotor.configure(intakeExtensionMotorConfig, null, PersistMode.kPersistParameters);
4954

5055

5156

@@ -58,11 +63,11 @@ public void stopExtensionMotor() {
5863

5964
@Override
6065
public void setExtensionMotorVoltage(double voltage){
61-
intakeExtensionMotor.setVoltage(voltage);
66+
//intakeExtensionMotor.setVoltage(voltage);
6267
}
6368

6469
public void setIntakeMotorVoltage(double voltage){
65-
intakeMotor.setVoltage(voltage);
70+
//intakeMotor.setVoltage(voltage);
6671
}
6772

6873
@Override

‎src/main/java/frc/robot/subsystems/WristStuff/Wrist.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ public Wrist(){
5454

5555
wristIO.updateInputs(inputs);
5656

57-
setTargetAngle(inputs.wristPosition);
57+
//setTargetAngle(inputs.wristPosition);
58+
setTargetAngle(Constants.WristConstants.WristAngleRad.PASSTHROUGH.positionRad);
5859

5960
}
6061

0 commit comments

Comments
 (0)