Compare commits
27 Commits
branch-rc-
...
c9ffd4f061
| Author | SHA1 | Date | |
|---|---|---|---|
| c9ffd4f061 | |||
| 021dfa7222 | |||
| 233b177cf6 | |||
| 0ab402af0f | |||
| aa496b8237 | |||
| 66f3339e26 | |||
| ad0a8d3374 | |||
| dc71eb4317 | |||
| 94144780b8 | |||
| a362d2e004 | |||
| 2008c3cd88 | |||
| c5be3cd932 | |||
| 7d83b9c254 | |||
| 5f50d053c5 | |||
| 0cfb57c643 | |||
| 50db1f9175 | |||
| 552bb3e25a | |||
| c1076a832c | |||
| 7bf8b0c357 | |||
| adfab3e8af | |||
| 0f7ea50907 | |||
| 239f168540 | |||
| f2bcdcc55a | |||
| e08aac773d | |||
| 3950a83ac1 | |||
| 552ff3f339 | |||
| 4eef485dab |
@ -1,93 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning")
|
||||
public class AsherOrientBlue extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(9.757, 84.983, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
/*
|
||||
* Only update this path
|
||||
*/
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(20.500, 7.800, Point.CARTESIAN),
|
||||
new Point(20.500, 87.500, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(20.500, 87.500, Point.CARTESIAN),
|
||||
new Point(7.800, 87.500, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
/*
|
||||
* End of only update this path
|
||||
*/
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,133 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning")
|
||||
public class AsherPathBlueV1 extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(9.757, 84.983, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
/*
|
||||
* Only update this path
|
||||
*/
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(7.800, 87.5, Point.CARTESIAN),
|
||||
new Point(19.000, 116.000, Point.CARTESIAN),
|
||||
new Point(93.000, 118.000, Point.CARTESIAN),
|
||||
new Point(45.000, 115.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(45.000, 115.000, Point.CARTESIAN),
|
||||
new Point(14.000, 126.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierCurve(
|
||||
new Point(14.000, 126.000, Point.CARTESIAN),
|
||||
new Point(43.000, 112.500, Point.CARTESIAN),
|
||||
new Point(64.000, 92.000, Point.CARTESIAN),
|
||||
new Point(77.000, 117.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(77.000, 117.000, Point.CARTESIAN),
|
||||
new Point(20.000, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierCurve(
|
||||
new Point(20.000, 135.000, Point.CARTESIAN),
|
||||
new Point(113.000, 95.000, Point.CARTESIAN),
|
||||
new Point(69.000, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(69.000, 135.000, Point.CARTESIAN),
|
||||
new Point(20.500, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierCurve(
|
||||
new Point(20.500, 135.000, Point.CARTESIAN),
|
||||
new Point(101.500, 95.500, Point.CARTESIAN),
|
||||
new Point(72.500, 95.500, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
/*
|
||||
* End of only update this path
|
||||
*/
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,35 +0,0 @@
|
||||
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();
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,24 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
|
||||
|
||||
@TeleOp(name = "CometBot Auto", group = "Development")
|
||||
public class CometBotDevAuto extends OpMode {
|
||||
|
||||
public CometBotAutoDevelopment runMode;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2);
|
||||
this.runMode.init();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
this.runMode.update();
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
@ -1,31 +0,0 @@
|
||||
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;
|
||||
|
||||
@TeleOp(name="ComeBot Drive", group="Debug")
|
||||
@Disabled
|
||||
public class CometBotDrive extends OpMode {
|
||||
|
||||
private Teleop teleop;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
teleop = new Teleop(hardwareMap,
|
||||
telemetry,
|
||||
new Follower(hardwareMap),
|
||||
gamepad1);
|
||||
teleop.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
teleop.update();
|
||||
}
|
||||
}
|
||||
@ -1,97 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
|
||||
@TeleOp(name = "Dev Teleop Remix", group = "Debug")
|
||||
@Disabled
|
||||
public class DevTeleOpRemix extends OpMode {
|
||||
|
||||
public ClawSubsystem claw;
|
||||
public ArmSubsystem arm;
|
||||
public WristSubsystem wrist;
|
||||
public LiftSubsystem lift;
|
||||
public MotorsSubsystem motors;
|
||||
|
||||
public Gamepad currentGamepad1;
|
||||
public Gamepad previousGamepad1;
|
||||
public Gamepad currentGamepad2;
|
||||
public Gamepad previousGamepad2;
|
||||
|
||||
public double power = .6;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
|
||||
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
|
||||
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
|
||||
lift = new LiftSubsystem(hardwareMap);
|
||||
motors = new MotorsSubsystem(hardwareMap, telemetry, power);
|
||||
|
||||
claw.init();
|
||||
arm.init();
|
||||
wrist.init();
|
||||
lift.init();
|
||||
motors.init();
|
||||
|
||||
currentGamepad1 = new Gamepad();
|
||||
previousGamepad1 = new Gamepad();
|
||||
currentGamepad2 = new Gamepad();
|
||||
previousGamepad2 = new Gamepad();
|
||||
}
|
||||
|
||||
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
if (currentGamepad1.a && !previousGamepad1.a) {
|
||||
wrist.floorWrist();
|
||||
arm.engageArm();
|
||||
}
|
||||
}
|
||||
|
||||
public void thePickup(ClawSubsystem claw) {
|
||||
if (currentGamepad1.x && !previousGamepad1.x) {
|
||||
claw.switchState();
|
||||
}
|
||||
}
|
||||
|
||||
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
if (currentGamepad1.b && !previousGamepad1.b) {
|
||||
arm.parkArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
|
||||
if (currentGamepad1.y && !previousGamepad1.y) {
|
||||
lift.toLowBucket();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
previousGamepad2.copy(currentGamepad2);
|
||||
currentGamepad2.copy(gamepad2);
|
||||
|
||||
theDrop(arm, wrist);
|
||||
thePickup(claw);
|
||||
theLift(arm, wrist);
|
||||
theLowBucketScore(lift, wrist, arm);
|
||||
|
||||
motors.calculateTrajectory(gamepad1);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,107 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
|
||||
@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
|
||||
@Disabled
|
||||
public class DevTeleOpRemixDeux extends OpMode {
|
||||
|
||||
private Follower follower;
|
||||
|
||||
public ClawSubsystem claw;
|
||||
public ArmSubsystem arm;
|
||||
public WristSubsystem wrist;
|
||||
public LiftSubsystem lift;
|
||||
public MotorsSubsystem motors;
|
||||
|
||||
public Gamepad currentGamepad1;
|
||||
public Gamepad previousGamepad1;
|
||||
public Gamepad currentGamepad2;
|
||||
public Gamepad previousGamepad2;
|
||||
|
||||
public double power = .6;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
|
||||
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
|
||||
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
|
||||
motors = new MotorsSubsystem(hardwareMap, telemetry);
|
||||
lift = new LiftSubsystem(hardwareMap);
|
||||
|
||||
claw.init();
|
||||
arm.init();
|
||||
wrist.init();
|
||||
lift.init();
|
||||
motors.init();
|
||||
|
||||
currentGamepad1 = new Gamepad();
|
||||
previousGamepad1 = new Gamepad();
|
||||
currentGamepad2 = new Gamepad();
|
||||
previousGamepad2 = new Gamepad();
|
||||
|
||||
follower.setMaxPower(this.power);
|
||||
follower.startTeleopDrive();
|
||||
|
||||
}
|
||||
|
||||
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
if (currentGamepad1.a && !previousGamepad1.a) {
|
||||
wrist.floorWrist();
|
||||
arm.engageArm();
|
||||
}
|
||||
}
|
||||
|
||||
public void thePickup(ClawSubsystem claw) {
|
||||
if (currentGamepad1.x && !previousGamepad1.x) {
|
||||
claw.switchState();
|
||||
}
|
||||
}
|
||||
|
||||
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
if (currentGamepad1.b && !previousGamepad1.b) {
|
||||
arm.parkArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
|
||||
if (currentGamepad1.y && !previousGamepad1.y) {
|
||||
lift.toLowBucket();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
previousGamepad2.copy(currentGamepad2);
|
||||
currentGamepad2.copy(gamepad2);
|
||||
|
||||
theDrop(arm, wrist);
|
||||
thePickup(claw);
|
||||
theLift(arm, wrist);
|
||||
theLowBucketScore(lift, wrist, arm);
|
||||
|
||||
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
|
||||
follower.update();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,196 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||
|
||||
import android.graphics.Point;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
|
||||
@TeleOp(name = "Dev Teleop", group = "Debug")
|
||||
public class DevTeleop extends OpMode {
|
||||
|
||||
public ClawSubsystem claw;
|
||||
public ArmSubsystem arm;
|
||||
public WristSubsystem wrist;
|
||||
public LiftSubsystem lift;
|
||||
public Gamepad currentGamepad1;
|
||||
public Gamepad previousGamepad1;
|
||||
public Gamepad currentGamepad2;
|
||||
public Gamepad previousGamepad2;
|
||||
public DcMotor frontLeftMotor;
|
||||
public DcMotor backLeftMotor;
|
||||
public DcMotor frontRightMotor;
|
||||
public DcMotor backRightMotor;
|
||||
|
||||
private double MAX_POWER = .45;
|
||||
@Override
|
||||
public void init() {
|
||||
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
|
||||
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
|
||||
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
|
||||
lift = new LiftSubsystem(hardwareMap);
|
||||
claw.init();
|
||||
arm.init();
|
||||
wrist.init();
|
||||
lift.init();
|
||||
|
||||
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
|
||||
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
|
||||
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
||||
|
||||
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
|
||||
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
|
||||
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||
|
||||
currentGamepad1 = new Gamepad();
|
||||
previousGamepad1 = new Gamepad();
|
||||
currentGamepad2 = new Gamepad();
|
||||
previousGamepad2 = new Gamepad();
|
||||
}
|
||||
|
||||
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
//pick up
|
||||
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
|
||||
wrist.floorWrist();
|
||||
arm.engageArm();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
public void thePickup(ClawSubsystem claw) {
|
||||
//claw open close
|
||||
if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
|
||||
claw.switchState();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
|
||||
|
||||
if (currentGamepad1.b && !previousGamepad1.b) {
|
||||
arm.parkArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
|
||||
}
|
||||
*/
|
||||
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
|
||||
//low bucket
|
||||
if (currentGamepad2.a && !previousGamepad2.a) {
|
||||
lift.toLowBucket();
|
||||
arm.bucketArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
|
||||
//high basket
|
||||
if (currentGamepad2.b && !previousGamepad2.b) {
|
||||
lift.toHighBucket();
|
||||
arm.bucketArm();
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
}
|
||||
|
||||
public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
|
||||
//
|
||||
if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){
|
||||
lift.toFloor();
|
||||
arm.bucketArm();
|
||||
wrist.floorWrist();
|
||||
}
|
||||
}
|
||||
|
||||
public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){
|
||||
if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){
|
||||
lift.toHover();
|
||||
wrist.floorWrist();
|
||||
arm.engageArm();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
previousGamepad2.copy(currentGamepad2);
|
||||
currentGamepad2.copy(gamepad2);
|
||||
|
||||
|
||||
theDrop(arm, wrist);
|
||||
thePickup(claw);
|
||||
// theLift(arm, wrist);
|
||||
theLowBucketScore(lift, wrist, arm);
|
||||
theHighBucketScore(lift, wrist, arm);
|
||||
theTravel(lift, arm, wrist);
|
||||
|
||||
double max;
|
||||
|
||||
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
||||
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
|
||||
double lateral = gamepad1.left_stick_x;
|
||||
double yaw = gamepad1.right_stick_x;
|
||||
|
||||
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
||||
// Set up a variable for each drive wheel to save the power level for telemetry.
|
||||
double leftFrontPower = axial + lateral + yaw;
|
||||
double rightFrontPower = axial - lateral - yaw;
|
||||
double leftBackPower = axial - lateral + yaw;
|
||||
double rightBackPower = axial + lateral - yaw;
|
||||
|
||||
// Normalize the values so no wheel power exceeds 100%
|
||||
// This ensures that the robot maintains the desired motion.
|
||||
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
|
||||
max = Math.max(max, Math.abs(leftBackPower));
|
||||
max = Math.max(max, Math.abs(rightBackPower));
|
||||
|
||||
if (max > 1.0) {
|
||||
leftFrontPower /= max;
|
||||
rightFrontPower /= max;
|
||||
leftBackPower /= max;
|
||||
rightBackPower /= max;
|
||||
}
|
||||
|
||||
|
||||
// Send calculated power to wheels
|
||||
frontLeftMotor.setPower(leftFrontPower * MAX_POWER);
|
||||
frontRightMotor.setPower(rightFrontPower * MAX_POWER);
|
||||
backLeftMotor.setPower(leftBackPower * MAX_POWER);
|
||||
backRightMotor.setPower(rightBackPower * MAX_POWER);
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||
telemetry.addData("Current Lift Position", lift.getPosition());
|
||||
telemetry.update();
|
||||
|
||||
}
|
||||
}
|
||||
@ -10,15 +10,12 @@ public class PedroConstants {
|
||||
/*
|
||||
Robot parameters
|
||||
*/
|
||||
// Turn localizer - -0.003
|
||||
|
||||
|
||||
// Robot motor configurations
|
||||
public static final String BRAIN_ROT = "Sikidi rizz 360 no teleop tf2 mama mia 2cool 4skool yasyasy yasyasyasyasyasyasyaysy ohio yes heh me is moar skeebeedee than u walked and got tripped on by your aunt my very educaded mother just served us nine what? just kydinfoiwfowefwofwioefoiejfeoiwjfomdsklfnslefknesfklnkfenfenkfeknfenkfeknfenkefnk";
|
||||
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
|
||||
public static final String BACK_LEFT_MOTOR = "Drive back lt";
|
||||
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
|
||||
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
|
||||
public static final String FRONT_LEFT_MOTOR = "front-left";
|
||||
public static final String BACK_LEFT_MOTOR = "back-left";
|
||||
public static final String FRONT_RIGHT_MOTOR = "front-right";
|
||||
public static final String BACK_RIGHT_MOTOR = "back-right";
|
||||
|
||||
// Robot motor direction
|
||||
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
|
||||
@ -26,43 +23,56 @@ public class PedroConstants {
|
||||
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
||||
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
||||
|
||||
/*
|
||||
Motor Max Power
|
||||
*/
|
||||
public static final double MAX_POWER = .75;
|
||||
|
||||
// Robot IMU configuration
|
||||
public static final String IMU = "imu";
|
||||
|
||||
// Robot IMU placement
|
||||
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
|
||||
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
|
||||
= RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
|
||||
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
|
||||
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
|
||||
= RevHubOrientationOnRobot.UsbFacingDirection.UP;
|
||||
|
||||
// Robot encoders
|
||||
public static final String LEFT_ENCODER = "encoder left";
|
||||
public static final String RIGHT_ENCODER = "encoder right";
|
||||
public static final String BACK_ENCODER = "encoder back";
|
||||
// NOTE: Encoders are plugged into the same ports as motors hence the weird names
|
||||
public static final String RIGHT_ENCODER = "front-left"; //2
|
||||
public static final String BACK_ENCODER = "front-right"; //1
|
||||
public static final String LEFT_ENCODER = "back-right"; //0
|
||||
|
||||
// Robot encoder direction
|
||||
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||
public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
|
||||
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||
|
||||
// Arm config
|
||||
public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
|
||||
public static final String LIFT_SLIDE_RIGHT_MOTOR = "lift-slide-right";
|
||||
public static final String Claw_Servo = "ClawServo";
|
||||
public static final String Wrist_Servo = "WristServo";
|
||||
public static final String Arm_Servo = "ArmServo";
|
||||
|
||||
/*
|
||||
Pedro's parameters
|
||||
*/
|
||||
|
||||
// The weight of the robot in Kilograms
|
||||
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
|
||||
public static final double ROBOT_WEIGHT_IN_KG = 9;
|
||||
|
||||
// Maximum velocity of the robot going forward
|
||||
public static final double ROBOT_SPEED_FORWARD = 51.4598;
|
||||
public static final double ROBOT_SPEED_FORWARD = 51.5;
|
||||
|
||||
// Maximum velocity of the robot going right
|
||||
public static final double ROBOT_SPEED_LATERAL = 28.7119;
|
||||
public static final double ROBOT_SPEED_LATERAL = 28.7;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving forward
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -59.805;
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -59.8;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672;
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -99.7;
|
||||
|
||||
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
|
||||
|
||||
@ -1,11 +1,9 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
@ -15,32 +13,16 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
|
||||
public class BlueBasketAuto extends OpMode {
|
||||
@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
|
||||
public class PreLoadedBlueBasketAuto extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(11.25, 95.75);
|
||||
private final Pose startPose = new Pose(7.875, 89.357);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
@ -53,90 +35,76 @@ public class BlueBasketAuto extends OpMode {
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(11.250, 95.750, Point.CARTESIAN),
|
||||
new Point(37.000, 108.000, Point.CARTESIAN)
|
||||
new Point(8.036, 89.196, Point.CARTESIAN),
|
||||
new Point(10.125, 126.804, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierCurve(
|
||||
new Point(37.000, 108.000, Point.CARTESIAN),
|
||||
new Point(73.286, 111.536, Point.CARTESIAN),
|
||||
new Point(67.821, 120.536, Point.CARTESIAN)
|
||||
new Point(10.125, 126.804, Point.CARTESIAN),
|
||||
new Point(37.607, 90.000, Point.CARTESIAN),
|
||||
new Point(62.357, 119.893, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(67.821, 120.536, Point.CARTESIAN),
|
||||
new Point(28.000, 121.500, Point.CARTESIAN)
|
||||
new BezierCurve(
|
||||
new Point(62.357, 119.893, Point.CARTESIAN),
|
||||
new Point(33.750, 112.500, Point.CARTESIAN),
|
||||
new Point(15.107, 130.661, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(28.000, 121.500, Point.CARTESIAN),
|
||||
new Point(18.000, 130.179, Point.CARTESIAN)
|
||||
new BezierCurve(
|
||||
new Point(15.107, 130.661, Point.CARTESIAN),
|
||||
new Point(58.821, 103.018, Point.CARTESIAN),
|
||||
new Point(59.625, 126.964, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierCurve(
|
||||
new Point(18.000, 130.179, Point.CARTESIAN),
|
||||
new Point(59.000, 102.500, Point.CARTESIAN),
|
||||
new Point(68.700, 130.500, Point.CARTESIAN)
|
||||
new BezierLine(
|
||||
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||
new Point(15.107, 130.339, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(68.700, 130.500, Point.CARTESIAN),
|
||||
new Point(18.000, 130.339, Point.CARTESIAN)
|
||||
new Point(15.107, 130.339, Point.CARTESIAN),
|
||||
new Point(59.625, 126.964, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierCurve(
|
||||
new Point(18.000, 130.339, Point.CARTESIAN),
|
||||
new Point(49.018, 121.179, Point.CARTESIAN),
|
||||
new Point(63.804, 135.321, Point.CARTESIAN)
|
||||
new BezierLine(
|
||||
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||
new Point(57.857, 133.071, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(63.804, 135.321, Point.CARTESIAN),
|
||||
new Point(53.036, 135.161, Point.CARTESIAN)
|
||||
new Point(57.857, 133.071, Point.CARTESIAN),
|
||||
new Point(18.964, 134.679, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(53.036, 135.161, Point.CARTESIAN),
|
||||
new Point(18.643, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(18.643, 135.000, Point.CARTESIAN),
|
||||
new Point(72.300, 97.400, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(18.643, 135.000, Point.CARTESIAN),
|
||||
new Point(83.250, 95.464, Point.CARTESIAN)
|
||||
new BezierCurve(
|
||||
new Point(18.964, 134.679, Point.CARTESIAN),
|
||||
new Point(84.536, 131.786, Point.CARTESIAN),
|
||||
new Point(80.036, 96.429, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
|
||||
@ -146,16 +114,9 @@ public class BlueBasketAuto extends OpMode {
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,135 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning")
|
||||
public class AsherPathV1 extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(10.0, 40, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
/*
|
||||
* Only update this path
|
||||
*/
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(9.757, 84.983, Point.CARTESIAN),
|
||||
new Point(33.000, 105.000, Point.CARTESIAN),
|
||||
new Point(80.000, 118.000, Point.CARTESIAN),
|
||||
new Point(55.000, 120.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierCurve(
|
||||
new Point(55.000, 120.000, Point.CARTESIAN),
|
||||
new Point(22.000, 106.000, Point.CARTESIAN),
|
||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierCurve(
|
||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
||||
new Point(75.000, 95.000, Point.CARTESIAN),
|
||||
new Point(112.000, 132.000, Point.CARTESIAN),
|
||||
new Point(61.000, 131.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(61.000, 131.000, Point.CARTESIAN),
|
||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierCurve(
|
||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
||||
new Point(100.000, 118.000, Point.CARTESIAN),
|
||||
new Point(103.000, 135.000, Point.CARTESIAN),
|
||||
new Point(61.000, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(61.000, 135.000, Point.CARTESIAN),
|
||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierCurve(
|
||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
||||
new Point(113.000, 95.000, Point.CARTESIAN),
|
||||
new Point(67.000, 95.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
/*
|
||||
* End of only update this path
|
||||
*/
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,79 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExample extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(0.0, 20.0, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.6);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(0.000, 20.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,106 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleFour extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(12,60, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.45);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(12.000, 60.000, Point.CARTESIAN),
|
||||
new Point(60.000, 60.000, Point.CARTESIAN),
|
||||
new Point(60.000, 12.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(60.000, 12.000, Point.CARTESIAN),
|
||||
new Point(40.000, 12.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierCurve(
|
||||
new Point(40.000, 12.000, Point.CARTESIAN),
|
||||
new Point(35.000, 35.000, Point.CARTESIAN),
|
||||
new Point(12.000, 35.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(12.000, 35.000, Point.CARTESIAN),
|
||||
new Point(12.000, 60.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,89 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleThree extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(10,45, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(10.000, 45.000, Point.CARTESIAN),
|
||||
new Point(45.000, 45.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(50.000, 20.000, Point.CARTESIAN),
|
||||
new Point(10.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,80 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleTwo extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(10.0, 45, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(10.000, 45.000, Point.CARTESIAN),
|
||||
new Point(45.000, 45.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -9,6 +9,7 @@ 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;
|
||||
@ -25,15 +26,15 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleSeason2025V1 extends OpMode {
|
||||
@Autonomous(name = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning")
|
||||
public class BlueNonBasketAuto extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(15.0, 35, 90);
|
||||
private final Pose startPose = new Pose(10.929, 55.446, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
@ -43,85 +44,93 @@ public class AutoExampleSeason2025V1 extends OpMode {
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.375);
|
||||
follower.setMaxPower(.45);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(15.000, 35.000, Point.CARTESIAN),
|
||||
new Point(60.000, 35.000, Point.CARTESIAN)
|
||||
new BezierCurve(
|
||||
new Point(10.929, 55.446, Point.CARTESIAN),
|
||||
new Point(42.429, 46.446, Point.CARTESIAN),
|
||||
new Point(36.321, 38.089, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(60.000, 35.000, Point.CARTESIAN),
|
||||
new Point(60.000, 25.000, Point.CARTESIAN)
|
||||
new Point(36.321, 38.089, Point.CARTESIAN),
|
||||
new Point(59.786, 36.643, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(60.000, 25.000, Point.CARTESIAN),
|
||||
new Point(15.000, 25.000, Point.CARTESIAN)
|
||||
new Point(59.786, 36.643, Point.CARTESIAN),
|
||||
new Point(59.304, 24.750, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(15.000, 25.000, Point.CARTESIAN),
|
||||
new Point(60.000, 25.000, Point.CARTESIAN)
|
||||
new Point(59.304, 24.750, Point.CARTESIAN),
|
||||
new Point(13.982, 23.946, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(60.000, 25.000, Point.CARTESIAN),
|
||||
new Point(60.000, 15.000, Point.CARTESIAN)
|
||||
new Point(13.982, 23.946, Point.CARTESIAN),
|
||||
new Point(59.464, 24.429, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(60.000, 15.000, Point.CARTESIAN),
|
||||
new Point(15.000, 15.000, Point.CARTESIAN)
|
||||
new Point(59.464, 24.429, Point.CARTESIAN),
|
||||
new Point(58.982, 15.268, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(15.000, 15.000, Point.CARTESIAN),
|
||||
new Point(60.000, 15.000, Point.CARTESIAN)
|
||||
new Point(58.982, 15.268, Point.CARTESIAN),
|
||||
new Point(13.821, 14.464, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(60.000, 15.000, Point.CARTESIAN),
|
||||
new Point(60.000, 8.000, Point.CARTESIAN)
|
||||
new Point(13.821, 14.464, Point.CARTESIAN),
|
||||
new Point(58.661, 13.500, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(60.000, 8.000, Point.CARTESIAN),
|
||||
new Point(15.000, 8.000, Point.CARTESIAN)
|
||||
new Point(58.661, 13.500, Point.CARTESIAN),
|
||||
new Point(58.339, 8.679, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
|
||||
follower.followPath(path);
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(58.339, 8.679, Point.CARTESIAN),
|
||||
new Point(14.625, 8.518, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
|
||||
follower.followPath(path, true);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
@ -135,7 +144,7 @@ public class AutoExampleSeason2025V1 extends OpMode {
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
follower.followPath(path, true);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
@ -1,246 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
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.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning")
|
||||
public class BluebAutoV1 extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(7.5, 72, 90);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.4);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(7.5, 72, Point.CARTESIAN),
|
||||
new Point(29.893, 38.250, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(29.893, 38.250, Point.CARTESIAN),
|
||||
new Point(65.250, 32.143, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(65.250, 32.143, Point.CARTESIAN),
|
||||
new Point(61.714, 24.429, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(61.714, 24.429, Point.CARTESIAN),
|
||||
new Point(13.821, 22.821, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(13.821, 22.821, Point.CARTESIAN),
|
||||
new Point(61.714, 24.429, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(61.714, 24.429, Point.CARTESIAN),
|
||||
new Point(60.750, 12.696, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(60.750, 12.696, Point.CARTESIAN),
|
||||
new Point(12.375, 13.179, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(12.375, 13.179, Point.CARTESIAN),
|
||||
new Point(60.750, 12.536, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 9
|
||||
new BezierLine(
|
||||
new Point(60.750, 12.536, Point.CARTESIAN),
|
||||
new Point(60.589, 9.321, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 10
|
||||
new BezierLine(
|
||||
new Point(60.589, 9.321, Point.CARTESIAN),
|
||||
new Point(12.536, 8.357, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 11
|
||||
new BezierLine(
|
||||
new Point(12.536, 8.357, Point.CARTESIAN),
|
||||
new Point(26.679, 8.679, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 12
|
||||
new BezierLine(
|
||||
new Point(26.679, 8.679, Point.CARTESIAN),
|
||||
new Point(22.821, 109.446, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 13
|
||||
new BezierLine(
|
||||
new Point(22.821, 109.446, Point.CARTESIAN),
|
||||
new Point(70.714, 109.446, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 14
|
||||
new BezierLine(
|
||||
new Point(70.714, 109.446, Point.CARTESIAN),
|
||||
new Point(71.036, 120.214, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 15
|
||||
new BezierLine(
|
||||
new Point(71.036, 120.214, Point.CARTESIAN),
|
||||
new Point(22.179, 120.214, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 16
|
||||
new BezierLine(
|
||||
new Point(22.179, 120.214, Point.CARTESIAN),
|
||||
new Point(11.089, 130.821, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 17
|
||||
new BezierLine(
|
||||
new Point(11.089, 130.821, Point.CARTESIAN),
|
||||
new Point(70.714, 112.018, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 18
|
||||
new BezierLine(
|
||||
new Point(70.714, 112.018, Point.CARTESIAN),
|
||||
new Point(70.714, 128.250, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 19
|
||||
new BezierLine(
|
||||
new Point(70.714, 128.250, Point.CARTESIAN),
|
||||
new Point(9.964, 130.018, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 20
|
||||
new BezierLine(
|
||||
new Point(9.964, 130.018, Point.CARTESIAN),
|
||||
new Point(70.554, 130.500, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 21
|
||||
new BezierLine(
|
||||
new Point(70.554, 130.500, Point.CARTESIAN),
|
||||
new Point(70.393, 135.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 22
|
||||
new BezierLine(
|
||||
new Point(70.393, 135.000, Point.CARTESIAN),
|
||||
new Point(13.821, 134.839, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
|
||||
follower.followPath(path);
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,4 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
public class BluenbAutov1 {
|
||||
}
|
||||
@ -0,0 +1,142 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
import com.acmerobotics.roadrunner.Action;
|
||||
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.MotorsSubsystem;
|
||||
|
||||
public class CometBotAutoDevelopment {
|
||||
|
||||
/*
|
||||
Subsystems
|
||||
*/
|
||||
private MotorsSubsystem motors;
|
||||
|
||||
/*
|
||||
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 boolean centricity = false;
|
||||
|
||||
private Follower follower;
|
||||
private HardwareMap hardwareMap;
|
||||
|
||||
public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
|
||||
this.motors = new MotorsSubsystem(hardwareMap, telemetry);
|
||||
this.GP1 = gp1;
|
||||
this.GP2 = gp2;
|
||||
this.hardwareMap = hardwareMap;
|
||||
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 class ZeroOutPower implements Action {
|
||||
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
follower = new Follower(hardwareMap);
|
||||
follower.setMaxPower(0);
|
||||
System.out.println("Running ZeroOutPower");
|
||||
return follower.isBusy();
|
||||
}
|
||||
}
|
||||
|
||||
public class ReturnToMaxPower implements Action {
|
||||
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
follower = new Follower(hardwareMap);
|
||||
follower.setMaxPower(MAX_POWER);
|
||||
follower.startTeleopDrive();
|
||||
System.out.println("Running ReturnToMaxPower");
|
||||
return follower.isBusy();
|
||||
}
|
||||
}
|
||||
|
||||
public Action zeroOutPower() {
|
||||
return new ZeroOutPower();
|
||||
}
|
||||
|
||||
public Action returnToMaxPower() {
|
||||
return new ReturnToMaxPower();
|
||||
}
|
||||
|
||||
public void init() {
|
||||
this.motors.init();
|
||||
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
follower.setMaxPower(MAX_POWER);
|
||||
follower.startTeleopDrive();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
this.previousGP1.copy(currentGP1);
|
||||
this.currentGP1.copy(this.GP1);
|
||||
this.previousGP2.copy(currentGP2);
|
||||
this.currentGP2.copy(this.GP2);
|
||||
|
||||
this.toFixMotorBlockingIssueFirstMethod();
|
||||
this.toFixMotorBlockingIssueSecondMethod();
|
||||
this.changeCentricity();
|
||||
|
||||
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity);
|
||||
follower.update();
|
||||
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
|
||||
}
|
||||
|
||||
public void changeCentricity() {
|
||||
if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
|
||||
this.centricity = !centricity;
|
||||
this.follower.breakFollowing();
|
||||
this.follower.startTeleopDrive();
|
||||
}
|
||||
}
|
||||
|
||||
public void toFixMotorBlockingIssueFirstMethod() {
|
||||
if (this.currentGP1.cross && !this.previousGP1.cross) {
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
this.zeroOutPower(),
|
||||
new SleepAction(3),
|
||||
this.returnToMaxPower()
|
||||
));
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
}
|
||||
}
|
||||
|
||||
public void toFixMotorBlockingIssueSecondMethod() {
|
||||
if (this.currentGP1.circle && !this.previousGP1.circle) {
|
||||
this.follower.breakFollowing();
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
new SleepAction(3)
|
||||
));
|
||||
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
this.follower.startTeleopDrive();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,172 +0,0 @@
|
||||
/* Copyright (c) 2022 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||
import org.firstinspires.ftc.teamcode.PedroConstants;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||
|
||||
/*
|
||||
* This OpMode shows how to use the new universal IMU interface. This
|
||||
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
|
||||
* on the robot with the name "imu".
|
||||
*
|
||||
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
|
||||
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
|
||||
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
|
||||
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
|
||||
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
|
||||
*
|
||||
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
|
||||
*
|
||||
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
|
||||
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
|
||||
*
|
||||
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
|
||||
* the alternative SensorIMUNonOrthogonal sample in this folder.
|
||||
*
|
||||
* This "Orthogonal" requirement means that:
|
||||
*
|
||||
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
|
||||
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
|
||||
*
|
||||
* 2) The USB ports can only be pointing in one of the same six directions:<br>
|
||||
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
|
||||
*
|
||||
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
|
||||
* logoFacingDirection<br>
|
||||
* usbFacingDirection
|
||||
*
|
||||
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
|
||||
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
|
||||
*
|
||||
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
|
||||
* to use those parameters.
|
||||
*/
|
||||
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
|
||||
@Disabled // Comment this out to add to the OpMode list
|
||||
public class SensorIMUOrthogonal extends LinearOpMode {
|
||||
// The IMU sensor object
|
||||
IMU imu;
|
||||
private Encoder leftEncoder;
|
||||
private Encoder rightEncoder;
|
||||
private Encoder strafeEncoder;
|
||||
|
||||
//----------------------------------------------------------------------------------------------
|
||||
// Main logic
|
||||
//----------------------------------------------------------------------------------------------
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
// Retrieve and initialize the IMU.
|
||||
// This sample expects the IMU to be in a REV Hub and named "imu".
|
||||
imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
|
||||
|
||||
// TODO: replace these with your encoder ports
|
||||
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
||||
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
|
||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
||||
|
||||
// TODO: reverse any encoders necessary
|
||||
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
|
||||
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
|
||||
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
|
||||
|
||||
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
|
||||
*
|
||||
* Two input parameters are required to fully specify the Orientation.
|
||||
* The first parameter specifies the direction the printed logo on the Hub is pointing.
|
||||
* The second parameter specifies the direction the USB connector on the Hub is pointing.
|
||||
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
|
||||
*
|
||||
* If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
|
||||
* RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
|
||||
*/
|
||||
|
||||
/* The next two lines define Hub orientation.
|
||||
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
|
||||
*
|
||||
* To Do: EDIT these two lines to match YOUR mounting configuration.
|
||||
*/
|
||||
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
|
||||
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
|
||||
|
||||
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
|
||||
|
||||
// Now initialize the IMU with this mounting orientation
|
||||
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
|
||||
imu.initialize(new IMU.Parameters(orientationOnRobot));
|
||||
|
||||
// Loop and update the dashboard
|
||||
while (!isStopRequested()) {
|
||||
|
||||
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
|
||||
|
||||
// Check to see if heading reset is requested
|
||||
if (gamepad1.y) {
|
||||
telemetry.addData("Yaw", "Resetting\n");
|
||||
imu.resetYaw();
|
||||
} else {
|
||||
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
|
||||
}
|
||||
|
||||
// Retrieve Rotational Angles and Velocities
|
||||
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
|
||||
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
|
||||
|
||||
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
|
||||
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
|
||||
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
|
||||
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
|
||||
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
|
||||
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -27,7 +27,7 @@
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots;
|
||||
package org.firstinspires.ftc.teamcode.cometbots.projects;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||
@ -1,95 +0,0 @@
|
||||
/* Copyright (c) 2021 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
|
||||
@TeleOp(name = "Arm Test", group = "Debug")
|
||||
public class ArmTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Arm
|
||||
*/
|
||||
ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
arm.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
arm.parkArm();
|
||||
}
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
arm.engageArm();
|
||||
}
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
arm.switchState();
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
arm.setPosition(arm.getPosition() - .05);
|
||||
}
|
||||
|
||||
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||
arm.setPosition(arm.getPosition() + .05);
|
||||
}
|
||||
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Arm State", arm.getState());
|
||||
telemetry.addData("Arm Position", arm.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,78 +0,0 @@
|
||||
/* Copyright (c) 2021 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
|
||||
@TeleOp(name = "Claw Test", group = "Debug")
|
||||
public class ClawTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Claw
|
||||
*/
|
||||
ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
claw.switchState();
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Claw State", claw.getState());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,124 +0,0 @@
|
||||
/* Copyright (c) 2021 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
@TeleOp(name = "Lift Raw Test", group = "Debug")
|
||||
public class LiftRawTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
private final int MIN_POINT = 0;
|
||||
private final int MAX_POINT = 3700;
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Lift
|
||||
*/
|
||||
DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor");
|
||||
liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
liftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
liftDrive.setPower(.5);
|
||||
|
||||
// Max position is 6800, safely setting to 6500
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
liftDrive.setTargetPosition(MIN_POINT);
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
|
||||
liftDrive.setTargetPosition(1500);
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
liftDrive.setTargetPosition(2750);
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
liftDrive.setTargetPosition(MAX_POINT);
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
int newPosition = liftDrive.getCurrentPosition() - 125;
|
||||
if (newPosition < MIN_POINT) {
|
||||
liftDrive.setTargetPosition(MIN_POINT);
|
||||
} else {
|
||||
liftDrive.setTargetPosition(newPosition);
|
||||
}
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||
int newPosition = liftDrive.getCurrentPosition() + 125;
|
||||
if (newPosition > MAX_POINT) {
|
||||
liftDrive.setTargetPosition(MAX_POINT);
|
||||
} else {
|
||||
liftDrive.setTargetPosition(newPosition);
|
||||
}
|
||||
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,98 +0,0 @@
|
||||
/* Copyright (c) 2021 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
|
||||
|
||||
@TeleOp(name = "Lift Test", group = "Debug")
|
||||
public class LiftTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
private final int MIN_POINT = 0;
|
||||
// 2000 ~ 2500
|
||||
|
||||
// 3750 max
|
||||
private final int MAX_POINT = 6500;
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Lift
|
||||
*/
|
||||
LiftSubsystem lift = new LiftSubsystem(hardwareMap);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
lift.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
lift.toFloor();
|
||||
}
|
||||
|
||||
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
|
||||
lift.toHighBucket();
|
||||
}
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
lift.toLowBucket();
|
||||
}
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
lift.switchState();
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Lift Drive Position", lift.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,96 +0,0 @@
|
||||
/* Copyright (c) 2021 FIRST. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
||||
* the following conditions are met:
|
||||
*
|
||||
* Redistributions of source code must retain the above copyright notice, this list
|
||||
* of conditions and the following disclaimer.
|
||||
*
|
||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
||||
* list of conditions and the following disclaimer in the documentation and/or
|
||||
* other materials provided with the distribution.
|
||||
*
|
||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific prior written permission.
|
||||
*
|
||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
|
||||
@TeleOp(name = "Wrist Test", group = "Debug")
|
||||
public class WristTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Wrist
|
||||
*/
|
||||
WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
wrist.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
wrist.bucketWrist();
|
||||
}
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
wrist.floorWrist();
|
||||
}
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
wrist.switchState();
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
wrist.setPosition(wrist.getPosition() - .05);
|
||||
}
|
||||
|
||||
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||
wrist.setPosition(wrist.getPosition() + .05);
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Wrist State", wrist.getState());
|
||||
telemetry.addData("Wrist Position", wrist.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,22 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.configs;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class RobotConstants {
|
||||
public static double clawClose = 1.00;
|
||||
public static double clawOpen = 0.05;
|
||||
|
||||
public static double armEngage = 0.5;
|
||||
public static double armPark = 0.125;
|
||||
public static double armBucket = 0.175;
|
||||
|
||||
public static double wristFloor = 0.625;
|
||||
public static double wristBucket = 0.215;
|
||||
public static int liftToFloorPos = 20;
|
||||
public static int liftToFloatPos = 150;
|
||||
public static int liftToLowBucketPos = 2250;
|
||||
public static int liftToHighBucketPos = 3900;
|
||||
public static double liftPower = .45;
|
||||
public static int liftToHoverState = 60;
|
||||
}
|
||||
@ -71,9 +71,8 @@ measurements will be in centimeters.
|
||||
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
|
||||
what works best for you is most important. Higher numbers will cause a faster brake, but increase
|
||||
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`.
|
||||
|
||||
* The drive PID is much, much more sensitive than the others. For reference,
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
|
||||
* sensitive than the others. For reference,
|
||||
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
|
||||
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||
|
||||
@ -5,7 +5,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.DriveEncoderLocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer;
|
||||
@ -71,7 +70,6 @@ public class PoseUpdater {
|
||||
public PoseUpdater(HardwareMap hardwareMap) {
|
||||
// TODO: replace the second argument with your preferred localizer
|
||||
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
|
||||
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -38,9 +38,9 @@ public class DriveEncoderLocalizer extends Localizer {
|
||||
private Encoder leftRear;
|
||||
private Encoder rightRear;
|
||||
private double totalHeading;
|
||||
public static double FORWARD_TICKS_TO_INCHES = -0.6308;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 46.4839;
|
||||
public static double TURN_TICKS_TO_RADIANS = -0.002;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 1;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 1;
|
||||
public static double TURN_TICKS_TO_RADIANS = 1;
|
||||
public static double ROBOT_WIDTH = 1;
|
||||
public static double ROBOT_LENGTH = 1;
|
||||
|
||||
|
||||
@ -65,9 +65,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
|
||||
private double previousIMUOrientation;
|
||||
private double deltaRadians;
|
||||
private double totalHeading;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 0.004;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double STRAFE_TICKS_TO_INCHES = -0.0036;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
public static double TURN_TICKS_TO_RADIANS = 0.0022;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
|
||||
public static boolean useIMU = true;
|
||||
|
||||
@ -96,9 +96,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
|
||||
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
|
||||
|
||||
// TODO: replace these with your encoder positions
|
||||
leftEncoderPose = new Pose(-7.625, 6.19375, 0);
|
||||
rightEncoderPose = new Pose(-7.625, -6.19375, 0);
|
||||
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
|
||||
leftEncoderPose = new Pose(0, 6.19375, 0);
|
||||
rightEncoderPose = new Pose(0, -6.19375, 0);
|
||||
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
|
||||
|
||||
// TODO: replace these with your encoder ports
|
||||
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
||||
|
||||
@ -57,11 +57,8 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
private Pose rightEncoderPose;
|
||||
private Pose strafeEncoderPose;
|
||||
private double totalHeading;
|
||||
// public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
// public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
// public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
|
||||
/**
|
||||
@ -83,8 +80,8 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
*/
|
||||
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
||||
// TODO: replace these with your encoder positions
|
||||
leftEncoderPose = new Pose(0, 6.19375, 0);
|
||||
rightEncoderPose = new Pose(0, -6.19375, 0);
|
||||
leftEncoderPose = new Pose(0, 6.25, 0);
|
||||
rightEncoderPose = new Pose(0, -6.25, 0);
|
||||
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
|
||||
|
||||
hardwareMap = map;
|
||||
|
||||
@ -63,7 +63,6 @@ public class LateralTuner extends OpMode {
|
||||
telemetryA.addData("distance moved", poseUpdater.getPose().getY());
|
||||
telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
|
||||
telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
|
||||
|
||||
telemetryA.update();
|
||||
|
||||
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
|
||||
|
||||
@ -1,5 +1,9 @@
|
||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
|
||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
|
||||
@ -58,8 +62,10 @@ public class LocalizationTest extends OpMode {
|
||||
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
|
||||
|
||||
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
|
||||
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
|
||||
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||
|
||||
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
|
||||
|
||||
|
||||
@ -53,14 +53,14 @@ public class FollowerConstants {
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the translational PIDF
|
||||
public static double translationalPIDFFeedForward = 0.015;
|
||||
public static double translationalPIDFFeedForward = 0.00;
|
||||
|
||||
|
||||
// Heading error PIDF coefficients
|
||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
2,
|
||||
0,
|
||||
.025,
|
||||
0.025,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the heading PIDF
|
||||
|
||||
@ -93,11 +93,13 @@ public class ForwardVelocityTuner extends OpMode {
|
||||
}
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
|
||||
telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
|
||||
telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
|
||||
telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
|
||||
telemetryA.update();
|
||||
// telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
|
||||
// telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
|
||||
// telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
|
||||
// telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
|
||||
//
|
||||
//
|
||||
// telemetryA.update();
|
||||
|
||||
}
|
||||
|
||||
@ -138,6 +140,13 @@ public class ForwardVelocityTuner extends OpMode {
|
||||
velocities.add(currentVelocity);
|
||||
velocities.remove(0);
|
||||
}
|
||||
telemetryA.addData("x", poseUpdater.getPose().getX());
|
||||
telemetryA.addData("y", poseUpdater.getPose().getY());
|
||||
telemetryA.addData("heading", poseUpdater.getPose().getHeading());
|
||||
telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||
telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
|
||||
telemetryA.update();
|
||||
|
||||
} else {
|
||||
double average = 0;
|
||||
for (Double velocity : velocities) {
|
||||
@ -146,7 +155,12 @@ public class ForwardVelocityTuner extends OpMode {
|
||||
average /= (double) velocities.size();
|
||||
|
||||
telemetryA.addData("forward velocity:", average);
|
||||
telemetryA.update();
|
||||
// telemetryA.addData("x", poseUpdater.getPose().getX());
|
||||
// telemetryA.addData("y", poseUpdater.getPose().getY());
|
||||
// telemetryA.addData("heading", poseUpdater.getPose().getHeading());
|
||||
// telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||
// telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
|
||||
// telemetryA.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -119,13 +119,6 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
telemetry.addData("x",poseUpdater.getPose().getX());
|
||||
telemetry.addData("y",poseUpdater.getPose().getY());
|
||||
telemetry.addData("heading",poseUpdater.getPose().getHeading());
|
||||
telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||
telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta());
|
||||
|
||||
if (gamepad1.cross || gamepad1.a) {
|
||||
requestOpModeStop();
|
||||
}
|
||||
|
||||
@ -1,114 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.runmodes;
|
||||
|
||||
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.pedroPathing.pathGeneration.BezierCurve;
|
||||
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;
|
||||
import org.firstinspires.ftc.teamcode.util.action.SleepAction;
|
||||
|
||||
public class Auto {
|
||||
|
||||
public ClawSubsystem claw;
|
||||
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 int caseState = 1;
|
||||
|
||||
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;
|
||||
|
||||
init();
|
||||
}
|
||||
|
||||
public void init() {
|
||||
claw.init();
|
||||
arm.init();
|
||||
wrist.init();
|
||||
}
|
||||
|
||||
public void start() {
|
||||
clawTimer.resetTimer();
|
||||
armTimer.resetTimer();
|
||||
wristTimer.resetTimer();
|
||||
|
||||
claw.start();
|
||||
arm.start();
|
||||
wrist.start();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
|
||||
this.telemetry.addData("Current State", caseState);
|
||||
this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
|
||||
this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
|
||||
this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
|
||||
this.telemetry.update();
|
||||
|
||||
switch(caseState) {
|
||||
case 1:
|
||||
claw.openClaw();
|
||||
caseState = 2;
|
||||
break;
|
||||
case 2:
|
||||
if (clawTimer.getElapsedTimeSeconds() > 2) {
|
||||
arm.engageArm();
|
||||
caseState = 3;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if (armTimer.getElapsedTimeSeconds() > 4) {
|
||||
wrist.floorWrist();
|
||||
caseState = 4;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
if (clawTimer.getElapsedTimeSeconds() > 6) {
|
||||
claw.closeClaw();
|
||||
caseState = 5;
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
if (armTimer.getElapsedTimeSeconds() > 8) {
|
||||
arm.bucketArm();
|
||||
wrist.bucketWrist();
|
||||
caseState = 6;
|
||||
}
|
||||
break;
|
||||
case 6:
|
||||
if (clawTimer.getElapsedTimeSeconds() > 10) {
|
||||
claw.openClaw();
|
||||
caseState = 7;
|
||||
}
|
||||
break;
|
||||
case 7:
|
||||
this.init();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,89 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.runmodes;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
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.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
|
||||
public class Teleop {
|
||||
|
||||
private ClawSubsystem claw;
|
||||
private Follower follower;
|
||||
private DcMotorEx leftFront;
|
||||
private DcMotorEx leftRear;
|
||||
private DcMotorEx rightFront;
|
||||
private DcMotorEx rightRear;
|
||||
private Telemetry telemetry;
|
||||
|
||||
private Gamepad gamepad1;
|
||||
private Gamepad currentGamepad1;
|
||||
private Gamepad previousGamepad1;
|
||||
|
||||
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
|
||||
|
||||
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
|
||||
initMotors(hardwareMap);
|
||||
|
||||
this.follower = follower;
|
||||
|
||||
this.telemetry = telemetry;
|
||||
this.gamepad1 = gamepad1;
|
||||
|
||||
this.currentGamepad1 = new Gamepad();
|
||||
this.previousGamepad1 = new Gamepad();
|
||||
}
|
||||
|
||||
public void start() {
|
||||
claw.start();
|
||||
follower.startTeleopDrive();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.a && !previousGamepad1.a)
|
||||
claw.switchState();
|
||||
|
||||
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
|
||||
follower.update();
|
||||
|
||||
telemetry.addData("X", follower.getPose().getX());
|
||||
telemetry.addData("Y", follower.getPose().getY());
|
||||
telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading()));
|
||||
telemetry.addData("Claw State", claw.getState());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
private void initMotors(HardwareMap hardwareMap) {
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
|
||||
rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
|
||||
leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
|
||||
|
||||
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
|
||||
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
|
||||
|
||||
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,18 @@
|
||||
package org.firstinspires.ftc.teamcode.states;
|
||||
|
||||
public class FieldStates {
|
||||
|
||||
public enum FieldLocation {
|
||||
BUCKET, SUBMARINE, FLOATING, TRAVELING
|
||||
}
|
||||
|
||||
private FieldLocation fieldLocation;
|
||||
|
||||
public FieldLocation getFieldLocation() {
|
||||
return fieldLocation;
|
||||
}
|
||||
|
||||
public void setFieldLocation(FieldLocation fieldLocation) {
|
||||
this.fieldLocation = fieldLocation;
|
||||
}
|
||||
}
|
||||
@ -1,89 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.util.action.Actions;
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
|
||||
public class ArmSubsystem {
|
||||
|
||||
public enum ArmState {
|
||||
PARK, ENGAGE, BUCKET
|
||||
}
|
||||
|
||||
public ServoImplEx arm;
|
||||
public ArmState state;
|
||||
public RunAction engageArm, parkArm, bucketArm;
|
||||
|
||||
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
|
||||
arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
|
||||
arm.resetDeviceConfigurationForOpMode();
|
||||
|
||||
this.state = armState;
|
||||
|
||||
parkArm = new RunAction(this::parkArm);
|
||||
engageArm = new RunAction(this::engageArm);
|
||||
bucketArm = new RunAction(this::bucketArm);
|
||||
}
|
||||
|
||||
public void setState(ArmState armState) {
|
||||
if (armState == ArmState.ENGAGE) {
|
||||
arm.setPosition(armEngage);
|
||||
this.state = ArmState.ENGAGE;
|
||||
} else if (armState == ArmState.PARK) {
|
||||
arm.setPosition(armPark);
|
||||
this.state = ArmState.PARK;
|
||||
} else if (armState == ArmState.BUCKET) {
|
||||
arm.setPosition(armBucket);
|
||||
this.state = ArmState.BUCKET;
|
||||
}
|
||||
}
|
||||
|
||||
public void engageArm() {
|
||||
setState(ArmState.ENGAGE);
|
||||
}
|
||||
|
||||
public void parkArm() {
|
||||
setState(ArmState.PARK);
|
||||
}
|
||||
|
||||
public void bucketArm() {
|
||||
setState(ArmState.BUCKET);
|
||||
}
|
||||
|
||||
public void switchState() {
|
||||
if (state == ArmState.ENGAGE) {
|
||||
setState(ArmState.PARK);
|
||||
} else if (state == ArmState.PARK) {
|
||||
setState(ArmState.BUCKET);
|
||||
} else if (state == ArmState.BUCKET) {
|
||||
setState(ArmState.ENGAGE);
|
||||
}
|
||||
}
|
||||
|
||||
public ArmState getState() {
|
||||
return this.state;
|
||||
}
|
||||
|
||||
public void init() {
|
||||
Actions.runBlocking(parkArm);
|
||||
}
|
||||
|
||||
public void start() {
|
||||
Actions.runBlocking(parkArm);
|
||||
}
|
||||
|
||||
public double getPosition() {
|
||||
return this.arm.getPosition();
|
||||
}
|
||||
|
||||
public void setPosition(double position) {
|
||||
this.arm.setPosition(position);
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,67 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.util.action.Actions;
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
|
||||
|
||||
public class ClawSubsystem {
|
||||
|
||||
public enum ClawState {
|
||||
CLOSED, OPEN
|
||||
}
|
||||
|
||||
private Servo claw;
|
||||
private ClawState state;
|
||||
public RunAction openClaw, closeClaw;
|
||||
|
||||
public ClawSubsystem(HardwareMap hardwareMap, ClawState clawState) {
|
||||
claw = hardwareMap.get(Servo.class, "claw-servo");
|
||||
this.state = clawState;
|
||||
|
||||
openClaw = new RunAction(this::openClaw);
|
||||
closeClaw = new RunAction(this::closeClaw);
|
||||
}
|
||||
|
||||
public void setState(ClawState clawState) {
|
||||
if (clawState == ClawState.CLOSED) {
|
||||
claw.setPosition(clawClose);
|
||||
this.state = ClawState.CLOSED;
|
||||
} else if (clawState == ClawState.OPEN) {
|
||||
claw.setPosition(clawOpen);
|
||||
this.state = ClawState.OPEN;
|
||||
}
|
||||
}
|
||||
|
||||
public ClawState getState() {
|
||||
return this.state;
|
||||
}
|
||||
|
||||
public void switchState() {
|
||||
if (state == ClawState.CLOSED) {
|
||||
setState(ClawState.OPEN);
|
||||
} else if (state == ClawState.OPEN) {
|
||||
setState(ClawState.CLOSED);
|
||||
}
|
||||
}
|
||||
|
||||
public void openClaw() {
|
||||
setState(ClawState.OPEN);
|
||||
}
|
||||
|
||||
public void closeClaw() {
|
||||
setState(ClawState.CLOSED);
|
||||
}
|
||||
|
||||
public void init() {
|
||||
Actions.runBlocking(closeClaw);
|
||||
}
|
||||
|
||||
public void start() {
|
||||
Actions.runBlocking(closeClaw);
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,72 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
|
||||
@Config
|
||||
@Autonomous(name = "Lift Motor Subsystem - PID Test")
|
||||
public class LiftMotorSubsystem extends LinearOpMode {
|
||||
|
||||
private DcMotorEx liftSlideLeft;
|
||||
private DcMotorEx liftSlideRight;
|
||||
public static double kp = 0.0015, ki = 0, kd = 0;
|
||||
private double lastError = 0;
|
||||
private double integralSum = 0;
|
||||
public static int targetPosition = 0;
|
||||
private final FtcDashboard dashboard = FtcDashboard.getInstance();
|
||||
private ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
TelemetryPacket packet = new TelemetryPacket();
|
||||
dashboard.setTelemetryTransmissionInterval(25);
|
||||
|
||||
liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
|
||||
liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
|
||||
|
||||
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
waitForStart();
|
||||
|
||||
while(opModeIsActive()) {
|
||||
double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
|
||||
|
||||
packet.put("Power", power);
|
||||
packet.put("Position", liftSlideLeft.getCurrentPosition());
|
||||
packet.put("Error", lastError);
|
||||
packet.put("Seconds", timer.seconds());
|
||||
|
||||
liftSlideLeft.setPower(power);
|
||||
liftSlideRight.setPower(power);
|
||||
|
||||
dashboard.sendTelemetryPacket(packet);
|
||||
}
|
||||
}
|
||||
|
||||
private double calculatePower(int targetPosition, int currentPosition) {
|
||||
// reference is targetPosition, state is currentPosition
|
||||
double error = targetPosition - currentPosition;
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
lastError = error;
|
||||
timer.reset();
|
||||
return (error * kp) + (derivative * kd) + (integralSum * ki);
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,94 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHoverState;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
|
||||
public class LiftSubsystem {
|
||||
|
||||
public DcMotor lift;
|
||||
public RunAction toFloor, toLowBucket, toHighBucket;
|
||||
|
||||
public enum LiftState {
|
||||
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER
|
||||
}
|
||||
|
||||
private LiftState liftState;
|
||||
|
||||
public LiftSubsystem(HardwareMap hardwareMap) {
|
||||
lift = hardwareMap.get(DcMotor.class, "lift-motor");
|
||||
toFloor = new RunAction(this::toFloor);
|
||||
toLowBucket = new RunAction(this::toLowBucket);
|
||||
toHighBucket = new RunAction(this::toHighBucket);
|
||||
}
|
||||
|
||||
public void setTarget(int b) {
|
||||
lift.setTargetPosition(b);
|
||||
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
public void switchState() {
|
||||
if (this.liftState == LiftState.FLOOR) {
|
||||
this.toFloor();
|
||||
} else if (this.liftState == LiftState.FLOAT) {
|
||||
this.toLowBucket();
|
||||
} else if (this.liftState == LiftState.LOW_BUCKET) {
|
||||
this.toHighBucket();
|
||||
} else if (this.liftState == LiftState.HIGH_BUCKET) {
|
||||
this.toFloor();
|
||||
}
|
||||
}
|
||||
|
||||
public void toHover() {
|
||||
this.setTarget(liftToHoverState);
|
||||
this.setState(LiftState.HOVER);
|
||||
}
|
||||
|
||||
public void toFloor() {
|
||||
this.setTarget(liftToFloorPos);
|
||||
this.setState(LiftState.FLOOR);
|
||||
}
|
||||
|
||||
public void toFloat() {
|
||||
this.setTarget(liftToFloatPos);
|
||||
this.setState(LiftState.FLOAT);
|
||||
}
|
||||
|
||||
public void toLowBucket() {
|
||||
this.setTarget(liftToLowBucketPos);
|
||||
this.setState(LiftState.LOW_BUCKET);
|
||||
}
|
||||
|
||||
public void toHighBucket() {
|
||||
this.setTarget(liftToHighBucketPos);
|
||||
this.setState(LiftState.HIGH_BUCKET);
|
||||
}
|
||||
|
||||
public void init() {
|
||||
lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
lift.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
lift.setPower(liftPower);
|
||||
}
|
||||
|
||||
private void setState(LiftState liftState) {
|
||||
this.liftState = liftState;
|
||||
}
|
||||
|
||||
public int getPosition() {
|
||||
return lift.getCurrentPosition();
|
||||
}
|
||||
|
||||
public void start() {
|
||||
this.toFloor();
|
||||
}
|
||||
|
||||
}
|
||||
@ -8,7 +8,12 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
import com.acmerobotics.roadrunner.Action;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
@ -26,7 +31,7 @@ public class MotorsSubsystem {
|
||||
public DcMotor backRightMotor;
|
||||
|
||||
public enum TravelState {
|
||||
PARKED, BUCKET, SUBMARINE
|
||||
STOPPED, MOVING
|
||||
}
|
||||
|
||||
public TravelState travelState;
|
||||
@ -36,7 +41,7 @@ public class MotorsSubsystem {
|
||||
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
|
||||
this.hardwareMap = hardwareMap;
|
||||
this.telemetry = telemetry;
|
||||
this.power = 1.0;
|
||||
this.power = MAX_POWER;
|
||||
}
|
||||
|
||||
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
|
||||
@ -60,6 +65,8 @@ public class MotorsSubsystem {
|
||||
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
this.setState(TravelState.STOPPED);
|
||||
}
|
||||
|
||||
public void setFrontLeftMotorPower(double power) {
|
||||
@ -78,46 +85,6 @@ public class MotorsSubsystem {
|
||||
backRightMotor.setPower(power);
|
||||
}
|
||||
|
||||
public void calculateTrajectory(Gamepad gamepad1) {
|
||||
double max;
|
||||
|
||||
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
||||
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
|
||||
double lateral = gamepad1.left_stick_x;
|
||||
double yaw = gamepad1.right_stick_x;
|
||||
|
||||
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
||||
// Set up a variable for each drive wheel to save the power level for telemetry.
|
||||
double leftFrontPower = axial + lateral + yaw;
|
||||
double leftBackPower = axial - lateral + yaw;
|
||||
|
||||
double rightFrontPower = axial - lateral - yaw;
|
||||
double rightBackPower = axial + lateral - yaw;
|
||||
|
||||
// Normalize the values so no wheel power exceeds 100%
|
||||
// This ensures that the robot maintains the desired motion.
|
||||
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
|
||||
max = Math.max(max, Math.abs(leftBackPower));
|
||||
max = Math.max(max, Math.abs(rightBackPower));
|
||||
|
||||
if (max > 1.0) {
|
||||
leftFrontPower /= max;
|
||||
rightFrontPower /= max;
|
||||
leftBackPower /= max;
|
||||
rightBackPower /= max;
|
||||
}
|
||||
|
||||
// Send calculated power to wheels
|
||||
this.setFrontLeftMotorPower(leftFrontPower * this.power);
|
||||
this.setFrontRightMotorPower(rightFrontPower * this.power);
|
||||
this.setBackLeftMotorPower(leftBackPower * this.power);
|
||||
this.setBackRightMotorPower(rightBackPower * this.power);
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||
this.telemetry.update();
|
||||
}
|
||||
|
||||
public void setState(TravelState travelState) {
|
||||
this.travelState = travelState;
|
||||
@ -127,4 +94,12 @@ public class MotorsSubsystem {
|
||||
return this.travelState;
|
||||
}
|
||||
|
||||
public void setPower(DcMotor motor, double power) {
|
||||
motor.setPower(power);
|
||||
if (power < 0.05) {
|
||||
this.setState(TravelState.MOVING);
|
||||
} else {
|
||||
this.setState(TravelState.STOPPED);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,79 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
|
||||
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.util.action.Actions;
|
||||
import org.firstinspires.ftc.teamcode.util.action.RunAction;
|
||||
|
||||
public class WristSubsystem {
|
||||
|
||||
public enum WristState {
|
||||
FLOOR, BUCKET
|
||||
}
|
||||
|
||||
public ServoImplEx wrist;
|
||||
public WristState state;
|
||||
public RunAction floorWrist, bucketWrist;
|
||||
|
||||
public WristSubsystem(HardwareMap hardwareMap, WristState wristState) {
|
||||
wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
|
||||
wrist.resetDeviceConfigurationForOpMode();
|
||||
|
||||
this.state = wristState;
|
||||
|
||||
bucketWrist = new RunAction(this::bucketWrist);
|
||||
floorWrist = new RunAction(this::floorWrist);
|
||||
}
|
||||
|
||||
public void setState(WristState wristState) {
|
||||
if (wristState == WristState.FLOOR) {
|
||||
wrist.setPosition(wristFloor);
|
||||
this.state = WristState.FLOOR;
|
||||
} else if (wristState == WristState.BUCKET) {
|
||||
wrist.setPosition(wristBucket);
|
||||
this.state = WristState.BUCKET;
|
||||
}
|
||||
}
|
||||
|
||||
public void floorWrist() {
|
||||
setState(WristState.FLOOR);
|
||||
}
|
||||
|
||||
public void bucketWrist() {
|
||||
setState(WristState.BUCKET);
|
||||
}
|
||||
|
||||
public void switchState() {
|
||||
if (state == WristState.FLOOR) {
|
||||
setState(WristState.BUCKET);
|
||||
} else if (state == WristState.BUCKET) {
|
||||
setState(WristState.FLOOR);
|
||||
}
|
||||
}
|
||||
|
||||
public WristState getState() {
|
||||
return this.state;
|
||||
}
|
||||
|
||||
public void init() {
|
||||
Actions.runBlocking(floorWrist);
|
||||
}
|
||||
|
||||
public void start() {
|
||||
Actions.runBlocking(floorWrist);
|
||||
}
|
||||
|
||||
public double getPosition() {
|
||||
return this.wrist.getPosition();
|
||||
}
|
||||
|
||||
public void setPosition(double position) {
|
||||
this.wrist.setPosition(position);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -1,11 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
|
||||
public interface Action {
|
||||
boolean run(TelemetryPacket p);
|
||||
|
||||
default void preview(Canvas fieldOverlay) {
|
||||
}
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
|
||||
public class Actions {
|
||||
|
||||
public static void runBlocking(Action a) {
|
||||
FtcDashboard dash = FtcDashboard.getInstance();
|
||||
Canvas c = new Canvas();
|
||||
a.preview(c);
|
||||
|
||||
boolean b = true;
|
||||
while (b && !Thread.currentThread().isInterrupted()) {
|
||||
TelemetryPacket p = new TelemetryPacket();
|
||||
p.fieldOverlay().getOperations().addAll(c.getOperations());
|
||||
|
||||
b = a.run(p);
|
||||
|
||||
dash.sendTelemetryPacket(p);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,39 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
|
||||
@Config
|
||||
public class FieldConstants {
|
||||
|
||||
public enum RobotStart {
|
||||
BLUE_BUCKET,
|
||||
BLUE_OBSERVATION,
|
||||
RED_BUCKET,
|
||||
RED_OBSERVATION
|
||||
}
|
||||
|
||||
public static final Pose blueBucketStartPose = new Pose(8, 79.5, Math.toRadians(180));
|
||||
public static final Pose blueObservationStartPose = new Pose(8, 36, Math.toRadians(180));
|
||||
public static final Pose redBucketStartPose = new Pose(144-8, 79.5, 0);
|
||||
public static final Pose redObservationStartPose = new Pose(144-8, 36, 0);
|
||||
|
||||
// Blue Preload Poses
|
||||
public static final Pose blueBucketPreloadPose = new Pose(34.5, 79.5, Math.toRadians(180));
|
||||
|
||||
// Blue Bucket Sample Poses
|
||||
public static final Pose blueBucketLeftSamplePose = new Pose(34.75, 113.5, Math.toRadians(66));
|
||||
public static final Pose blueBucketLeftSampleControlPose = new Pose(32, 108);
|
||||
public static final Pose blueBucketMidSamplePose = new Pose(33, 125.5, Math.toRadians(73));
|
||||
public static final Pose blueBucketMidSampleControlPose = new Pose(47.5, 110);
|
||||
public static final Pose blueBucketRightSamplePose = new Pose(33, 133, Math.toRadians(74));
|
||||
public static final Pose blueBucketRightSampleControlPose = new Pose(46, 101);
|
||||
|
||||
public static final Pose blueBucketScorePose = new Pose(16, 128, Math.toRadians(-45));
|
||||
|
||||
public static final Pose blueBucketParkPose = new Pose(65, 97.75, Math.toRadians(90));
|
||||
public static final Pose blueBucketParkControlPose = new Pose(60.25, 123.5);
|
||||
|
||||
|
||||
}
|
||||
@ -1,30 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
|
||||
public class RunAction implements Action {
|
||||
|
||||
private final Runnable runnable;
|
||||
private Runnable callback;
|
||||
|
||||
public RunAction(Runnable runnable) {
|
||||
this.runnable = runnable;
|
||||
}
|
||||
|
||||
public void runAction() {
|
||||
runnable.run();
|
||||
if (callback != null) {
|
||||
callback.run();
|
||||
}
|
||||
}
|
||||
|
||||
public void setCallback(Runnable callback) {
|
||||
this.callback = callback;
|
||||
}
|
||||
|
||||
// Adapter to make Action compatible with the Action interface
|
||||
public boolean run(TelemetryPacket p) {
|
||||
runAction();
|
||||
return false; // Regular actions complete after one execution
|
||||
}
|
||||
}
|
||||
@ -1,43 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class SequentialAction implements Action {
|
||||
private List<Action> actions;
|
||||
|
||||
public SequentialAction(List<Action> actions) {
|
||||
this.actions = new ArrayList<>(actions);
|
||||
}
|
||||
|
||||
public SequentialAction(Action... actions) {
|
||||
this(Arrays.asList(actions));
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean run(TelemetryPacket p) {
|
||||
if (actions.isEmpty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (actions.get(0).run(p)) {
|
||||
return true;
|
||||
} else {
|
||||
actions.remove(0);
|
||||
return run(p);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void preview(Canvas fieldOverlay) {
|
||||
for (Action a : actions) {
|
||||
a.preview(fieldOverlay);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -1,32 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.util.action;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
|
||||
import java.util.Timer;
|
||||
import java.util.TimerTask;
|
||||
|
||||
public class SleepAction implements Action {
|
||||
private final double dt;
|
||||
private boolean isFinished = false;
|
||||
|
||||
public SleepAction(double dt) {
|
||||
this.dt = dt;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean run(TelemetryPacket p) {
|
||||
if (!isFinished) {
|
||||
Timer timer = new Timer();
|
||||
timer.schedule(new TimerTask() {
|
||||
@Override
|
||||
public void run() {
|
||||
isFinished = true;
|
||||
}
|
||||
}, (long) (dt * 1000));
|
||||
} else {
|
||||
isFinished = false; // Reset the flag after sleep is complete
|
||||
return false; // Indicate that the action is finished
|
||||
}
|
||||
return true; // Indicate that the action is still running
|
||||
}
|
||||
}
|
||||
@ -16,6 +16,9 @@ dependencies {
|
||||
implementation 'org.firstinspires.ftc:Vision:10.1.0'
|
||||
implementation 'androidx.appcompat:appcompat:1.2.0'
|
||||
|
||||
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
|
||||
implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
|
||||
implementation "com.acmerobotics.roadrunner:core:1.0.0"
|
||||
implementation "com.acmerobotics.roadrunner:actions:1.0.0"
|
||||
implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user