Scaffolding of Autonomous class
This commit is contained in:
@ -0,0 +1,35 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.runmodes.Auto;
|
||||
|
||||
|
||||
@Autonomous(name = "CometBot Auto", group = "Debug")
|
||||
public class CometBotAuto extends OpMode {
|
||||
public Auto auto;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
auto.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
auto.update();
|
||||
telemetry.addData("Arm State", auto.arm.getState());
|
||||
telemetry.addData("Arm Position", auto.arm.getPosition());
|
||||
telemetry.addData("Claw State", auto.claw.getState());
|
||||
telemetry.addData("Wrist State", auto.wrist.getState());
|
||||
telemetry.addData("Wrist Position", auto.wrist.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
}
|
||||
@ -9,9 +9,12 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.util.action.Action;
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
|
||||
@ -19,114 +22,62 @@ import org.firstinspires.ftc.teamcode.util.action.SleepAction;
|
||||
|
||||
public class Auto {
|
||||
|
||||
private RobotStart startLocation;
|
||||
|
||||
public ClawSubsystem claw;
|
||||
public ClawSubsystem.ClawState clawState;
|
||||
public ArmSubsystem arm;
|
||||
public WristSubsystem wrist;
|
||||
|
||||
public Timer clawTimer = new Timer();
|
||||
public Timer armTimer = new Timer();
|
||||
public Timer wristTimer = new Timer();
|
||||
|
||||
public Follower follower;
|
||||
public Telemetry telemetry;
|
||||
|
||||
public RunAction transfer;
|
||||
public Path preload, element1, score1, element2, score2, element3, score3, park;
|
||||
private Pose startPose, preloadPose, element1Pose, element1ControlPose, element2Pose, element2ControlPose, element3Pose, element3ControlPose, elementScorePose, parkControlPose, parkPose;
|
||||
|
||||
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, boolean isBlue, boolean isBucket) {
|
||||
claw = new ClawSubsystem(hardwareMap, clawState);
|
||||
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
|
||||
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
|
||||
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
|
||||
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
|
||||
|
||||
this.follower = follower;
|
||||
this.telemetry = telemetry;
|
||||
|
||||
startLocation = isBlue ? (isBucket ? RobotStart.BLUE_BUCKET : RobotStart.BLUE_OBSERVATION) : (isBucket ? RobotStart.RED_BUCKET : RobotStart.RED_OBSERVATION);
|
||||
|
||||
createPoses();
|
||||
buildPaths();
|
||||
|
||||
transfer = new RunAction(this::transfer);
|
||||
init();
|
||||
}
|
||||
|
||||
public void init_loop() {
|
||||
}
|
||||
|
||||
public void createPoses() {
|
||||
switch (startLocation) {
|
||||
case BLUE_BUCKET:
|
||||
startPose = blueBucketStartPose;
|
||||
preloadPose = blueBucketPreloadPose;
|
||||
element1ControlPose = blueBucketLeftSampleControlPose;
|
||||
element1Pose = blueBucketLeftSamplePose;
|
||||
element2ControlPose = blueBucketMidSampleControlPose;
|
||||
element2Pose = blueBucketMidSamplePose;
|
||||
element3ControlPose = blueBucketRightSampleControlPose;
|
||||
element3Pose = blueBucketRightSamplePose;
|
||||
elementScorePose = blueBucketScorePose;
|
||||
parkControlPose = blueBucketParkControlPose;
|
||||
parkPose = blueBucketParkPose;
|
||||
break;
|
||||
|
||||
case BLUE_OBSERVATION:
|
||||
startPose = blueObservationStartPose;
|
||||
//parkPose = blueObservationPark;
|
||||
break;
|
||||
|
||||
case RED_BUCKET:
|
||||
startPose = redBucketStartPose;
|
||||
//parkPose = redBucketPark;
|
||||
break;
|
||||
|
||||
case RED_OBSERVATION:
|
||||
startPose = redObservationStartPose;
|
||||
//parkPose = redObservationPark;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void buildPaths() {
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
preload = new Path(new BezierLine(new Point(startPose), new Point(preloadPose)));
|
||||
preload.setLinearHeadingInterpolation(startPose.getHeading(), preloadPose.getHeading());
|
||||
|
||||
element1 = new Path(new BezierCurve(new Point(preloadPose), new Point(element1ControlPose), new Point(element1Pose)));
|
||||
element1.setLinearHeadingInterpolation(preloadPose.getHeading(), element1Pose.getHeading());
|
||||
|
||||
score1 = new Path(new BezierLine(new Point(element1Pose), new Point(elementScorePose)));
|
||||
score1.setLinearHeadingInterpolation(element1Pose.getHeading(), elementScorePose.getHeading());
|
||||
|
||||
element2 = new Path(new BezierCurve(new Point(element1Pose), new Point(element2ControlPose), new Point(element2Pose)));
|
||||
element2.setLinearHeadingInterpolation(element1Pose.getHeading(), element2Pose.getHeading());
|
||||
|
||||
score2 = new Path(new BezierLine(new Point(element2Pose), new Point(elementScorePose)));
|
||||
score2.setLinearHeadingInterpolation(element2Pose.getHeading(), elementScorePose.getHeading());
|
||||
|
||||
element3 = new Path(new BezierCurve(new Point(element2Pose), new Point(element3ControlPose), new Point(element3Pose)));
|
||||
element3.setLinearHeadingInterpolation(element2Pose.getHeading(), element3Pose.getHeading());
|
||||
|
||||
score3 = new Path(new BezierLine(new Point(element3Pose), new Point(elementScorePose)));
|
||||
score3.setLinearHeadingInterpolation(element3Pose.getHeading(), elementScorePose.getHeading());
|
||||
|
||||
park = new Path(new BezierCurve(new Point(elementScorePose), new Point(parkControlPose), new Point(parkPose)));
|
||||
park.setLinearHeadingInterpolation(elementScorePose.getHeading(), parkPose.getHeading());
|
||||
public void init() {
|
||||
claw.init();
|
||||
arm.init();
|
||||
wrist.init();
|
||||
}
|
||||
|
||||
public void start() {
|
||||
clawTimer.resetTimer();
|
||||
armTimer.resetTimer();
|
||||
wristTimer.resetTimer();
|
||||
|
||||
claw.start();
|
||||
//lift.start();
|
||||
//extend.start();
|
||||
//intake.start();
|
||||
//box.start();
|
||||
arm.start();
|
||||
wrist.start();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
follower.update();
|
||||
//lift.updatePIDF();
|
||||
//extend.updatePIDF();
|
||||
//follower.update();
|
||||
if (clawTimer.getElapsedTimeSeconds() > 2) {
|
||||
claw.openClaw();
|
||||
}
|
||||
if (armTimer.getElapsedTimeSeconds() > 4) {
|
||||
arm.engageArm();
|
||||
}
|
||||
if (clawTimer.getElapsedTimeSeconds() > 6) {
|
||||
claw.closeClaw();
|
||||
}
|
||||
if (armTimer.getElapsedTimeSeconds() > 8) {
|
||||
arm.bucketArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
if (clawTimer.getElapsedTimeSeconds() > 10) {
|
||||
claw.openClaw();
|
||||
}
|
||||
}
|
||||
|
||||
private Action transfer() {
|
||||
return new SequentialAction(
|
||||
new SleepAction(1)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user