9 Commits

Author SHA1 Message Date
f9b00bd558 Fine tuning to autonomous prior to switching to new robot 2025-01-20 09:00:55 -08:00
653c81ca7e 3 samples in high bucket
overtime
2025-01-16 16:12:15 -08:00
d41add449c 3 samples in high bucket
overtime
2025-01-16 16:11:19 -08:00
1c90a851e7 3 samples in high bucket
overtime
2025-01-16 15:59:16 -08:00
11df322ec1 Two Samples in bucket needs fine tuning 2025-01-14 17:28:22 -08:00
a39332954e Fixed floor position issue 2025-01-14 15:39:17 -08:00
8602dd878b Add V2 code that actually works, missing breaks in case statements (╯°□°)╯︵ ┻━┻ 2025-01-09 18:05:56 -08:00
13e13c21c4 Actions Code 2025-01-09 18:05:56 -08:00
793f75371e Actions Code 2025-01-09 18:05:56 -08:00
16 changed files with 609 additions and 101 deletions

View File

@ -22,6 +22,7 @@ import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@Autonomous(name = "Auto Test Competition", group = "Dev")
@ -32,7 +33,7 @@ public class BlueBasketAuto extends OpMode {
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private AutoPark pathPark;
private SkyHookSubsystem hook;
private CometBotTeleopCompetition comp;
private ElapsedTime runtime;
@ -49,6 +50,7 @@ public class BlueBasketAuto extends OpMode {
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
runtime = new ElapsedTime();
hook = new SkyHookSubsystem(hardwareMap);
state = 0;
}
@ -76,17 +78,32 @@ public class BlueBasketAuto extends OpMode {
if (runtime.seconds() > 15) {
telemetry.addData("case2", "case2");
new SleepAction(.5);
path2.moveToPath1(follower);
// new SleepAsction(.5);
//path2.moveToPath1(follower);
//For next time, add encoder control to skyhook and extend here
//comp.moveSkyHook();
//pathPark.moveToPark(follower);
state = 5;
state = 3;
}
case 3:
if (runtime.seconds() > 15) {
telemetry.addData("case3", "case3");
hook.toLevel1Position();
state = 4;
}
case 4:
if (runtime.seconds() > 15) {
telemetry.addData("case3", "case3");
hook.toLevel1Position();
state = 4;
}
default:
//System.out.println("default");
//telemetry.addData("default", "default");
//telemetry.update();

View File

@ -0,0 +1,290 @@
package org.firstinspires.ftc.teamcode;
import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
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;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@Autonomous(name = "Auto Test Competition V2", group = "Dev")
public class ComeBotDriveDevV2 extends OpMode {
private Follower follower;
private int state;
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private HighBasketAutoPath3 path3;
private HighBasketAutoPath4 path4;
private HighBasketAutoPath5 path5;
private HighBasketAutoPath6 path6;
private AutoPark pathPark;
private SkyHookSubsystem hook;
private CometBotTeleopCompetition comp;
private static ElapsedTime runtime;
private static boolean initalized = false;
private static boolean followingPath = false;
private LiftActionsSubsystem liftActionsSubsystem;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
path3 = new HighBasketAutoPath3();
path4 = new HighBasketAutoPath4();
path5 = new HighBasketAutoPath5();
path6 = new HighBasketAutoPath6();
pathPark = new AutoPark();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
hook = new SkyHookSubsystem(hardwareMap);
state = 0;
}
public void loop() {
telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
switch (state) {
case 0:
moveToPathOneAndHighBucket();
break;
case 1:
doArmThing();
break;
case 2:
moveToPathTwoAndPickSampleUp();
break;
case 3:
doPickUpThing();
break;
case 4:
moveToBasketPath3();
break;
case 5:
theArmThing();
break;
case 6:
moveToPickupAgainPath4();
break;
case 7:
doPickUpThingAgain();
break;
case 8:
moveToPickupAgainPath5();
break;
case 9:
theArmThingAgain();
break;
case 10:
//moveToParkPath6();
break;
case 11:
}
telemetry.update();
follower.update();
}
private void moveToPathOneAndHighBucket() {
if (!followingPath) {
runtime = new ElapsedTime();
path1.moveToPath1(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 4) {
state = 1;
followingPath = false;
}
}
}
public class SetStateAction implements Action {
private int value;
public SetStateAction(int value) {
this.value = value;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
state = value;
return false;
}
}
private Action setStateValue(int value) {
return new SetStateAction(value);
}
private void doArmThing() {
comp.highBucketDropAuto();
state = 2;
}
private void theArmThing() {
telemetry.addData("busy?", follower.isBusy());
telemetry.addData("end?", follower.atParametricEnd());
if (follower.atParametricEnd()){
follower.breakFollowing();
comp.highBucketDropAuto();
state = 6;
}
// follower.breakFollowing();
}
private void theArmThingAgain() {
follower.breakFollowing();
comp.highBucketDropAuto();
state = 10;
}
private void moveToPathTwoAndPickSampleUp() {
if (!followingPath) {
path2.moveToPath2(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
state = 3;
followingPath = false;
}
}
}
private void moveToPickupAgainPath4() {
if (!followingPath) {
path4.moveToPickupAgainPath4(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
state = 7;
followingPath = false;
}
}
}
private void moveToPickupAgainPath5() {
if (!followingPath) {
path5.moveToPickupAgainPath5(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
state = 9;
followingPath = false;
}
}
}
private void moveToParkPath6() {
if (!followingPath) {
path6.moveToParkPath6(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
state = 11;
followingPath = false;
}
}
}
private void moveToBasketPath3() {
if (!followingPath) {
path3.moveToBasketPath3(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
state = 5;
followingPath = false;
}
}
}
private void thePickUpAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.1),
comp.claw.openClaw(),
new SleepAction(.2),
comp.wrist.toPickupPosition(),
new SleepAction(.2),
comp.arm.toSubmarinePosition(),
new SleepAction(.5),
comp.claw.closeClaw(),
new SleepAction(.3),
comp.wrist.toFloorPosition(),
new SleepAction(.2),
comp.arm.toParkPosition(),
new SleepAction(.2)
));
}
private void thePickUp() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
comp.wrist.toPickupPosition(),
new SleepAction(.5),
comp.arm.toSubmarinePosition(),
new SleepAction(.7),
comp.claw.closeClaw(),
new SleepAction(.7),
comp.wrist.toFloorPosition(),
new SleepAction(.5),
comp.arm.toParkPosition(),
new SleepAction(.5)
));
}
private void doPickUpThing() {
follower.breakFollowing();
thePickUpAuto();
state = 4;
}
private void doPickUpThingAgain() {
follower.breakFollowing();
thePickUpAuto();
state = 8;
}
}

