SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Almost Working Speciem hanging code!

Browse Source
This commit is contained in:
robotics1
2024-11-19 17:31:33 -08:00
parent 05e284b59f
commit dbc9cf929b
4 changed files with 52 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java
View File

@ -29,6 +29,9 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
@ -37,6 +40,8 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
public class LiftWristArmTest extends LinearOpMode {
@ -52,6 +57,8 @@ public class LiftWristArmTest extends LinearOpMode {
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
@ -62,6 +69,7 @@ public class LiftWristArmTest extends LinearOpMode {
lift.init();
wrist.init();
arm.init();
claw.init();
waitForStart();
runtime.reset();
@ -79,6 +87,23 @@ public class LiftWristArmTest extends LinearOpMode {
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
claw.switchState();
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
Actions.runBlocking(
new SequentialAction(
lift.toHighRung(),
arm.toBucketPosition(),
wrist.toRungPosition(),
new SleepAction(2),
lift.toHighRungAttach(),
new SleepAction(2),
claw.openClaw())
);
//Delete open claw }
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
wrist.setPosition(wrist.getPosition() + .05);
@ -96,6 +121,7 @@ public class LiftWristArmTest extends LinearOpMode {
lift.setPosition(lift.getPosition() - 25);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -14,12 +14,16 @@ public class RobotConstants {
public final static double wristFloor = 0.55;
public final static double wristBucket = 0.25;
public final static double wristRung = 0.55;
public final static double wristPickup = 0.1;
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToLowBucketPos = 2250;
public final static int liftToHighRung = 2250;
public final static int liftToHighRung = 1245;
public final static int liftToHighRungAttach = 1050;
public final static int liftToHighBucketPos = 3850;
public final static double liftPower = .625;

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java
View File

@ -6,6 +6,10 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorP
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
import androidx.annotation.NonNull;
@ -21,7 +25,7 @@ public class LiftActionsSubsystem {
public DcMotor lift;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE,
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG
}
private LiftState liftState;
@ -56,6 +60,16 @@ public class LiftActionsSubsystem {
return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR);
}
public Action toHighRung() {
return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
}
public Action toHighRungAttach() {
return new MoveToPosition(liftToHighRungAttach, LiftState.HIGH_RUNG);
}
public Action toLowBucketPosition() {
return new MoveToPosition(liftToLowBucketPos, LiftState.LOW_BUCKET);

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java
View File

@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristRung;
import androidx.annotation.NonNull;
@ -15,7 +17,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristActionsSubsystem {
public enum WristState {
FLOOR, BUCKET, PICKUP
FLOOR, BUCKET, PICKUP, RUNG
}
public ServoImplEx wrist;
@ -46,6 +48,9 @@ public class WristActionsSubsystem {
public Action toFloorPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toRungPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toBucketPosition() {
return new MoveToPosition(wristBucket, WristState.BUCKET);