Getting parking to work again as path 6b. Original path during competition was not working properly.
This commit is contained in:
robotics1
@ -0,0 +1,333 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.cometbots.paths;
|
||||||
|
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.acmerobotics.roadrunner.Action;
|
||||||
|
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.pedroPathing.follower.Follower;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
|
||||||
|
|
||||||
|
@Autonomous(name = "Copy of Auto Competition V2", group = "A")
|
||||||
|
public class CopyofCometBotDriveV2 extends OpMode {
|
||||||
|
private Follower follower;
|
||||||
|
private int state;
|
||||||
|
|
||||||
|
private HighBasketPath1 path1;
|
||||||
|
private HighBasketPath2 path2;
|
||||||
|
private HighBasketPath3 path3;
|
||||||
|
private HighBasketPath4 path4;
|
||||||
|
private HighBasketPath5 path5;
|
||||||
|
//private HighBasketPath6 path6;
|
||||||
|
private HighBasketPath6b path6;
|
||||||
|
|
||||||
|
|
||||||
|
//private CometBotTeleopCompetition comp;
|
||||||
|
private static ElapsedTime runtime;
|
||||||
|
private static boolean initalized = false;
|
||||||
|
private static boolean followingPath = false;
|
||||||
|
private DualMotorSliderSubsystem lift;
|
||||||
|
private ClawSubsystem claw;
|
||||||
|
private WristSubsystem wrist;
|
||||||
|
private ArmSubsystem arm;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
path1 = new HighBasketPath1();
|
||||||
|
path2 = new HighBasketPath2();
|
||||||
|
path3 = new HighBasketPath3();
|
||||||
|
path4 = new HighBasketPath4();
|
||||||
|
path5 = new HighBasketPath5();
|
||||||
|
path6 = new HighBasketPath6b();
|
||||||
|
//path6 = new HighBasketPath6();
|
||||||
|
lift = new DualMotorSliderSubsystem(hardwareMap);
|
||||||
|
arm = new ArmSubsystem(hardwareMap);
|
||||||
|
wrist = new WristSubsystem(hardwareMap);
|
||||||
|
claw = new ClawSubsystem(hardwareMap);
|
||||||
|
|
||||||
|
|
||||||
|
lift.init();
|
||||||
|
arm.initAuto();
|
||||||
|
wrist.InitAuto();
|
||||||
|
claw.init();
|
||||||
|
|
||||||
|
//comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
|
||||||
|
// comp.initCloseClaw();
|
||||||
|
runtime = new ElapsedTime();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public void loop() {
|
||||||
|
telemetry.addData("state", state);
|
||||||
|
// telemetry.addData("arm", arm);
|
||||||
|
// telemetry.addData("lift", lift);
|
||||||
|
// telemetry.addData("wrist", wrist);
|
||||||
|
// telemetry.addData("claw", claw);
|
||||||
|
telemetry.addData("followingPath", followingPath);
|
||||||
|
telemetry.addData("busy", follower.isBusy());
|
||||||
|
|
||||||
|
if (runtime != null) {
|
||||||
|
telemetry.addData("Runtime (seconds)", runtime.seconds());
|
||||||
|
}
|
||||||
|
switch (state) {
|
||||||
|
case 0:
|
||||||
|
|
||||||
|
runtime.reset();
|
||||||
|
moveToPathOneAndHighBucket();
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
doArmThing();
|
||||||
|
state = 2;
|
||||||
|
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:
|
||||||
|
moveToBasketPath5();
|
||||||
|
break;
|
||||||
|
case 9:
|
||||||
|
theArmThingAgain();
|
||||||
|
break;
|
||||||
|
case 10:
|
||||||
|
moveToParkPath6();
|
||||||
|
break;
|
||||||
|
case 11:
|
||||||
|
moveToPark();
|
||||||
|
break;
|
||||||
|
case 99:
|
||||||
|
wrist.toFloorPosition();
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
telemetry.addLine("default");
|
||||||
|
|
||||||
|
}
|
||||||
|
telemetry.update();
|
||||||
|
lift.update();
|
||||||
|
follower.update();
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void moveToPark() {
|
||||||
|
if (runtime.seconds() > 26) {
|
||||||
|
path6.moveToBasketPath6(follower);
|
||||||
|
state = 99;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void moveToPathOneAndHighBucket() {
|
||||||
|
path1.moveToPath1(follower);
|
||||||
|
state = 1;
|
||||||
|
}
|
||||||
|
private void moveToBasketPath5() {
|
||||||
|
if(runtime.seconds() > 19){
|
||||||
|
path5.moveToBasketPath5(follower);
|
||||||
|
lift.toHighBucketReverseDrop();
|
||||||
|
wrist.toTravelPosition();
|
||||||
|
state = 9;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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() {
|
||||||
|
|
||||||
|
claw.thumbOutOfTheWay();
|
||||||
|
wrist.toTravelPosition();
|
||||||
|
lift.toHighBucketReverseDrop();
|
||||||
|
//arm.toReverseBucket();
|
||||||
|
//wrist.toReverseBucket();
|
||||||
|
//claw.openClaw();
|
||||||
|
//wrist.toFloorPosition();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private void theArmThing() {
|
||||||
|
if(lift.getFixedPosition() >= 4280){
|
||||||
|
arm.toReverseBucket();
|
||||||
|
if(runtime.seconds() > 12) {
|
||||||
|
wrist.toReverseBucket();
|
||||||
|
}
|
||||||
|
if(runtime.seconds() > 12.75) {
|
||||||
|
claw.autoOpenClaw();
|
||||||
|
state = 6;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void theArmThingAgain() {
|
||||||
|
if(lift.getFixedPosition() >= 4280){
|
||||||
|
arm.toReverseBucket();
|
||||||
|
if(runtime.seconds() > 22) {
|
||||||
|
wrist.toReverseBucket();
|
||||||
|
}
|
||||||
|
if(runtime.seconds() > 23.00) {
|
||||||
|
claw.autoOpenClaw();
|
||||||
|
state = 10;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private void moveToPathTwoAndPickSampleUp() {
|
||||||
|
if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
|
||||||
|
wrist.toReverseBucket();
|
||||||
|
}
|
||||||
|
if (runtime.seconds() > 3.5) {
|
||||||
|
claw.autoOpenClaw();
|
||||||
|
}
|
||||||
|
if (runtime.seconds() > 4) {
|
||||||
|
wrist.toTravelPosition();
|
||||||
|
lift.toFloorPosition();
|
||||||
|
path2.moveToPath2(follower);
|
||||||
|
state = 3;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
private void moveToBasketPath3() {
|
||||||
|
if (runtime.seconds() > 7.25) {
|
||||||
|
lift.toHighBucketReverseDrop();
|
||||||
|
path3.moveToBasketPath3(follower);
|
||||||
|
state = 5;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void moveToPickupAgainPath4() {
|
||||||
|
if (runtime.seconds() > 13) {
|
||||||
|
wrist.toTravelPosition();
|
||||||
|
}
|
||||||
|
if (runtime.seconds() > 14) {
|
||||||
|
lift.toFloorPosition();
|
||||||
|
path4.moveToPickupPath4(follower);
|
||||||
|
state = 7;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
private void moveToParkPath6() {
|
||||||
|
if (runtime.seconds() > 24.5) {
|
||||||
|
arm.toBucketPosition();
|
||||||
|
wrist.toTravelPosition();
|
||||||
|
}
|
||||||
|
if (runtime.seconds() > 25.) {
|
||||||
|
lift.toFloorPosition();
|
||||||
|
claw.closeClaw();
|
||||||
|
state = 11;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//
|
||||||
|
// 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() {
|
||||||
|
wrist.toPickupPosition();
|
||||||
|
arm.toFloorPosition();
|
||||||
|
if (runtime.seconds() > 7){
|
||||||
|
claw.closeClaw();
|
||||||
|
state = 4;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void doPickUpThingAgain() {
|
||||||
|
wrist.toPickupPosition();
|
||||||
|
arm.toFloorPosition();
|
||||||
|
if (runtime.seconds() > 18){
|
||||||
|
claw.closeClaw();
|
||||||
|
state = 8;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,44 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.cometbots.paths;
|
||||||
|
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||||
|
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 HighBasketPath6b {
|
||||||
|
|
||||||
|
|
||||||
|
public void moveToBasketPath6(Follower robot) {
|
||||||
|
PathChain pathChain;
|
||||||
|
PathBuilder builder = new PathBuilder();
|
||||||
|
builder
|
||||||
|
.addPath(
|
||||||
|
// Line 1
|
||||||
|
new BezierLine(
|
||||||
|
new Point(27.000, 128.000, Point.CARTESIAN),
|
||||||
|
new Point(92.448, 125.671, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(270))
|
||||||
|
.addPath(
|
||||||
|
// Line 2
|
||||||
|
new BezierLine(
|
||||||
|
new Point(92.448, 125.671, Point.CARTESIAN),
|
||||||
|
new Point(93.000, 94.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setTangentHeadingInterpolation();
|
||||||
|
pathChain = builder.build();
|
||||||
|
robot.followPath(pathChain);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
Reference in New Issue
Block a user