View File

@ -31,7 +31,7 @@ public class PedroConstants {
/*
Motor Max Power
*/
public static final double MAX_POWER = .675;
public static final double MAX_POWER = .8;
/*
IMU

View File

@ -105,7 +105,7 @@ public class CometBotTeleopCompetition {
this.currentGP1.copy(this.GP1);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.moveSkyHook();
this.toHighBucketScore();
this.toLowBucketScore();
this.toArmParkPosition();
@ -113,7 +113,6 @@ public class CometBotTeleopCompetition {
this.clawControl();
this.decreaseMaxPower();
this.increaseMaxPower();
this.moveSkyHook();
Actions.runBlocking(this.lift.toFloorPosition());
@ -158,6 +157,18 @@ public class CometBotTeleopCompetition {
this.follower.setMaxPower(this.currentPower);
}
}
public void moveSkyHook() {
if (this.currentGP2.dpad_down) {
hook.moveSkyHook(-1.00);
}
else if (this.currentGP2.dpad_up) {
hook.moveSkyHook(1.00);
}
else{
hook.moveSkyHook(0.00);
}
}
/*
Type: PS4 / Logitech
@ -201,20 +212,20 @@ public class CometBotTeleopCompetition {
}
public void highBucketDropAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
new SleepAction(.1),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
new SleepAction(.25),
this.arm.toBucketPosition(),
new SleepAction(.5),
new SleepAction(.25),
this.wrist.toBucketPosition(),
new SleepAction(.5),
new SleepAction(.25),
this.claw.openClaw(),
new SleepAction(.5),
new SleepAction(.25),
this.wrist.toFloorPosition(),
new SleepAction(.5),
new SleepAction(.25),
this.arm.toParkPosition(),
this.lift.toZeroPosition(),
new SleepAction(.5)
new SleepAction(.25)
));
}
/*
@ -287,20 +298,6 @@ public class CometBotTeleopCompetition {
public void moveSkyHook() {
if (this.currentGP2.dpad_down) {
hook.moveSkyHook(-1.00);
}
else if (this.currentGP2.dpad_up) {
hook.moveSkyHook(1.00);
}
else{
hook.moveSkyHook(0.00);
}
}
/*
Type: PS4

View File

@ -115,11 +115,13 @@ public class LiftWristArmTest extends LinearOpMode {
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.setPosition(lift.getPosition() + 175);
lift.setPosition(lift.getPosition() + 100);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.setPosition(lift.getPosition() - 25);
if (lift.getPosition() > 1000) {
lift.setPosition(lift.getPosition()-1000);
}
}

View File

@ -0,0 +1,88 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
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;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@TeleOp(name = "Skyhook Test", group = "Debug")
public class SkyhookTest extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
SkyHookSubsystem hook = new SkyHookSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad2 = new Gamepad();
Gamepad previousGamepad2 = new Gamepad();
hook.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
if (currentGamepad2.dpad_up) {
hook.moveSkyHook(-1.00);;
} else if (currentGamepad2.dpad_down) {
hook.moveSkyHook(1.00);
} else {
hook.moveSkyHook(0.00);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Skyhook Position", hook.getHookPosition());
telemetry.update();
}
}
}

View File

@ -49,7 +49,6 @@ 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.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;

View File

@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawClose = 0.8;
public final static double clawOpen = 0.05;
public final static double clawOpen = 0.15;
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
@ -20,7 +20,7 @@ public class RobotConstants {
public final static double wristSpeciemen = 0.1;
public final static int liftToFloorPos = 550;
public final static int liftToFloorPos = 500;
public final static int liftToLowBucketPos = 2650;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;

View File

@ -1,40 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine2 {
private PathChain pathChain;
private PathChain goToStore;
private Pose startPose = new Pose(37.5, 72);
public AutoLine2(Follower robot) {
robot.setStartingPose(startPose);
pathChain = robot.pathBuilder().addPath(
new BezierLine(
new Point(37.500, 72.000, Point.CARTESIAN),
new Point(36.000, 72.000, Point.CARTESIAN)
)
).setConstantHeadingInterpolation(Math.toRadians(0)).build();
robot.followPath(pathChain);
}
}

View File

@ -29,7 +29,7 @@ public class HighBasketAutoPath1 {
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}

View File

@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@ -15,19 +16,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/
public class HighBasketAutoPath2 {
public void moveToPath1(Follower robot) {
public void moveToPath2(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 2
new BezierCurve(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
new Point(25.500, 120.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}

View File

@ -0,0 +1,35 @@
package org.firstinspires.ftc.teamcode.subsystem;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath3 {
public void moveToBasketPath3(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(29.000, 120.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

View File

@ -0,0 +1,34 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath4 {
public void moveToPickupAgainPath4(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(25.000, 129.900, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

View File

@ -0,0 +1,34 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath5 {
public void moveToPickupAgainPath5(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(24.000, 131.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

View File

@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath6 {
public void moveToParkPath6(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270))
.addPath(
// Line 2
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(81.000, 125.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270))
.addPath(
// Line 3
new BezierLine(
new Point(81.000, 125.000, Point.CARTESIAN),
new Point(81.000, 100.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

View File

@ -6,45 +6,47 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import com.acmerobotics.roadrunner.Action;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
public class SkyHookSubsystem {
public DcMotor hook;
public enum SkyHookState {
UP, DOWN, STOP
}
private SkyHookState skyHookState;
public DcMotorEx hook;
public SkyHookSubsystem(HardwareMap hardwareMap) {
hook = hardwareMap.get(DcMotor.class, SKYHOOK_NAME);
hook = hardwareMap.get(DcMotorEx.class, SKYHOOK_NAME);
}
public void moveSkyHook(int position){
hook.setTargetPosition(position);
hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void moveSkyHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
//this.setState(SkyHookState.DOWN);
public void moveSkyHook(double power) {
hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
hook.setPower(power);
}
public void init() {
hook.setPower(0);
hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
hook.setTargetPosition(0);
}
private void setState(SkyHookState liftState) {
this.skyHookState = liftState;
public void toParkPosition() {
moveSkyHook(0);
}
public SkyHookState getState() {
return this.skyHookState;
public void toLevel1Position() {
moveSkyHook(1500);
}
public int getHookPosition() {
return hook.getCurrentPosition();
}
}