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-12-03 17:31:46 -08:00
parent c20c5ba624
commit 238dcd4ae9
1 changed files with 59 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java
View File

@ -29,6 +29,10 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
@ -38,6 +42,11 @@ import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
@ -49,9 +58,29 @@ public class SpecimenTest extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
private PathChain path;
private final Pose startPose = new Pose(36, 72);
public class MoveToPath implements Action {
private Follower actionRobot;
private PathChain pathChain;
private MoveToPath(PathChain path, Follower robot) {
this.actionRobot = robot;
this.pathChain = path;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
return false;
}
}
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
@ -61,18 +90,33 @@ public class SpecimenTest extends LinearOpMode {
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
Follower robot = new Follower(hardwareMap);
/*
Robot stuff
*/
robot.setStartingPose(startPose);
path = robot.pathBuilder() .addPath(
// Line 1
new BezierLine(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(37.500, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
Gamepad currentGamepad2 = new Gamepad();
Gamepad previousGamepad2 = new Gamepad();
lift.init();
wrist.init();
arm.init();
claw.init();
robot.setMaxPower(.75);
robot.startTeleopDrive();
robot.followPath(path);
waitForStart();
runtime.reset();
@ -84,6 +128,10 @@ public class SpecimenTest extends LinearOpMode {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
//robot.update();
@ -104,6 +152,7 @@ public class SpecimenTest extends LinearOpMode {
lift.toHighRung(),
wrist.toSpeciemenBar(),
lift.dropToHighRung()
)
);
@ -136,6 +185,15 @@ public class SpecimenTest extends LinearOpMode {
lift.setPosition(lift.getPosition() - 25);
}
if (currentGamepad2.x && !previousGamepad2.x){
while(true) {
robot.update();
if (!robot.isBusy()) {
break;
}
}
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());