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

Working SkyHookSubsystem

Browse Source
This commit is contained in:
robotics1
2024-11-14 18:04:24 -08:00
parent 2328788f0a
commit 707d7b0609
3 changed files with 196 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java Normal file
View File

@ -0,0 +1,122 @@
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 = "Pre Loaded Blue Basket Auto", group = "Competition")
public class PreLoadedBlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(7.875, 89.357);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(8.036, 89.196, Point.CARTESIAN),
new Point(10.125, 126.804, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(10.125, 126.804, Point.CARTESIAN),
new Point(37.607, 90.000, Point.CARTESIAN),
new Point(62.357, 119.893, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(62.357, 119.893, Point.CARTESIAN),
new Point(33.750, 112.500, Point.CARTESIAN),
new Point(15.107, 130.661, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierCurve(
new Point(15.107, 130.661, Point.CARTESIAN),
new Point(58.821, 103.018, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(15.107, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(15.107, 130.339, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(57.857, 133.071, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(57.857, 133.071, Point.CARTESIAN),
new Point(18.964, 134.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierCurve(
new Point(18.964, 134.679, Point.CARTESIAN),
new Point(84.536, 131.786, Point.CARTESIAN),
new Point(80.036, 96.429, 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);
}
}

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

@ -16,6 +16,7 @@ import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
public class CometBotTeleopCompetition {
@ -28,6 +29,7 @@ public class CometBotTeleopCompetition {
public ArmActionsSubsystem arm;
public WristActionsSubsystem wrist;
public LiftActionsSubsystem lift;
public SkyHookSubsystem hook;
/*
Controllers
@ -61,6 +63,8 @@ public class CometBotTeleopCompetition {
this.arm = new ArmActionsSubsystem(hardwareMap);
this.wrist = new WristActionsSubsystem(hardwareMap);
this.lift = new LiftActionsSubsystem(hardwareMap);
this.hook = new SkyHookSubsystem(hardwareMap);
this.GP1 = gp1;
this.GP2 = gp2;
this.telemetry = telemetry;
@ -74,6 +78,7 @@ public class CometBotTeleopCompetition {
public void init() {
this.motors.init();
this.hook.init();
this.claw.init();
this.arm.init();
this.wrist.init();
@ -96,6 +101,8 @@ public class CometBotTeleopCompetition {
this.clawControl();
this.decreaseMaxPower();
this.increaseMaxPower();
this.raiseSkyHook();
this.lowerSkyHook();
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY);
follower.update();
@ -282,5 +289,20 @@ public class CometBotTeleopCompetition {
}
}
}
public void raiseSkyHook() {
if (this.currentGP2.dpad_right && !previousGP2.dpad_right) {
hook.raiseHook();
}
}
public void lowerSkyHook() {
if (this.currentGP2.dpad_left && !previousGP2.dpad_left) {
hook.lowerHook();
}
}
}

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java Normal file
View File

@ -0,0 +1,52 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
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.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class SkyHookSubsystem {
public DcMotor hook;
public enum SkyHookState {
UP, DOWN, STOP
}
private SkyHookState skyHookState;
public SkyHookSubsystem(HardwareMap hardwareMap) {
hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER);
}
public void raiseHook(){
this.hook.setPower(1.00);
this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
this.setState(SkyHookState.UP);
}
public void lowerHook(){
this.hook.setPower(1.00);
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
this.setState(SkyHookState.DOWN);
}
public void init() {
hook.setPower(0);
hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
private void setState(SkyHookState liftState) {
this.skyHookState = liftState;
}
public SkyHookState getState() {
return this.skyHookState;
}
}