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

competition code updates for blue basket auto. Move to blue basket, put block in basket, park.

Browse Source
This commit is contained in:
robotics1
2024-12-13 20:39:49 -08:00
parent 2720d596b5
commit 9adcba68c9
12 changed files with 139 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java
View File

@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.SleepAction;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@ -9,6 +10,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
import org.firstinspires.ftc.teamcode.configs.RobotConstants;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
@ -16,28 +18,39 @@ 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;
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;
@Disabled
@Autonomous(name = "Auto Test", group = "Dev")
@Autonomous(name = "Auto Test Competition", group = "Dev")
public class BlueBasketAuto extends OpMode {
private Follower follower;
private int state;
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private AutoPark pathPark;
private CometBotTeleopCompetition comp;
private ElapsedTime runtime;
private LiftActionsSubsystem liftActionsSubsystem;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
pathPark = new AutoPark();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
runtime = new ElapsedTime();
state = 0;
}
@ -45,18 +58,41 @@ public class BlueBasketAuto extends OpMode {
public void loop() {
switch(state) {
case 0:
telemetry.addData("case0", "case0");
path1.moveToPath1(follower);
state = 1;
runtime.reset();
case 1:
if (runtime.seconds() > 5) {
path2.moveToPath1(follower);
telemetry.addData("case1", "case1");
new SleepAction(.5);
comp.highBucketDropAuto();
state = 2;
}
case 2:
if (runtime.seconds() > 15) {
telemetry.addData("case2", "case2");
new SleepAction(.5);
path2.moveToPath1(follower);
//For next time, add encoder control to skyhook and extend here
//comp.moveSkyHook();
//pathPark.moveToPark(follower);
state = 5;
}
default:
System.out.println("default");
//System.out.println("default");
//telemetry.addData("default", "default");
//telemetry.update();
}
telemetry.update();
follower.update();
follower.telemetryDebug(telemetry);
//follower.telemetryDebug(telemetry);
}
}

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java
View File

@ -1,55 +0,0 @@
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.Disabled;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Disabled
@Autonomous(name = "DKJBFWOFHBOUW EHBF", group = "Dev")
public class BlueBasketAutoWithDrop3 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(32.000, 123.000, Point.CARTESIAN),
new Point(16.000, 120.000, Point.CARTESIAN),
new Point(16.000, 128.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java
View File

@ -1,55 +0,0 @@
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.Disabled;
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;
@Disabled
@Autonomous(name = "rndnohsdnhngph", group = "Dev")
public class BlueBasketAutoWithDrop4 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(16.000, 128.000, Point.CARTESIAN),
new Point(32.000, 133.500, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java
View File

@ -1,56 +0,0 @@
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.Disabled;
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;
@Disabled
@Autonomous(name = "rboeruhbgoeruhbg", group = "Dev")
public class BlueBasketAutoWithDrop5 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path = follower.pathBuilder()
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(32.000, 133.500, Point.CARTESIAN),
new Point(16.000, 128.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java
View File

@ -1,53 +0,0 @@
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.Disabled;
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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Disabled
@Autonomous(name = "piqeripgqreipbripbipb", group = "Dev")
public class BlueBasketAutoWithDrop6 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path = follower.pathBuilder()
.addPath(
// Line 6
new BezierCurve(
new Point(16.000, 128.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

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

@ -113,8 +113,8 @@ public class CometBotTeleopCompetition {
this.clawControl();
this.decreaseMaxPower();
this.increaseMaxPower();
this.raiseSkyHook();
//this.lowerSkyHook();
this.moveSkyHook();
Actions.runBlocking(this.lift.toFloorPosition());
@ -199,7 +199,24 @@ public class CometBotTeleopCompetition {
new SleepAction(.5)
));
}
public void highBucketDropAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toZeroPosition(),
new SleepAction(.5)
));
}
/*
Type: PS4 / Logitech
Controller: 2
@ -268,26 +285,17 @@ public class CometBotTeleopCompetition {
}
}
public void lowerSkyHook() {
if (this.currentGP2.dpad_down) {
hook.lowerHook(1.00);
}
else{
hook.lowerHook(0.00);
}
}
public void raiseSkyHook() {
public void moveSkyHook() {
if (this.currentGP2.dpad_down) {
hook.lowerHook(1.00);
hook.moveSkyHook(-1.00);
}
else if (this.currentGP2.dpad_up) {
hook.raiseHook(1.00);
hook.moveSkyHook(1.00);
}
else{
hook.raiseHook(0.00);
hook.lowerHook(0.00);
hook.moveSkyHook(0.00);
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View File

@ -929,31 +929,31 @@ public class Follower {
* method will use to output the debug data.
*/
public void telemetryDebug(MultipleTelemetry telemetry) {
telemetry.addData("follower busy", isBusy());
telemetry.addData("heading error", headingError);
telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
telemetry.addData("corrective vector heading", correctiveVector.getTheta());
telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
telemetry.addData("translational error direction", getTranslationalError().getTheta());
telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
telemetry.addData("translational vector heading", translationalVector.getMagnitude());
telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
telemetry.addData("drive error", driveError);
telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
telemetry.addData("drive vector heading", driveVector.getTheta());
telemetry.addData("x", getPose().getX());
telemetry.addData("y", getPose().getY());
telemetry.addData("heading", getPose().getHeading());
telemetry.addData("total heading", poseUpdater.getTotalHeading());
telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
telemetry.addData("velocity heading", getVelocity().getTheta());
driveKalmanFilter.debug(telemetry);
telemetry.update();
if (drawOnDashboard) {
Drawing.drawDebug(this);
}
// telemetry.addData("follower busy", isBusy());
// telemetry.addData("heading error", headingError);
// telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
// telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
// telemetry.addData("corrective vector heading", correctiveVector.getTheta());
// telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
// telemetry.addData("translational error direction", getTranslationalError().getTheta());
// telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
// telemetry.addData("translational vector heading", translationalVector.getMagnitude());
// telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
// telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
// telemetry.addData("drive error", driveError);
// telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
// telemetry.addData("drive vector heading", driveVector.getTheta());
// telemetry.addData("x", getPose().getX());
// telemetry.addData("y", getPose().getY());
// telemetry.addData("heading", getPose().getHeading());
// telemetry.addData("total heading", poseUpdater.getTotalHeading());
// telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
// telemetry.addData("velocity heading", getVelocity().getTheta());
// driveKalmanFilter.debug(telemetry);
// telemetry.update();
// if (drawOnDashboard) {
// Drawing.drawDebug(this);
// }
}
/**

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoPark.java Normal file
View File

@ -0,0 +1,36 @@
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.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 AutoPark {
public void moveToPark(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java
View File

@ -23,14 +23,13 @@ public class HighBasketAutoPath1 {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(16.000, 128.000, Point.CARTESIAN)
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
pathChain = builder.build();
robot.followPath(pathChain);
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java
View File

@ -22,12 +22,12 @@ public class HighBasketAutoPath2 {
.addPath(
// Line 2
new BezierCurve(
new Point(16.000, 128.000, Point.CARTESIAN),
new Point(16.000, 120.000, Point.CARTESIAN),
new Point(32.000, 123.000, Point.CARTESIAN)
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}

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

@ -72,6 +72,9 @@ public class LiftActionsSubsystem {
}
public Action toZeroPosition() {
return new MoveToPosition(0, LiftState.FLOOR);
}

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

@ -25,12 +25,8 @@ public class SkyHookSubsystem {
public void raiseHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
//this.setState(SkyHookState.UP);
}
public void lowerHook(double power){
public void moveSkyHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
//this.setState(SkyHookState.DOWN);