Compare commits
2 Commits
fe39ffda11
...
1bf475772c
| Author | SHA1 | Date | |
|---|---|---|---|
| 1bf475772c | |||
| 46dac51c28 |
@ -1,195 +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.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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -252,6 +252,18 @@ public class CometBotTeleopCompetition {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void lowerSkyHook() {
|
||||||
|
if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down) {
|
||||||
|
hook.lowerHook(1.00);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void raiseSkyHook() {
|
||||||
|
if (this.currentGP2.dpad_up && !this.previousGP2.dpad_up) {
|
||||||
|
hook.raiseHook(1.00);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Type: PS4
|
Type: PS4
|
||||||
Controller: 2
|
Controller: 2
|
||||||
@ -294,18 +306,16 @@ public class CometBotTeleopCompetition {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public void lowerSkyHook() {
|
|
||||||
hook.raiseHook(currentGP2.left_trigger);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public void raiseSkyHook() {
|
|
||||||
hook.lowerHook(currentGP2.right_trigger * 2.0);
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user