Commit competition code
This commit is contained in:
@ -7,7 +7,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.runmodes.Auto;
|
||||
|
||||
|
||||
@Autonomous(name = "CometBot Auto", group = "Debug")
|
||||
@Autonomous(name = "CometBot Auto", group = "Competition")
|
||||
public class CometBotAuto extends OpMode {
|
||||
public Auto auto;
|
||||
|
||||
|
||||
@ -1,31 +1,27 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
|
||||
import org.firstinspires.ftc.teamcode.runmodes.CometBotTeleopCompetition;
|
||||
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition;
|
||||
|
||||
@TeleOp(name="ComeBot Drive", group="Debug")
|
||||
@TeleOp(name = "ComeBot Drive", group = "Competition")
|
||||
@Disabled
|
||||
public class CometBotDrive extends OpMode {
|
||||
|
||||
private Teleop teleop;
|
||||
public CometBotTeleopCompetition runMode;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
teleop = new Teleop(hardwareMap,
|
||||
telemetry,
|
||||
new Follower(hardwareMap),
|
||||
gamepad1);
|
||||
teleop.start();
|
||||
this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
|
||||
this.runMode.init();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
teleop.update();
|
||||
this.runMode.update();
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,185 @@
|
||||
package org.firstinspires.ftc.teamcode.runmodes;
|
||||
|
||||
import com.acmerobotics.roadrunner.SequentialAction;
|
||||
import com.acmerobotics.roadrunner.SleepAction;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.states.FieldStates;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
|
||||
|
||||
public class CometBotTeleopCompetition {
|
||||
|
||||
/*
|
||||
Subsystems
|
||||
*/
|
||||
private MotorsSubsystem motors;
|
||||
public ClawActionsSubsystem claw;
|
||||
public ArmActionsSubsystem arm;
|
||||
public WristActionsSubsystem wrist;
|
||||
public LiftActionsSubsystem lift;
|
||||
|
||||
/*
|
||||
Controllers
|
||||
*/
|
||||
public Gamepad GP1;
|
||||
public Gamepad GP2;
|
||||
public Gamepad currentGP1;
|
||||
public Gamepad previousGP1;
|
||||
public Gamepad currentGP2;
|
||||
public Gamepad previousGP2;
|
||||
private Telemetry telemetry;
|
||||
public FieldStates fieldStates;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
|
||||
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
|
||||
this.claw = new ClawActionsSubsystem(hardwareMap);
|
||||
this.arm = new ArmActionsSubsystem(hardwareMap);
|
||||
this.wrist = new WristActionsSubsystem(hardwareMap);
|
||||
this.lift = new LiftActionsSubsystem(hardwareMap);
|
||||
this.GP1 = gp1;
|
||||
this.GP2 = gp2;
|
||||
this.telemetry = telemetry;
|
||||
this.currentGP1 = new Gamepad();
|
||||
this.currentGP2 = new Gamepad();
|
||||
this.previousGP1 = new Gamepad();
|
||||
this.previousGP2 = new Gamepad();
|
||||
this.fieldStates = new FieldStates();
|
||||
this.follower = new Follower(hardwareMap);
|
||||
}
|
||||
|
||||
public void init() {
|
||||
this.motors.init();
|
||||
this.claw.init();
|
||||
this.arm.init();
|
||||
this.wrist.init();
|
||||
this.lift.init();
|
||||
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
follower.setMaxPower(.75);
|
||||
follower.startTeleopDrive();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
this.previousGP1.copy(currentGP1);
|
||||
this.currentGP1.copy(this.GP1);
|
||||
this.previousGP2.copy(currentGP2);
|
||||
this.currentGP2.copy(this.GP2);
|
||||
|
||||
this.toHighBucketScore();
|
||||
this.toLowBucketScore();
|
||||
this.toArmParkPosition();
|
||||
this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
|
||||
this.clawControl();
|
||||
|
||||
// this.motors.calculateTrajectory(this.GP1);
|
||||
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
|
||||
follower.update();
|
||||
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
|
||||
this.telemetry.addData("Claw State", this.claw.getState());
|
||||
this.telemetry.addData("Claw Position", this.claw.getPosition());
|
||||
this.telemetry.addData("Wrist State", this.wrist.getState());
|
||||
this.telemetry.addData("Arm State", this.arm.getState());
|
||||
this.telemetry.addData("Lift State", this.lift.getState());
|
||||
this.telemetry.addData("Lift Position", this.lift.getPosition());
|
||||
}
|
||||
|
||||
/*
|
||||
Controller: 1
|
||||
Button: A
|
||||
Action: On button press, Arm hovers the floor with wrist parallel to arm
|
||||
*/
|
||||
public void toHighBucketScore() {
|
||||
if (this.currentGP1.triangle && !this.previousGP1.triangle) {
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
this.wrist.toFloorPosition(),
|
||||
this.arm.toParkPosition(),
|
||||
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.toFloorPosition()
|
||||
));
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
}
|
||||
}
|
||||
|
||||
public void toLowBucketScore() {
|
||||
if (this.currentGP1.circle && !this.previousGP1.circle) {
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
this.wrist.toFloorPosition(),
|
||||
this.arm.toParkPosition(),
|
||||
this.lift.toLowBucketPosition(),
|
||||
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.toFloorPosition()
|
||||
));
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
}
|
||||
}
|
||||
|
||||
public void clawControl() {
|
||||
if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
|
||||
this.claw.switchState();
|
||||
}
|
||||
}
|
||||
|
||||
public void toArmParkPosition() {
|
||||
if (this.currentGP1.square && !this.previousGP1.square) {
|
||||
Actions.runBlocking(this.arm.toParkPosition());
|
||||
}
|
||||
}
|
||||
|
||||
public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
|
||||
if (this.currentGP1.cross && !previousGP1.cross) {
|
||||
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
|
||||
Actions.runBlocking(
|
||||
new SequentialAction(
|
||||
this.arm.toSubmarinePosition(),
|
||||
this.wrist.toFloorPosition()
|
||||
)
|
||||
);
|
||||
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) {
|
||||
Actions.runBlocking(
|
||||
new SequentialAction(
|
||||
this.arm.toFloorPosition(),
|
||||
this.wrist.toFloorPosition()
|
||||
)
|
||||
);
|
||||
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) {
|
||||
Actions.runBlocking(
|
||||
new SequentialAction(
|
||||
this.arm.toSubmarinePosition(),
|
||||
this.wrist.toFloorPosition()
|
||||
)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user