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

Working actions files sky hook and more

Browse Source
This commit is contained in:
robotics1
2024-12-10 16:48:14 -08:00
parent 1b6f8f9b62
commit 6695140d04
10 changed files with 281 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

195
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java Normal file
View File

@ -0,0 +1,195 @@
package org.firstinspires.ftc.teamcode;
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 = "Auto Test", group = "Dev")
public class BlueBasketAutoWithDrop extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(33.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(33.000, 65.000, Point.CARTESIAN),
new Point(31.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(31.000, 65.000, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 24.000, Point.CARTESIAN),
new Point(14.500, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierLine(
new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 1
new BezierCurve(
new Point(18.000, 24.000, Point.CARTESIAN),
new Point(18.000, 67.000, Point.CARTESIAN),
new Point(31.000, 67.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(33.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(33.000, 67.500, Point.CARTESIAN),
new Point(31.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(12.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierLine(
new Point(12.000, 12.000, Point.CARTESIAN),
new Point(20.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(20.000, 12.000, Point.CARTESIAN),
new Point(20.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 15
new BezierLine(
new Point(20.000, 24.000, Point.CARTESIAN),
new Point(14.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 16
new BezierCurve(
new Point(14.000, 24.000, Point.CARTESIAN),
new Point(14.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.addPath(
// Line 17
new BezierLine(
new Point(31.000, 70.000, Point.CARTESIAN),
new Point(33.000, 70.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 18
new BezierLine(
new Point(33.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

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

@ -18,14 +18,10 @@ public class NetAuto extends OpMode {
follower = new Follower(hardwareMap);
}
public Action autoLine1() {return new AutoLine1(follower);}
@Override
public void loop() {
follower.update();
Actions.runBlocking(
autoLine1()
);
follower.telemetryDebug(telemetry);
}
}

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

@ -10,10 +10,10 @@ public class PedroConstants {
/*
Motor configuration names
*/
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
public static final String BACK_LEFT_MOTOR = "Drive back lt";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
public static final String FRONT_LEFT_MOTOR = "front-left";
public static final String BACK_LEFT_MOTOR = "back-left";
public static final String FRONT_RIGHT_MOTOR = "front-right";
public static final String BACK_RIGHT_MOTOR = "back-right";
/*
Motor directions
@ -31,7 +31,7 @@ public class PedroConstants {
/*
Motor Max Power
*/
public static final double MAX_POWER = .50;
public static final double MAX_POWER = .675;
/*
IMU
@ -45,9 +45,9 @@ public class PedroConstants {
/*
Dead wheels
*/
public static final String LEFT_ENCODER = "encoder left";
public static final String RIGHT_ENCODER = "encoder right";
public static final String BACK_ENCODER = "encoder back";
public static final String RIGHT_ENCODER = "back-right";
public static final String BACK_ENCODER = "front-right";
public static final String LEFT_ENCODER = "front-left";
/*
Dead wheel directions

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

@ -99,9 +99,6 @@ public class CometBotAutoDevelopment {
this.telemetry.addData("Lift Position", this.lift.getPosition());
}
public Action autoLine1() {return new AutoLine1(follower);}
/*
Controller: 1

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
View File

@ -294,15 +294,15 @@ public class CometBotTeleopCompetition {
}
}
public void raiseSkyHook() {
public void lowerSkyHook() {
hook.raiseHook(currentGP2.left_trigger);
}
public void lowerSkyHook() {
hook.lowerHook(currentGP2.right_trigger);
public void raiseSkyHook() {
hook.lowerHook(currentGP2.right_trigger * 2.0);
}

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

@ -48,6 +48,8 @@ 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.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;
@ -60,22 +62,14 @@ public class SpecimenTest extends LinearOpMode {
private PathChain path;
private Follower robot;
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;
}
}
public AutoLine1 firstPath = new AutoLine1();
@Override
public void runOpMode() {
@ -89,16 +83,17 @@ public class SpecimenTest extends LinearOpMode {
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
Follower robot = new Follower(hardwareMap);
firstPath.moveToPath1(robot);
/*
Robot stuff
*/
robot.setStartingPose(startPose);
path = robot.pathBuilder() .addPath(
path = robot.pathBuilder().addPath(
// Line 1
new BezierLine(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(37.500, 72.000, Point.CARTESIAN)
new Point(37.500, 72.000, Point.CARTESIAN),
new Point(36.000, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
@ -115,7 +110,7 @@ public class SpecimenTest extends LinearOpMode {
wrist.init();
arm.init();
claw.init();
robot.setMaxPower(.75);
robot.setMaxPower(.40);
robot.followPath(path);
waitForStart();
@ -146,26 +141,19 @@ public class SpecimenTest extends LinearOpMode {
claw.switchState();
}
if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
robot.update();
Actions.runBlocking(
new SequentialAction(
wrist.toFloorPosition(),
lift.toHighRung(),
wrist.toSpeciemenBar(),
lift.dropToHighRung()
// reverseMoveToPath(robot),
// claw.openClaw(),
// wrist.toFloorPosition(),
// lift.toFloorPosition()
)
);
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
Actions.runBlocking(
new SequentialAction(
claw.openClaw(),
wrist.toFloorPosition(),
lift.toFloorPosition()
)
);
}
@ -185,10 +173,12 @@ public class SpecimenTest extends LinearOpMode {
lift.setPosition(lift.getPosition() - 25);
}
if (currentGamepad2.x && !previousGamepad2.x){
while(true) {
if (currentGamepad2.x && !previousGamepad2.x) {
while (true) {
robot.update();
if (!robot.isBusy()) {
robot.breakFollowing();
robot.startTeleopDrive();
break;
}
}
@ -204,4 +194,4 @@ public class SpecimenTest extends LinearOpMode {
telemetry.update();
}
}
}
}

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

@ -22,13 +22,13 @@ public class RobotConstants {
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToLowBucketPos = 2250;
public final static int liftToLowBucketPos = 2650;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;
public final static int liftToHighRungAttach = 1050;
public final static int liftToHighBucketPos = 3850;
public final static double liftPower = .625;
public final static int liftToHighBucketPos = 4900;
public final static double liftPower = 1;
}

61
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java
View File

@ -1,81 +1,36 @@
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.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;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine1 implements Action {
public class AutoLine1 {
private Follower actionRobot;
private PathChain pathChain;
private Pose startPose = new Pose(8,65);
public AutoLine1(Follower robot) {
this.actionRobot = robot;
this.actionRobot.setStartingPose(startPose);
private Pose startPose = new Pose(36, 72);
public void moveToPath1(Follower robot) {
PathChain pathChain;
robot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(36.000, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(36.000, 36.000, Point.CARTESIAN),
new Point(48.000, 36.000, Point.CARTESIAN)
new Point(37.500, 72.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 3
new BezierCurve(
new Point(48.000, 36.000, Point.CARTESIAN),
new Point(72.000, 30.000, Point.CARTESIAN),
new Point(57.000, 24.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 4
new BezierLine(
new Point(57.000, 24.000, Point.CARTESIAN),
new Point(10.000, 24.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
);
pathChain = builder.build();
this.actionRobot.followPath(this.pathChain);
robot.followPath(pathChain);
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
this.actionRobot.update();
return this.actionRobot.isBusy();
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java Normal file
View File

@ -0,0 +1,40 @@
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);
}
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java
View File

@ -19,7 +19,7 @@ public class SkyHookSubsystem {
private SkyHookState skyHookState;
public SkyHookSubsystem(HardwareMap hardwareMap) {
hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER);
hook = hardwareMap.get(DcMotor.class, "skyhook");
}
@ -34,6 +34,7 @@ public class SkyHookSubsystem {
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
this.setState(SkyHookState.DOWN);
}
public void init() {
hook.setPower(0);
hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);