SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: SCDS:branch-rc-chassis-14493-subsystem
Branches Tags
SCDS:master SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep
..
compare: SCDS:branch-pedro-led-light
Branches Tags
SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep SCDS:master

1 Commits
branch-rc- ... branch-ped

Author SHA1 Message Date
Carlos Rivas
c1023e831b Add Demo for goBILDA Indicator Light 2024-10-21 18:48:36 -07:00
47 changed files with 127 additions and 3510 deletions
Expand all files Collapse all files

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java
View File

@ -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);
}
}

133
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java
View File

@ -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);
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java
View File

@ -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();
}
}

31
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java
View File

@ -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();
}
}

92
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DemoGoBildaIndicatorLight.java Normal file
View File

@ -0,0 +1,92 @@
/*
Copyright (c) 2024 Alan Smith
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 Alan Smith 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 FITNESSFOR 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;
/*
* This OpMode illustrates how to use the goBILDA Indicator Light
*
* To make this work, this Light uses a Servo motor and setPositions.
* Valid values are from .277 to .722.
*
* If it's less than .277 then no light
* If it's greater than .722 then white light
*
* Spec Sheet is here: https://cdn11.bigcommerce.com/s-x56mtydx1w/images/stencil/original/products/2275/13464/3118-0808-0002-Product-Insight-4__03940__01348.1728056113.png?c=1
*/
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name = "Demo: goBILDA Indicator Light", group = "Concept")
public class DemoGoBildaIndicatorLight extends LinearOpMode {
Servo front_led;
private final double buffer = .277;
@Override
public void runOpMode() {
front_led = hardwareMap.get(Servo.class, "test_servo");
waitForStart();
while (opModeIsActive()) {
double left_x = Math.abs(gamepad1.left_stick_x) / 4;
double left_y = Math.abs(gamepad1.left_stick_y) / 4;
double right_x = Math.abs(gamepad1.right_stick_x) / 4;
double right_y = Math.abs(gamepad1.right_stick_y) / 4;
double total_position = Math.min(left_x + left_y + right_x + right_y + buffer, 1.0);
telemetry.addData("left_x", left_x);
telemetry.addData("left_y", left_y);
telemetry.addData("right_x", right_x);
telemetry.addData("right_y", right_y);
telemetry.addData("total (plus buffer)", total_position);
telemetry.update();
front_led.setPosition(total_position);
if (gamepad1.cross) {
front_led.setPosition(.611);
sleep(2000);
}
if (gamepad1.triangle) {
front_led.setPosition(.444);
sleep(2000);
}
if (gamepad1.square) {
front_led.setPosition(.7);
sleep(2000);
}
if (gamepad1.circle) {
front_led.setPosition(.3);
sleep(2000);
}
}
}
}

97
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java
View File

@ -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);
}
}

107
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java
View File

@ -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();
}
}

196
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
View File

@ -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();
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
View File

@ -10,11 +10,8 @@ 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";
@ -43,7 +40,7 @@ public class PedroConstants {
// 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 BACK_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
/*
Pedro's parameters
@ -53,21 +50,21 @@ public class PedroConstants {
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
// Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 51.4598;
public static final double ROBOT_SPEED_FORWARD = 72.0693;
// Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 28.7119;
public static final double ROBOT_SPEED_LATERAL = 24.1401;
// 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 = -74.3779;
// 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 = -111.8409;
// 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;
public static final double ZERO_POWER_ACCEL_MULT = 4;
/* Centripetal force correction - increase if robot is correcting into the path
- decrease if robot is correcting away from the path */
public static final double CENTRIPETAL_SCALING = 0.0004;
public static final double CENTRIPETAL_SCALING = 0.0005;
}

135
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java
View File

@ -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);
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java
View File

@ -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);
}
}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java
View File

@ -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);
}
}

142
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java
View File

