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