Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit 600cbd1

Browse files
schen479schen479
committed
new claw code for the 3rd time
1 parent 477e89a commit 600cbd1

File tree

6 files changed

+87
-48
lines changed

6 files changed

+87
-48
lines changed

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java‎

Lines changed: 23 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,11 @@
1111
import com.qualcomm.robotcore.hardware.Gamepad;
1212
import com.qualcomm.robotcore.hardware.HardwareMap;
1313

14-
import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
15-
import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
1614
import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
1715
import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
16+
import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
17+
import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateReady;
18+
import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
1819
import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
1920
import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoReady;
2021
import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
@@ -30,11 +31,14 @@
3031
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
3132
import org.firstinspires.ftc.teamcode.subsystems.Extendo;
3233
import org.firstinspires.ftc.teamcode.subsystems.Lift;
34+
import org.firstinspires.ftc.teamcode.utils.commands.GamepadTrigger;
3335
import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
3436

3537
@Config
3638
public class CommandRobot {
37-
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close, slamdown, specimen;
39+
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, slamdown, specimen;
40+
public GamepadTrigger intakeAccept, intakeReject;
41+
3842
private TeleOpMode mode;
3943

4044
private final MultipleTelemetry telemetry;
@@ -98,8 +102,8 @@ public void startThreads() {
98102
public void configureCommands() {
99103
this.ready = new SequentialCommandGroup(
100104
new LiftAccepting(this.telemetry, this.lift),
101-
new ClawClose(this.telemetry, this.claw),
102-
new ClawPivotScore(this.telemetry, this.claw),
105+
new ClawRotateReady(this.telemetry, this.claw),
106+
new ClawPivotAccepting(this.telemetry, this.claw),
103107
new WaitCommand(CommandRobot.CLAW_RETRACT_DELAY),
104108
new ExtendoReady(this.telemetry, this.extendo)
105109
);
@@ -108,48 +112,53 @@ public void configureCommands() {
108112
new LiftAccepting(this.telemetry, this.lift),
109113
new ExtendoAccepting(this.telemetry, this.extendo),
110114
new WaitCommand(CommandRobot.CLAW_ACCEPT_DELAY),
111-
new ClawOpen(this.telemetry, this.claw),
115+
new ClawRotateDown(this.telemetry, this.claw),
112116
new ClawPivotAccepting(this.telemetry, this.claw)
113117
);
114118

115119
this.highBasket = new SequentialCommandGroup(
116120
new LiftHighBasket(this.telemetry, this.lift),
117121
new ExtendoScore(this.telemetry, this.extendo),
118-
new ClawPivotScore(this.telemetry, this.claw)
122+
new ClawPivotScore(this.telemetry, this.claw),
123+
new ClawRotateUp(this.telemetry, this.claw)
119124
);
120125

121126
this.lowBasket = new SequentialCommandGroup(
122127
new LiftLowBasket(this.telemetry, this.lift),
123128
new ExtendoScore(this.telemetry, this.extendo),
124-
new ClawPivotScore(this.telemetry, this.claw)
129+
new ClawPivotScore(this.telemetry, this.claw),
130+
new ClawRotateUp(this.telemetry, this.claw)
125131
);
126132

127133
this.highRung = new SequentialCommandGroup(
128134
new LiftHighRung(this.telemetry, this.lift),
129135
new ExtendoScore(this.telemetry, this.extendo),
130-
new ClawPivotScore(this.telemetry, this.claw)
136+
new ClawPivotScore(this.telemetry, this.claw),
137+
new ClawRotateUp(this.telemetry, this.claw)
131138
);
132139

133140
this.lowRung = new SequentialCommandGroup(
134141
new LiftLowRung(this.telemetry, this.lift),
135142
new ExtendoScore(this.telemetry, this.extendo),
136-
new ClawPivotScore(this.telemetry, this.claw)
143+
new ClawPivotScore(this.telemetry, this.claw),
144+
new ClawRotateUp(this.telemetry, this.claw)
145+
137146
);
138147

139148
this.specimen = new SequentialCommandGroup(
140149
new LiftLowRung(this.telemetry, this.lift),
141150
new ExtendoReady(this.telemetry, this.extendo),
142151
new ClawPivotScore(this.telemetry, this.claw),
143-
new ClawOpen(this.telemetry, this.claw)
152+
new ClawRotateReady(this.telemetry, this.claw)
144153
);
145154

155+
this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad2);
156+
this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad2);
157+
146158
this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);
147159