@ -1,142 +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 = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
public class AutoExampleSeason2025V1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(15.0, 35, 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(.375);
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)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 2
new BezierLine(
new Point(60.000, 35.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 3
new BezierLine(
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(15.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 4
new BezierLine(
new Point(15.000, 25.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 6
new BezierLine(
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(15.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 7
new BezierLine(
new Point(15.000, 15.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 8
new BezierLine(
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(60.000, 8.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 9
new BezierLine(
new Point(60.000, 8.000, Point.CARTESIAN),
new Point(15.000, 8.000, 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);
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java
View File

@ -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);
}
}

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java
View File

@ -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);
}
}

197
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java
View File

@ -1,197 +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;
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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This file contains an example of a Linear "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
*
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
*
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
*
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* 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
*/
@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")
public class BasicOmniOpMode_Linear extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
// Initialize the hardware variables. Note that the strings used here must correspond
// to the names assigned during the robot configuration step on the DS or RC devices.
DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
// TODO: replace these with your encoder ports
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
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);
// ########################################################################################
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
// ########################################################################################
// Most robots need the motors on one side to be reversed to drive forward.
// The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft)
// If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure
// that your motors are turning in the correct direction. So, start out with the reversals here, BUT
// when you first test your robot, push the left joystick forward and observe the direction the wheels turn.
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.
leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
// Wait for the game to start (driver presses START)
telemetry.addData("Status", "Initialized");
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
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;
}
// This is test code:
//
// Uncomment the following code to test your motor directions.
// Each button should make the corresponding motor run FORWARD.
// 1) First get all the motors to take to correct positions on the robot
// by adjusting your Robot Configuration if necessary.
// 2) Then make sure they run in the correct direction by modifying the
// the setDirection() calls above.
// Once the correct motors move in the correct direction re-comment this code.
/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/
// Send calculated power to wheels
leftFrontDrive.setPower(leftFrontPower);
rightFrontDrive.setPower(rightFrontPower);
leftBackDrive.setPower(leftBackPower);
rightBackDrive.setPower(rightBackPower);
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
}
}}

161
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java
View File

@ -1,161 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
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 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 = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75);
/**
* 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 BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, 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)
)
)
.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)
)
)
.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)
)
)
.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)
)
)
.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)
)
)
.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)
)
)
.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)
)
)
.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)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).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);
}
}

246
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java
View File

@ -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);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java
View File

@ -1,4 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
public class BluenbAutov1 {
}

172
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java
View File

@ -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();
}
}
}

95
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java
View File

@ -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();
}
}
}

78
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
View File

@ -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();
}
}
}

124
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java
View File

@ -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();
}
}
}

98
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java
View File

@ -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();
}
}
}

96
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java
View File

@ -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();
}
}
}

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -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;
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md
View File

@ -71,9 +71,7 @@ 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`

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -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;
@ -70,8 +69,7 @@ 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));
this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
}
/**

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java
View File

@ -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;

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java
View File

@ -57,12 +57,9 @@ 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;
public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//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 TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
/**
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
@ -83,9 +80,9 @@ 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);
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0);
strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;
@ -95,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
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);
//leftEncoder.setDirection(Encoder.REVERSE);
// rightEncoder.setDirection(Encoder.REVERSE);
//strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java
View File

@ -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");

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -42,7 +42,7 @@ public class FollowerConstants {
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
0.1,
0,
0.01,
0,
0);
// Translational Integral
@ -58,9 +58,9 @@ public class FollowerConstants {
// Heading error PIDF coefficients
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
2,
1,
0,
0,
.025,
0);
// Feed forward constant added on to the heading PIDF
@ -69,10 +69,10 @@ public class FollowerConstants {
// Drive PIDF coefficients
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.006,
0.025,
0,
0.00001,
0.8,
0.6,
0);
// Feed forward constant added on to the drive PIDF
@ -81,7 +81,7 @@ public class FollowerConstants {
// Kalman filter parameters for the drive error Kalman filter
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
6,
3);
1);
// Mass of robot in kilograms

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java
View File

@ -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();
}

114
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
View File

@ -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;
}
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java
View File

@ -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);
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
View File

@ -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);
}
}

67
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java
View File

@ -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);
}
}

94
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
View File

@ -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();
}
}

130
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java
View File

@ -1,130 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
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.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class MotorsSubsystem {
public HardwareMap hardwareMap;
public Telemetry telemetry;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
public enum TravelState {
PARKED, BUCKET, SUBMARINE
}
public TravelState travelState;
public double power;
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = 1.0;
}
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = power;
}
public void 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);
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void setFrontLeftMotorPower(double power) {
frontLeftMotor.setPower(power);
}
public void setBackLeftMotorPower(double power) {
backLeftMotor.setPower(power);
}
public void setFrontRightMotorPower(double power) {
frontRightMotor.setPower(power);
}
public void setBackRightMotorPower(double power) {
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;
}
public TravelState getState() {
return this.travelState;
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
View File

@ -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);
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java
View File

@ -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) {
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java
View File

@ -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);
}
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java
View File

@ -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);
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java
View File

@ -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
}
}

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java
View File

@ -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);
}
}
}

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java
View File

@ -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
build.dependencies.gradle
View File

@ -6,14 +6,14 @@ repositories {
}
dependencies {
implementation 'org.firstinspires.ftc:Inspection:10.1.0'
implementation 'org.firstinspires.ftc:Blocks:10.1.0'
implementation 'org.firstinspires.ftc:RobotCore:10.1.0'
implementation 'org.firstinspires.ftc:RobotServer:10.1.0'
implementation 'org.firstinspires.ftc:OnBotJava:10.1.0'
implementation 'org.firstinspires.ftc:Hardware:10.1.0'
implementation 'org.firstinspires.ftc:FtcCommon:10.1.0'
implementation 'org.firstinspires.ftc:Vision:10.1.0'
implementation 'org.firstinspires.ftc:Inspection:10.0.0'
implementation 'org.firstinspires.ftc:Blocks:10.0.0'
implementation 'org.firstinspires.ftc:RobotCore:10.0.0'
implementation 'org.firstinspires.ftc:RobotServer:10.0.0'
implementation 'org.firstinspires.ftc:OnBotJava:10.0.0'
implementation 'org.firstinspires.ftc:Hardware:10.0.0'
implementation 'org.firstinspires.ftc:FtcCommon:10.0.0'
implementation 'org.firstinspires.ftc:Vision:10.0.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'