Skip to content

Commit

Permalink
fix claw
Browse files Browse the repository at this point in the history
  • Loading branch information
@Bi1ku
Bi1ku committed Oct 20, 2024
1 parent 3d6ebe7 commit bd8aade
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 23 deletions.
30 changes: 9 additions & 21 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,15 +29,12 @@ public class CommandRobot {
private final Lift lift;
private final Extendo extendo;
private final Claw claw;

private GamepadEx gamepad1;
private GamepadEx gamepad2;

private final OpModeCore opMode;

public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, score, liftIncrement, liftDecrement, extendoIncrement, extendoDecrement;

public GamepadTrigger intakeAccept, intakeReject;
private GamepadEx gamepad1;
private GamepadEx gamepad2;

// TELEOP
public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, OpModeCore opMode) {
this.telemetry = telemetry;
Expand Down Expand Up @@ -80,12 +77,12 @@ public MecanumDrive getDrive() {
public void startThreads() {
this.drivetrain.startThread(this.gamepad1, this.opMode);
this.lift.startThread(this.opMode);
this.claw.startThreads(this.opMode);
// TODO: Add in more threads if needed
}

public void configureCommands() {
this.ready = new SequentialCommandGroup(
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE),
new ClawSetPosition(this.telemetry, this.claw, Claw.READY),
new WaitCommand(100),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.READY),
Expand All @@ -96,54 +93,45 @@ public void configureCommands() {
new LiftSetPosition(this.telemetry, this.lift, Lift.ACCEPTING),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.ACCEPTING),
new WaitCommand(100),
new ClawSetPosition(this.telemetry, this.claw, Claw.DOWN),
new ClawSetPosition(this.telemetry, this.claw, Claw.OPEN)
new ClawSetPosition(this.telemetry, this.claw, Claw.DOWN)
);

this.highBasket = new ParallelCommandGroup(
new LiftSetPosition(this.telemetry, this.lift, Lift.HIGH_BASKET),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
new ClawSetPosition(this.telemetry, this.claw, Claw.UP),
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE)
new ClawSetPosition(this.telemetry, this.claw, Claw.UP)
);

this.lowBasket = new ParallelCommandGroup(
new LiftSetPosition(this.telemetry, this.lift, Lift.LOW_BASKET),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
new ClawSetPosition(this.telemetry, this.claw, Claw.UP),
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE)
new ClawSetPosition(this.telemetry, this.claw, Claw.UP)
);

this.highRung = new SequentialCommandGroup(
new LiftSetPosition(this.telemetry, this.lift, Lift.HIGH_RUNG),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
new ClawSetPosition(this.telemetry, this.claw, Claw.UP),
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE)
new ClawSetPosition(this.telemetry, this.claw, Claw.UP)
);

this.lowRung = new SequentialCommandGroup(
new LiftSetPosition(this.telemetry, this.lift, Lift.LOW_RUNG),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
new ClawSetPosition(this.telemetry, this.claw, Claw.UP),
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE)
new ClawSetPosition(this.telemetry, this.claw, Claw.UP)
);

this.score = new SequentialCommandGroup(
new ClawSetPosition(this.telemetry, this.claw, Claw.OPEN),
new WaitCommand(800),
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE),
new ClawSetPosition(this.telemetry, this.claw, Claw.READY),
new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.READY),
new LiftSetPosition(this.telemetry, this.lift, Lift.ACCEPTING)
);

this.liftIncrement = new SequentialCommandGroup(
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE),
new LiftSetPosition(this.telemetry, this.lift, this.lift.getTarget() + Lift.INCREMENT)
);

this.liftDecrement = new SequentialCommandGroup(
new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE),
new LiftSetPosition(this.telemetry, this.lift, this.lift.getTarget() - Lift.INCREMENT)

);
Expand Down
13 changes: 11 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,14 @@
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.command.SubsystemBase;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.ColorRangeSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
import org.firstinspires.ftc.teamcode.utils.hardware.EnhancedColorSensor;

public class Claw extends SubsystemBase {
public static double OPEN = 0.5;
public static double CLOSE = 0.0;
public static double UP = 1;
public static double READY = 0.5;
public static double DOWN = 0.0;
Expand All @@ -17,6 +19,7 @@ public class Claw extends SubsystemBase {

private final Servo leftPivot, rightPivot;
private final CRServo leftCRServo, rightCRServo;
private final EnhancedColorSensor sensor;

public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
this.telemetry = telemetry;
Expand All @@ -26,6 +29,12 @@ public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {

this.rightPivot = hwMap.get(Servo.class, "rightPivot");
this.rightCRServo = hwMap.get(CRServo.class, "rightCRServo");

this.sensor = new EnhancedColorSensor(hwMap.get(ColorRangeSensor.class, "sensor"));
}

public void startThreads(OpModeCore opMode) {
this.sensor.startThread(opMode);
}

public void setPosition(double position) {
Expand Down

0 comments on commit bd8aade

Please sign in to comment.