148160
this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
149161

150-
this.open = new ClawOpen(this.telemetry, this.claw);
151-
152-
this.close = new ClawClose(this.telemetry, this.claw);
153162
}
154163

155164
public void configureControls() {
@@ -175,10 +184,6 @@ public void configureControls() {
175184
.whenPressed(this.highBasket);
176185
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
177186
.whenPressed(this.lowBasket);
178-
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
179-
.whenPressed(this.open);
180-
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
181-
.whenPressed(this.close);
182187
break;
183188

184189
/* ------------------------------------- */
@@ -202,10 +207,6 @@ public void configureControls() {
202207
.whenPressed(this.lowRung);
203208
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
204209
.whenPressed(this.highRung);
205-
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
206-
.whenPressed(this.open);
207-
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
208-
.whenPressed(this.close);
209210
break;
210211

211212
/* ------------------------------------- */
@@ -229,11 +230,6 @@ public void configureControls() {
229230
.whenPressed(this.lowRung);
230231
this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
231232
.whenPressed(this.highRung);
232-
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
233-
.whenPressed(this.open);
234-
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
235-
.whenPressed(this.close);
236-
237233
break;
238234
}
239235
}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawOpen.java renamed to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateDown.java

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,11 @@
55

66
import org.firstinspires.ftc.teamcode.subsystems.Claw;
77

8-
public class ClawOpen extends CommandBase {
8+
public class ClawRotateDown extends CommandBase {
99
private final MultipleTelemetry telemetry;
1010
private final Claw claw;
1111

12-
public ClawOpen(final MultipleTelemetry telemetry, final Claw claw) {
12+
public ClawRotateDown(final MultipleTelemetry telemetry, final Claw claw) {
1313
this.telemetry = telemetry;
1414

1515
this.claw = claw;
@@ -18,11 +18,12 @@ public ClawOpen(final MultipleTelemetry telemetry, final Claw claw) {
1818

1919
@Override
2020
public void initialize() {
21-
this.claw.setClawPosition(Claw.OPEN);
21+
this.claw.setPivotPosition(Claw.DOWN);
2222
}
2323

2424
@Override
2525
public boolean isFinished() {
2626
return true;
2727
}
2828
}
29+

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package org.firstinspires.ftc.teamcode.commands.claw;
2+
3+
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
4+
import com.arcrobotics.ftclib.command.CommandBase;
5+
6+
import org.firstinspires.ftc.teamcode.subsystems.Claw;
7+
8+
public class ClawRotateReady extends CommandBase {
9+
private final MultipleTelemetry telemetry;
10+
private final Claw claw;
11+
12+
public ClawRotateReady(final MultipleTelemetry telemetry, final Claw claw) {
13+
this.telemetry = telemetry;
14+
15+
this.claw = claw;
16+
super.addRequirements(claw);
17+
}
18+
19+
@Override
20+
public void initialize() {
21+
this.claw.setPivotPosition(Claw.READY);
22+
}
23+
24+
@Override
25+
public boolean isFinished() {
26+
return true;
27+
}
28+
}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawClose.java renamed to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateUp.java

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,11 @@
55

66
import org.firstinspires.ftc.teamcode.subsystems.Claw;
77

8-
public class ClawClose extends CommandBase {
9-
private final Claw claw;
8+
public class ClawRotateUp extends CommandBase {
109
private final MultipleTelemetry telemetry;
10+
private final Claw claw;
1111

12-
public ClawClose(final MultipleTelemetry telemetry, final Claw claw) {
12+
public ClawRotateUp(final MultipleTelemetry telemetry, final Claw claw) {
1313
this.telemetry = telemetry;
1414

1515
this.claw = claw;
@@ -18,7 +18,7 @@ public ClawClose(final MultipleTelemetry telemetry, final Claw claw) {
1818

1919
@Override
2020
public void initialize() {
21-
this.claw.setClawPosition(Claw.CLOSE);
21+
this.claw.setPivotPosition(Claw.UP);
2222
}
2323

2424
@Override

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,10 @@
55
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
66
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
77

8-
import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
9-
import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
108
import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
119
import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
10+
import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
11+
import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
1212
import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
1313
import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
1414
import org.firstinspires.ftc.teamcode.subsystems.Claw;
@@ -21,20 +21,22 @@ public class ClawTest extends OpModeCore {
2121
private Claw claw;
2222
private Extendo extendo;
2323
private GamepadEx gamepad;
24-
private GamepadTrigger intake, outtake;
24+
private GamepadTrigger intakeAccept, intakeReject;
2525

2626
@Override
2727
public void initialize() {
2828
this.gamepad = new GamepadEx(super.gamepad1);
2929
this.claw = new Claw(super.hardwareMap, super.multipleTelemetry);
3030
this.extendo = new Extendo(super.hardwareMap, super.multipleTelemetry);
3131

32-
this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawOpen(super.multipleTelemetry, this.claw));
33-
this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawClose(super.multipleTelemetry, this.claw));
32+
this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
33+
this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
34+
this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_LEFT).whenPressed(new ClawRotateUp(super.multipleTelemetry, this.claw));
3435
this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ClawPivotScore(super.multipleTelemetry, this.claw));
3536
this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new ClawPivotAccepting(super.multipleTelemetry, this.claw));
36-
this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(new ExtendoScore(super.multipleTelemetry, this.extendo));
37-
this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(new ExtendoAccepting(super.multipleTelemetry, this.extendo));
37+
38+
this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad);
39+
this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad);
3840
}
3941

4042
@Override

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -4,37 +4,49 @@
44
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
55
import com.arcrobotics.ftclib.command.SubsystemBase;
66
import com.qualcomm.robotcore.hardware.HardwareMap;
7+
import com.qualcomm.robotcore.hardware.CRServo;
78
import com.qualcomm.robotcore.hardware.Servo;
89

910
@Config
1011
public class Claw extends SubsystemBase {
1112
public static double SCORE = 1;
1213
public static double ACCEPTING = 0.55;
1314

14-
public static double OPEN = 1;
15-
public static double CLOSE = 0.49;
15+
//TODO: Get positions for claw rotate
16+
public static double UP = 1;
17+
public static double DOWN = 0.49;
18+
public static double READY = 0;
1619

1720
private final MultipleTelemetry telemetry;
1821

19-
private final Servo leftPivot, rightPivot, claw;
22+
private final Servo leftPivot, rightPivot, clawRotate;
23+
private final CRServo leftClaw, rightClaw;
24+
2025

2126
public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
2227
this.telemetry = telemetry;
2328

2429
this.leftPivot = hwMap.get(Servo.class, "leftPivot");
2530
this.rightPivot = hwMap.get(Servo.class, "rightPivot");
26-
this.claw = hwMap.get(Servo.class, "claw");
31+
this.leftClaw = hwMap.get(CRServo.class, "leftClaw");
32+
this.rightClaw = hwMap.get(CRServo.class, "rightClaw");
33+
this.clawRotate = hwMap.get(Servo.class, "clawRotate");
2734

2835
this.setPivotPosition(Claw.SCORE);
29-
this.setClawPosition(Claw.CLOSE);
36+
this.setRotatePosition(Claw.READY);
3037
}
3138

3239
public void setPivotPosition(double position) {
3340
this.leftPivot.setPosition(position);
3441
this.rightPivot.setPosition(1 - position);
3542
}
3643

37-
public void setClawPosition(double position) {
38-
this.claw.setPosition(position);
44+
public void setRotatePosition(double position) {
45+
this.clawRotate.setPosition(position);
46+
}
47+
48+
public void setClawPower(double power) {
49+
this.leftClaw.setPower(power);
50+
this.rightClaw.setPower(-power);
3951
}
4052
}

0 commit comments

Comments
 (0)