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

New TeleOp

Browse Source
This commit is contained in:
robotics3
2025-01-16 08:01:31 -08:00
parent 39e416e35a
commit 8c6ce96ae4
4 changed files with 211 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GeneratedPath.java Normal file
View File

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MultipleTelemetry.java Normal file
View File

84
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
View File

@ -15,8 +15,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
public class CometBotAutoDevelopment {
@ -24,7 +27,9 @@ public class CometBotAutoDevelopment {
Subsystems
*/
private DualMotorSliderSubsystem dualSlides;
private ClawSubsystem claw;
private WristSubsystem wrist;
private ArmSubsystem arm;
/*
Controllers
*/
@ -35,10 +40,15 @@ public class CometBotAutoDevelopment {
public Gamepad previousGamepad1;
public Gamepad previousGamepad2;
private Follower follower;
public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
dualSlides = new DualMotorSliderSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
this.gamepad1 = gamepad1;
this.gamepad2 = gamepad2;
currentGamepad1 = new Gamepad();
@ -50,6 +60,9 @@ public class CometBotAutoDevelopment {
public void init() {
dualSlides.init();
claw.init();
wrist.init();
arm.init();
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
@ -60,25 +73,78 @@ public class CometBotAutoDevelopment {
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
/*
Check if dpad_up/down is being pressed for slides
*/
//slides
dualSlides.update();
dualSlidesToLowBucketPosition();
dualSlidesToHighBucketPosition();
dualSlidesToFloorPosition();
//arm
armToParkPosition();
armAndWristToFloor();
//claw
openClaw();
openThumb();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
private void openClaw(){
if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
claw.switchState();
}
}
private void openThumb(){
if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
claw.switchState();
}
}
private void armAndWristToFloor(){
if(currentGamepad2.a && !previousGamepad2.a){
wrist.switchState();
}
if(currentGamepad2.a && !previousGamepad2.a){
double increment = 0.6375 - arm.getPosition();
for(int i = 0; i < 3; i ++){
arm.setPosition(arm.getPosition() + increment);
try {
Thread.sleep(50);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
arm.toFloorPosition();
}
}
private void armToBucketPosition(){
if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
arm.toBucketPosition();
wrist.toBucketPosition();
}
}
private void armToParkPosition(){
if(currentGamepad2.x && !previousGamepad2.x){
arm.toParkPosition();
private void dualSlidesToHighBucketPosition() {
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
dualSlides.toHighBucketPosition();
}
}
private void dualSlidesToHighBucketPosition() {
if (currentGamepad2.y && !previousGamepad2.y) {
dualSlides.toHighBucketPosition();
}
}
private void dualSlidesToFloorPosition() {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
dualSlides.toHighBucketPosition();
}
}
private void dualSlidesToLowBucketPosition() {
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
if (currentGamepad2.b && !previousGamepad2.b) {
dualSlides.toLowBucketPosition();
}
}

136
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java Normal file
View File

@ -0,0 +1,136 @@
package org.firstinspires.ftc.teamcode.states;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Blue Basket Auto States", group = "Competition")
public class BlueBasketAutoStates extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(37.000, 108.000, Point.CARTESIAN),
new Point(73.286, 111.536, Point.CARTESIAN),
new Point(67.821, 120.536, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(67.821, 120.536, Point.CARTESIAN),
new Point(28.000, 121.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(28.000, 121.500, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierCurve(
new Point(18.000, 130.179, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN),
new Point(68.700, 130.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(68.700, 130.500, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(63.804, 135.321, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(53.036, 135.161, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(72.300, 97.400, Point.CARTESIAN)
)
)
.addPath(
// Line 11
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(83.250, 95.464, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}