33 Commits

Author SHA1 Message Date
047d0fa3c3 Dual Motor Slide Subsystem 2024-12-25 15:09:39 -08:00
c63319f9c4 Simplification of class files 2024-12-25 15:08:36 -08:00
c824580b33 Moving class to test package 2024-12-25 15:07:48 -08:00
883906885b Fixed imports and static names 2024-12-23 16:32:53 -08:00
5595fcccd4 Removing unnecessary files 2024-12-23 16:32:21 -08:00
3e79d86443 Finalize tunings 2024-12-18 20:42:14 -08:00
c9ffd4f061 Added driver hub config naming 2024-12-17 21:35:16 -08:00
021dfa7222 Added lift configurations 2024-12-17 21:34:49 -08:00
233b177cf6 Removing more files to keep things light for this branch 2024-12-17 21:32:10 -08:00
0ab402af0f Lightening the load and configured the robot with appropriate values (encoders/motors) 2024-12-17 19:07:34 -08:00
aa496b8237 Ollie's work committed 2024-12-08 12:48:30 -08:00
66f3339e26 Working Pre loaded auto! Can score 11 points consistently! 2024-11-14 17:10:46 -08:00
ad0a8d3374 Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493 2024-11-14 16:23:46 -08:00
dc71eb4317 Feature where driver can override centricity (robot vs field). 2024-11-13 15:09:59 -08:00
94144780b8 Tentative fix for robot/runBlocking problem as per issue #2 2024-11-13 09:13:24 -08:00
a362d2e004 Massive upgrade and shift of files 2024-11-12 23:16:23 -08:00
2008c3cd88 Working somehitng i don't know 2024-11-12 16:55:35 -08:00
c5be3cd932 Silver Branch Code 2024-11-03 09:13:37 -08:00
7d83b9c254 Add sample cometbot package 2024-10-31 15:37:09 -07:00
5f50d053c5 Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
2024-10-21 21:57:45 -07:00
0cfb57c643 Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493 2024-10-17 11:07:25 -07:00
50db1f9175 Add new files (2nd try) 2024-10-17 11:07:10 -07:00
552bb3e25a Add new files 2024-10-17 11:06:13 -07:00
c1076a832c Rebased changes 2024-10-17 11:05:12 -07:00
7bf8b0c357 Add files 2024-10-17 11:04:02 -07:00
adfab3e8af Add sample code 2024-10-15 15:44:49 -07:00
0f7ea50907 Updated constants for silver and it, somehow, works 2024-10-15 15:43:53 -07:00
239f168540 Retuned to success, PIDF (P = .05) and translationalPIDFeedForward is back to 0 2024-10-15 13:59:02 -07:00
f2bcdcc55a Add constants 2024-10-01 17:05:54 -07:00
e08aac773d Static entries for arm servos 2024-10-01 15:45:02 -07:00
3950a83ac1 Added encoder naming and usage 2024-10-01 10:19:40 -07:00
552ff3f339 Updated to 10.1 2024-09-30 22:37:16 -07:00
4eef485dab Re-wired and reconfigured 2024-09-30 20:39:24 -07:00
59 changed files with 457 additions and 4664 deletions

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

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

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

View File

@ -0,0 +1,24 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
@TeleOp(name = "CometBot Auto v2", group = "Development")
public class CometBotDevAuto extends OpMode {
public CometBotAutoDevelopment runMode;
@Override
public void init() {
runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2);
runMode.init();
}
@Override
public void loop() {
runMode.update();
telemetry.update();
}
}

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

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

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

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

View File

@ -10,15 +10,12 @@ public class PedroConstants {
/*
Robot parameters
*/
// Turn localizer - -0.003
// Robot motor configurations
public static final String BRAIN_ROT = "Sikidi rizz 360 no teleop tf2 mama mia 2cool 4skool yasyasy yasyasyasyasyasyasyaysy ohio yes heh me is moar skeebeedee than u walked and got tripped on by your aunt my very educaded mother just served us nine what? just kydinfoiwfowefwofwioefoiejfeoiwjfomdsklfnslefknesfklnkfenfenkfeknfenkfeknfenkefnk";
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
public static final String BACK_LEFT_MOTOR = "Drive back lt";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
public static final String FRONT_LEFT_MOTOR = "front-left";
public static final String BACK_LEFT_MOTOR = "back-left";
public static final String FRONT_RIGHT_MOTOR = "front-right";
public static final String BACK_RIGHT_MOTOR = "back-right";
// Robot motor direction
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
@ -26,46 +23,60 @@ public class PedroConstants {
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
/*
Motor Max Power
*/
public static final double MAX_POWER = .75;
// Robot IMU configuration
public static final String IMU = "imu";
// Robot IMU placement
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
= RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
= RevHubOrientationOnRobot.UsbFacingDirection.UP;
// Robot encoders
public static final String LEFT_ENCODER = "encoder left";
public static final String RIGHT_ENCODER = "encoder right";
public static final String BACK_ENCODER = "encoder back";
// NOTE: Encoders are plugged into the same ports as motors hence the weird names
public static final String RIGHT_ENCODER = "front-left"; //2
public static final String BACK_ENCODER = "front-right"; //1
public static final String LEFT_ENCODER = "back-right"; //0
// Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
// Arm config
public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
public static final String LIFT_SLIDE_RIGHT_MOTOR = "lift-slide-right";
public static final String CLAW_SERVO = "claw-servo";
public static final String WRIST_SERVO = "wrist-servo";
public static final String ARM_SERVO = "arm-servo";
public static final String THUMB_SERVO = "thumb-servo";
/*
Pedro's parameters
*/
// The weight of the robot in Kilograms
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
public static final double ROBOT_WEIGHT_IN_KG = 9;
// Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 51.4598;
public static final double ROBOT_SPEED_FORWARD = 53.223;
// Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 28.7119;
public static final double ROBOT_SPEED_LATERAL = 41.4081;
// 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 = -76.8421;
// 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 = -93.4183;
// 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.0;
/* Centripetal force correction - increase if robot is correcting into the path
- decrease if robot is correcting away from the path */

View File

@ -1,11 +1,9 @@
package org.firstinspires.ftc.teamcode.cometbots;
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
@ -15,32 +13,16 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
public class BlueBasketAuto extends OpMode {
@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
public class PreLoadedBlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75);
private final Pose startPose = new Pose(7.875, 89.357);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
@ -53,90 +35,76 @@ public class BlueBasketAuto extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN)
new Point(8.036, 89.196, Point.CARTESIAN),
new Point(10.125, 126.804, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(37.000, 108.000, Point.CARTESIAN),
new Point(73.286, 111.536, Point.CARTESIAN),
new Point(67.821, 120.536, Point.CARTESIAN)
new Point(10.125, 126.804, Point.CARTESIAN),
new Point(37.607, 90.000, Point.CARTESIAN),
new Point(62.357, 119.893, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(67.821, 120.536, Point.CARTESIAN),
new Point(28.000, 121.500, Point.CARTESIAN)
new BezierCurve(
new Point(62.357, 119.893, Point.CARTESIAN),
new Point(33.750, 112.500, Point.CARTESIAN),
new Point(15.107, 130.661, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(28.000, 121.500, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN)
new BezierCurve(
new Point(15.107, 130.661, Point.CARTESIAN),
new Point(58.821, 103.018, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierCurve(
new Point(18.000, 130.179, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN),
new Point(68.700, 130.500, Point.CARTESIAN)
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(15.107, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(68.700, 130.500, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN)
new Point(15.107, 130.339, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN)
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(57.857, 133.071, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(63.804, 135.321, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN)
new Point(57.857, 133.071, Point.CARTESIAN),
new Point(18.964, 134.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(53.036, 135.161, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(72.300, 97.400, Point.CARTESIAN)
)
)
.addPath(
// Line 9
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(83.250, 95.464, Point.CARTESIAN)
new BezierCurve(
new Point(18.964, 134.679, Point.CARTESIAN),
new Point(84.536, 131.786, Point.CARTESIAN),
new Point(80.036, 96.429, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
@ -146,16 +114,9 @@ public class BlueBasketAuto extends OpMode {
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

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

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

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

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

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

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

View File

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

View File

@ -0,0 +1,86 @@
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
public class CometBotAutoDevelopment {
/*
Subsystems
*/
private DualMotorSliderSubsystem dualSlides;
/*
Controllers
*/
public Gamepad gamepad1;
public Gamepad gamepad2;
public Gamepad currentGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad1;
public Gamepad previousGamepad2;
private Follower follower;
public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
dualSlides = new DualMotorSliderSubsystem(hardwareMap);
this.gamepad1 = gamepad1;
this.gamepad2 = gamepad2;
currentGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad1 = new Gamepad();
previousGamepad2 = new Gamepad();
follower = new Follower(hardwareMap);
}
public void init() {
dualSlides.init();
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
public void update() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
/*
Check if dpad_up/down is being pressed for slides
*/
dualSlides.update();
dualSlidesToLowBucketPosition();
dualSlidesToHighBucketPosition();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
private void dualSlidesToHighBucketPosition() {
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
dualSlides.toHighBucketPosition();
}
}
private void dualSlidesToLowBucketPosition() {
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
dualSlides.toLowBucketPosition();
}
}
}

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

View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots;
package org.firstinspires.ftc.teamcode.cometbots.projects;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;

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

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

View File

@ -0,0 +1,72 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
@Config
@Autonomous(name = "Lift Motor Subsystem - PID Test")
public class DualMotorSliderTest extends LinearOpMode {
private DcMotorEx liftSlideLeft;
private DcMotorEx liftSlideRight;
public static double kp = 0.0015, ki = 0, kd = 0;
private double lastError = 0;
private double integralSum = 0;
public static int targetPosition = 0;
private final FtcDashboard dashboard = FtcDashboard.getInstance();
private ElapsedTime timer = new ElapsedTime();
@Override
public void runOpMode() throws InterruptedException {
TelemetryPacket packet = new TelemetryPacket();
dashboard.setTelemetryTransmissionInterval(25);
liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
while(opModeIsActive()) {
double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
packet.put("Power", power);
packet.put("Position", liftSlideLeft.getCurrentPosition());
packet.put("Error", lastError);
packet.put("Seconds", timer.seconds());
liftSlideLeft.setPower(power);
liftSlideRight.setPower(power);
dashboard.sendTelemetryPacket(packet);
}
}
private double calculatePower(int targetPosition, int currentPosition) {
// reference is targetPosition, state is currentPosition
double error = targetPosition - currentPosition;
integralSum += error * timer.seconds();
double derivative = (error - lastError) / timer.seconds();
lastError = error;
timer.reset();
return (error * kp) + (derivative * kd) + (integralSum * ki);
}
}

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

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

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

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

View File

@ -71,9 +71,8 @@ measurements will be in centimeters.
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
what works best for you is most important. Higher numbers will cause a faster brake, but increase
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`.
* The drive PID is much, much more sensitive than the others. For reference,
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
* sensitive than the others. For reference,
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`

View File

@ -5,10 +5,7 @@ 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;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
@ -71,7 +68,6 @@ public class PoseUpdater {
public PoseUpdater(HardwareMap hardwareMap) {
// TODO: replace the second argument with your preferred localizer
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
}
/**

View File

@ -1,272 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the drive encoder set up.
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class DriveEncoderLocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftFront;
private Encoder rightFront;
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 ROBOT_WIDTH = 1;
public static double ROBOT_LENGTH = 1;
/**
* This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public DriveEncoderLocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName));
// TODO: reverse any encoders necessary
leftFront.setDirection(Encoder.REVERSE);
rightRear.setDirection(Encoder.REVERSE);
leftRear.setDirection(Encoder.FORWARD);
rightRear.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders. Then, the robot's global change in position is calculated
* using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders.
*/
public void updateEncoders() {
leftFront.update();
rightFront.update();
leftRear.update();
rightRear.update();
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
leftFront.reset();
rightFront.reset();
leftRear.reset();
rightRear.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
// theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
/**
* This does nothing since this localizer does not use the IMU.
*/
public void resetIMU() {
}
}

View File

@ -1,218 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
/**
* This is the OTOSLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the SparkFun OTOS. The diagram below, which is modified from
* Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/20/2024
*/
public class OTOSLocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private SparkFunOTOS otos;
private double previousHeading;
private double totalHeading;
/**
* This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public OTOSLocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
// TODO: replace this with your OTOS port
/*
TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the
'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a
"SparkFunOTOS Corrected" in your robot confg
*/
SparkFunOTOS
otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
otos.setLinearUnit(DistanceUnit.INCH);
otos.setAngularUnit(AngleUnit.RADIANS);
// TODO: replace this with your OTOS offset from the center of the robot
// For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being
// positive y and forward being positive x. PI/2 radians is facing forward, and clockwise
// rotation is negative rotation.
otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2));
// TODO: replace these with your tuned multipliers
otos.setLinearScalar(1.0);
otos.setAngularScalar(1.0);
otos.calibrateImu();
otos.resetTracking();
setStartPose(setStartPose);
totalHeading = 0;
previousHeading = startPose.getHeading();
resetOTOS();
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
SparkFunOTOS.Pose2D pose = otos.getPosition();
return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h));
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity();
return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return getVelocity().getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
resetOTOS();
Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose);
otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading()));
}
/**
* This updates the total heading of the robot. The OTOS handles all other updates itself.
*/
@Override
public void update() {
totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading);
previousHeading = otos.getPosition().h;
}
/**
* This resets the OTOS.
*/
public void resetOTOS() {
otos.resetTracking();
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from OTOS
* ticks to inches. For the OTOS, this value is the same as the lateral multiplier.
* This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return otos.getLinearScalar();
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier.
* This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return otos.getLinearScalar();
}
/**
* This returns the multiplier applied to turning movement measurement to convert from OTOS ticks
* to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return otos.getAngularScalar();
}
/**
* This does nothing since this localizer does not use the IMU.
*/
public void resetIMU() {
}
}

View File

@ -1,159 +0,0 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
//
//import java.util.ArrayList;
//import java.util.List;
//
///**
// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and
// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
// * localizer system.
// *
// * @author Anyi Lin - 10158 Scott's Bots
// * @version 1.0, 5/9/2024
// */
//public class RRToPedroThreeWheelLocalizer extends Localizer {
// private RoadRunnerThreeWheelLocalizer localizer;
// private double totalHeading;
// private Pose startPose;
// private Pose previousPose;
//
// /**
// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously
// * used Road Runner localization system to the new Pedro Pathing localization system.
// *
// * @param hardwareMap the HardwareMap
// */
// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
// List<Integer> lastTrackingEncPositions = new ArrayList<>();
// List<Integer> lastTrackingEncVels = new ArrayList<>();
//
// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
//
// startPose = new Pose();
// previousPose = new Pose();
// }
//
// /**
// * This returns the current pose estimate as a Pose.
// *
// * @return returns the current pose estimate
// */
// @Override
// public Pose getPose() {
// Pose2d pose = localizer.getPoseEstimate();
// return new Pose(pose.getX(), pose.getY(), pose.getHeading());
// }
//
// /**
// * This returns the current velocity estimate as a Pose.
// *
// * @return returns the current velocity estimate
// */
// @Override
// public Pose getVelocity() {
// Pose2d pose = localizer.getPoseVelocity();
// return new Pose(pose.getX(), pose.getY(), pose.getHeading());
// }
//
// /**
// * This returns the current velocity estimate as a Vector.
// *
// * @return returns the current velocity estimate
// */
// @Override
// public Vector getVelocityVector() {
// Pose2d pose = localizer.getPoseVelocity();
// Vector returnVector = new Vector();
// returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
// return returnVector;
// }
//
// /**
// * This sets the start pose. Any movement of the robot is treated as a displacement from the
// * start pose, so moving the start pose will move the current pose estimate the same amount.
// *
// * @param setStart the new start pose
// */
// @Override
// public void setStartPose(Pose setStart) {
// Pose oldStart = startPose;
// startPose = setStart;
// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
// }
//
// /**
// * This sets the current pose estimate. This has no effect on the start pose.
// *
// * @param setPose the new current pose estimate
// */
// @Override
// public void setPose(Pose setPose) {
// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
// }
//
// /**
// * This updates the total heading and previous pose estimate. Everything else is handled by the
// * Road Runner localizer on its own, but updating this tells you how far the robot has really
// * turned.
// */
// @Override
// public void update() {
// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
// previousPose = getPose();
// }
//
// /**
// * This returns how far the robot has actually turned.
// *
// * @return returns the total angle turned, in degrees.
// */
// @Override
// public double getTotalHeading() {
// return totalHeading;
// }
//
// /**
// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks
// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
// * multiplied together should be. If you do use that, then do be aware that the value returned is
// * the product of the Road Runner ticks to inches and the x multiplier.
// *
// * @return returns the forward multiplier
// */
// @Override
// public double getForwardMultiplier() {
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
// }
//
// /**
// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks
// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
// * multiplied together should be. If you do use that, then do be aware that the value returned is
// * the product of the Road Runner ticks to inches and the y multiplier.
// *
// * @return returns the lateral multiplier
// */
// @Override
// public double getLateralMultiplier() {
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
// }
//
// /**
// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist.
// * There really isn't a point in tuning the turning for the Road Runner localizer. This will
// * actually just return the average of the two other multipliers.
// *
// * @return returns the turning multiplier
// */
// @Override
// public double getTurningMultiplier() {
// return (getForwardMultiplier() + getLateralMultiplier()) / 2;
// }
//}

View File

@ -1,132 +0,0 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import com.acmerobotics.roadrunner.util.NanoClock;
//import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.DcMotorSimple;
//
///**
// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
// * velocity counts and allow reversing independently of the corresponding slot's motor direction.
// *
// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
// * import statements, so I'm not crediting myself as an author for this.
// *
// * @author Road Runner dev team
// * @version 1.0, 5/9/2024
// */
//public class RoadRunnerEncoder {
// private final static int CPS_STEP = 0x10000;
//
// private static double inverseOverflow(double input, double estimate) {
// // convert to uint16
// int real = (int) input & 0xffff;
// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
// real += ((real % 20) / 4) * CPS_STEP;
// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
// return real;
// }
//
// public enum Direction {
// FORWARD(1),
// REVERSE(-1);
//
// private int multiplier;
//
// Direction(int multiplier) {
// this.multiplier = multiplier;
// }
//
// public int getMultiplier() {
// return multiplier;
// }
// }
//
// private DcMotorEx motor;
// private NanoClock clock;
//
// private Direction direction;
//
// private int lastPosition;
// private int velocityEstimateIdx;
// private double[] velocityEstimates;
// private double lastUpdateTime;
//
// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
// this.motor = motor;
// this.clock = clock;
//
// this.direction = Direction.FORWARD;
//
// this.lastPosition = 0;
// this.velocityEstimates = new double[3];
// this.lastUpdateTime = clock.seconds();
// }
//
// public RoadRunnerEncoder(DcMotorEx motor) {
// this(motor, NanoClock.system());
// }
//
// public Direction getDirection() {
// return direction;
// }
//
// private int getMultiplier() {
// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
// }
//
// /**
// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
// * @param direction either reverse or forward depending on if encoder counts should be negated
// */
// public void setDirection(Direction direction) {
// this.direction = direction;
// }
//
// /**
// * Gets the position from the underlying motor and adjusts for the set direction.
// * Additionally, this method updates the velocity estimates used for compensated velocity
// *
// * @return encoder position
// */
// public int getCurrentPosition() {
// int multiplier = getMultiplier();
// int currentPosition = motor.getCurrentPosition() * multiplier;
// if (currentPosition != lastPosition) {
// double currentTime = clock.seconds();
// double dt = currentTime - lastUpdateTime;
// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
// lastPosition = currentPosition;
// lastUpdateTime = currentTime;
// }
// return currentPosition;
// }
//
// /**
// * Gets the velocity directly from the underlying motor and compensates for the direction
// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
// *
// * @return raw velocity
// */
// public double getRawVelocity() {
// int multiplier = getMultiplier();
// return motor.getVelocity() * multiplier;
// }
//
// /**
// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
// * that are lost in overflow due to velocity being transmitted as 16 bits.
// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
// *
// * @return corrected velocity
// */
// public double getCorrectedVelocity() {
// double median = velocityEstimates[0] > velocityEstimates[1]
// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
// return inverseOverflow(getRawVelocity(), median);
// }
//}

View File

@ -1,123 +0,0 @@
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
//
//import androidx.annotation.NonNull;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
//import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import java.util.Arrays;
//import java.util.List;
//
///*
// * Sample tracking wheel localizer implementation assuming the standard configuration:
// *
// * left on robot is y pos
// *
// * front of robot is x pos
// *
// * /--------------\
// * | ____ |
// * | ---- |
// * | || || |
// * | || || |
// * | |
// * | |
// * \--------------/
// *
// */
//
///**
// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an
// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
// * Pedro Pathing to avoid having imports.
// *
// * @author Road Runner dev team
// * @author Anyi Lin - 10158 Scott's Bots
// * @version 1.0, 5/9/2024
// */
//@Config
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
// public static double TICKS_PER_REV = 8192;
// public static double WHEEL_RADIUS = 1.37795; // in
// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
//
// public static double X_MULTIPLIER = 0.5008239963;
// public static double Y_MULTIPLIER = 0.5018874659;
//
// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
//
// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
//
// private List<Integer> lastEncPositions, lastEncVels;
//
// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
// super(Arrays.asList(
// new Pose2d(leftX, leftY, 0), // left
// new Pose2d(rightX, rightY, 0), // right
// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
// ));
//
// lastEncPositions = lastTrackingEncPositions;
// lastEncVels = lastTrackingEncVels;
//
// // TODO: redo the configs here
// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
//
// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
// }
//
// public void resetHeading(double heading) {
// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
// }
//
// public static double encoderTicksToInches(double ticks) {
// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
// }
//
// @NonNull
// @Override
// public List<Double> getWheelPositions() {
// int leftPos = leftEncoder.getCurrentPosition();
// int rightPos = rightEncoder.getCurrentPosition();
// int frontPos = strafeEncoder.getCurrentPosition();
//
// lastEncPositions.clear();
// lastEncPositions.add(leftPos);
// lastEncPositions.add(rightPos);
// lastEncPositions.add(frontPos);
//
// return Arrays.asList(
// encoderTicksToInches(leftPos) * X_MULTIPLIER,
// encoderTicksToInches(rightPos) * X_MULTIPLIER,
// encoderTicksToInches(frontPos) * Y_MULTIPLIER
// );
// }
//
// @NonNull
// @Override
// public List<Double> getWheelVelocities() {
// int leftVel = (int) leftEncoder.getCorrectedVelocity();
// int rightVel = (int) rightEncoder.getCorrectedVelocity();
// int frontVel = (int) strafeEncoder.getCorrectedVelocity();
//
// lastEncVels.clear();
// lastEncVels.add(leftVel);
// lastEncVels.add(rightVel);
// lastEncVels.add(frontVel);
//
// return Arrays.asList(
// encoderTicksToInches(leftVel) * X_MULTIPLIER,
// encoderTicksToInches(rightVel) * X_MULTIPLIER,
// encoderTicksToInches(frontVel) * Y_MULTIPLIER
// );
// }
//}

View File

@ -1,317 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.*;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
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.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the three wheel odometry set up with the IMU to have more accurate heading
* readings. The diagram below, which is modified from Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Logan Nash
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/9/2024
*/
@Config
public class ThreeWheelIMULocalizer extends Localizer {
private HardwareMap hardwareMap;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
private Pose leftEncoderPose;
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
public final IMU imu;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.004;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.0036;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static boolean useIMU = true;
/**
* This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public ThreeWheelIMULocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) {
hardwareMap = map;
imu = hardwareMap.get(IMU.class, IMU);
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(-7.625, 6.19375, 0);
rightEncoderPose = new Pose(-7.625, -6.19375, 0);
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
// 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);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
totalHeading = 0;
resetEncoders();
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders. Then, the robot's global change in position is calculated
* using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders.
*/
public void updateEncoders() {
leftEncoder.update();
rightEncoder.update();
strafeEncoder.update();
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
strafeEncoder.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
// theta/turning
if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) {
returnMatrix.set(2, 0, deltaRadians);
} else {
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
}
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return TURN_TICKS_TO_RADIANS;
}
/**
* This resets the IMU.
*/
public void resetIMU() {
imu.resetYaw();
}
}

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.003;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.0029;//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(0.25, 6.25, 0);
rightEncoderPose = new Pose(0.25, -6.25, 0);
strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90));
hardwareMap = map;

View File

@ -1,302 +0,0 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorEx;
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.Encoder;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
/**
* This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
* localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from
* Road Runner, shows a typical set up.
*
* The view is from the top of the robot looking downwards.
*
* left on robot is the y positive direction
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
*/
@Config
public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work
private HardwareMap hardwareMap;
private IMU imu;
private Pose startPose;
private Pose displacementPose;
private Pose currentVelocity;
private Matrix prevRotationMatrix;
private NanoTimer timer;
private long deltaTimeNano;
private Encoder forwardEncoder;
private Encoder strafeEncoder;
private Pose forwardEncoderPose;
private Pose strafeEncoderPose;
private double previousIMUOrientation;
private double deltaRadians;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
/**
* This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
* facing 0 heading.
*
* @param map the HardwareMap
*/
public TwoWheelLocalizer(HardwareMap map) {
this(map, new Pose());
}
/**
* This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
* specifying the starting pose of the localizer.
*
* @param map the HardwareMap
* @param setStartPose the Pose to start from
*/
public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) {
// TODO: replace these with your encoder positions
forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;
imu = hardwareMap.get(IMU.class, "imu");
// TODO: replace this with your IMU's orientation
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
// TODO: replace these with your encoder ports
forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
forwardEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose);
timer = new NanoTimer();
deltaTimeNano = 1;
displacementPose = new Pose();
currentVelocity = new Pose();
previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = 0;
}
/**
* This returns the current pose estimate.
*
* @return returns the current pose estimate as a Pose
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, displacementPose);
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Pose
*/
@Override
public Pose getVelocity() {
return currentVelocity.copy();
}
/**
* This returns the current velocity estimate.
*
* @return returns the current velocity estimate as a Vector
*/
@Override
public Vector getVelocityVector() {
return currentVelocity.getVector();
}
/**
* This sets the start pose. Changing the start pose should move the robot as if all its
* previous movements were displacing it from its new start pose.
*
* @param setStart the new start pose
*/
@Override
public void setStartPose(Pose setStart) {
startPose = setStart;
}
/**
* This sets the Matrix that contains the previous pose's heading rotation.
*
* @param heading the rotation of the Matrix
*/
public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3);
prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading));
prevRotationMatrix.set(1, 1, Math.cos(heading));
prevRotationMatrix.set(2, 2, 1.0);
}
/**
* This sets the current pose estimate. Changing this should just change the robot's current
* pose estimate, not anything to do with the start pose.
*
* @param setPose the new current pose estimate
*/
@Override
public void setPose(Pose setPose) {
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
resetEncoders();
}
/**
* This updates the elapsed time timer that keeps track of time between updates, as well as the
* change position of the Encoders and the IMU readings. Then, the robot's global change in
* position is calculated using the pose exponential method.
*/
@Override
public void update() {
deltaTimeNano = timer.getElapsedTime();
timer.resetTimer();
updateEncoders();
Matrix robotDeltas = getRobotDeltas();
Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(2, 2, 1.0);
} else {
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
transformation.set(2, 2, 1.0);
}
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
totalHeading += globalDeltas.get(2, 0);
}
/**
* This updates the Encoders as well as the IMU.
*/
public void updateEncoders() {
forwardEncoder.update();
strafeEncoder.update();
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}
/**
* This resets the Encoders.
*/
public void resetEncoders() {
forwardEncoder.reset();
strafeEncoder.reset();
}
/**
* This calculates the change in position from the perspective of the robot using information
* from the Encoders and IMU.
*
* @return returns a Matrix containing the robot relative movement.
*/
public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1);
// x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
//y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
// theta/turning
returnMatrix.set(2,0, deltaRadians);
return returnMatrix;
}
/**
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
*
* @return returns how far the robot has turned in total, in radians.
*/
public double getTotalHeading() {
return totalHeading;
}
/**
* This returns the multiplier applied to forward movement measurement to convert from encoder
* ticks to inches. This is found empirically through a tuner.
*
* @return returns the forward ticks to inches multiplier
*/
public double getForwardMultiplier() {
return FORWARD_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
* encoder ticks to inches. This is found empirically through a tuner.
*
* @return returns the lateral/strafe ticks to inches multiplier
*/
public double getLateralMultiplier() {
return STRAFE_TICKS_TO_INCHES;
}
/**
* This returns the multiplier applied to turning movement measurement to convert from encoder
* ticks to radians. This is found empirically through a tuner.
*
* @return returns the turning ticks to radians multiplier
*/
public double getTurningMultiplier() {
return 1;
}
/**
* This resets the IMU.
*/
public void resetIMU() {
imu.resetYaw();
}
}

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

View File

@ -1,5 +1,9 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
@ -58,8 +62,10 @@ public class LocalizationTest extends OpMode {
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);

View File

@ -40,9 +40,9 @@ public class FollowerConstants {
// Translational PIDF coefficients (don't use integral)
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
0.1,
.25,
0,
0.01,
0.0375,
0);
// Translational Integral
@ -53,14 +53,14 @@ public class FollowerConstants {
0);
// Feed forward constant added on to the translational PIDF
public static double translationalPIDFFeedForward = 0.015;
public static double translationalPIDFFeedForward = 0.00;
// Heading error PIDF coefficients
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
2,
0,
.025,
0.0375,
0);
// Feed forward constant added on to the heading PIDF
@ -69,9 +69,9 @@ public class FollowerConstants {
// Drive PIDF coefficients
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.006,
0.00375,
0,
0.00001,
0.00003,
0.8,
0);

View File

@ -93,11 +93,13 @@ public class ForwardVelocityTuner extends OpMode {
}
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
telemetryA.update();
// telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
// telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
// telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
// telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
//
//
// telemetryA.update();
}
@ -138,6 +140,13 @@ public class ForwardVelocityTuner extends OpMode {
velocities.add(currentVelocity);
velocities.remove(0);
}
telemetryA.addData("x", poseUpdater.getPose().getX());
telemetryA.addData("y", poseUpdater.getPose().getY());
telemetryA.addData("heading", poseUpdater.getPose().getHeading());
telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
telemetryA.update();
} else {
double average = 0;
for (Double velocity : velocities) {
@ -146,7 +155,12 @@ public class ForwardVelocityTuner extends OpMode {
average /= (double) velocities.size();
telemetryA.addData("forward velocity:", average);
telemetryA.update();
// telemetryA.addData("x", poseUpdater.getPose().getX());
// telemetryA.addData("y", poseUpdater.getPose().getY());
// telemetryA.addData("heading", poseUpdater.getPose().getHeading());
// telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
// telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
// telemetryA.update();
}
}
}

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

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

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

View File

@ -0,0 +1,18 @@
package org.firstinspires.ftc.teamcode.states;
public class FieldStates {
public enum FieldLocation {
BUCKET, SUBMARINE, FLOATING, TRAVELING
}
private FieldLocation fieldLocation;
public FieldLocation getFieldLocation() {
return fieldLocation;
}
public void setFieldLocation(FieldLocation fieldLocation) {
this.fieldLocation = fieldLocation;
}
}

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

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

View File

@ -0,0 +1,131 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;
public class DualMotorSliderSubsystem {
/*
liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object
Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet.
It expects an object of type "DcMotorEx".
*/
private DcMotorEx liftSlideLeft;
/*
liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object
Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet.
It expects an object of type "DcMotorEx".
*/
private DcMotorEx liftSlideRight;
/*
targetPosition - Variable that holds target position of slides.
*/
private int targetPosition = 0;
/*
getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and,
instead, use a "setter" to set the target position value or
a "getter" to get the target position value.
*/
public void setTargetPosition(int value) {
targetPosition = value;
}
private int getTargetPosition() { return targetPosition; }
/*
PID - Proportional/Integral/Derivative Values
For a dual motor linear slide, we only tune the P - Proportion.
The Proportion variable (kp) answers the question "how fast do we want to get to our destination?"
It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
Since we're going straight, we don't need to worry about.
*/
public final static double kp = 0.0015, ki = 0, kd = 0;
/*
lastError/integralSum/timer - These 3 variables are placeholders in determining how much
power to send to both motors.
*/
private double lastError = 0;
private double integralSum = 0;
private ElapsedTime timer = new ElapsedTime();
public DualMotorSliderSubsystem(HardwareMap hardwareMap) {
/*
liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable
We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the
driver hub configuration name that matches the motor on the slide.
*/
liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
}
public void init() {
/*
Initialize the motors with the following settings (assuming slide is at the very bottom position):
- Reset the encoders to be zero
- When the motor stops moving (zero power), brake. This means we can't move the motors, not even gravity.
- Sets the motor to run without the encoder. This doesn't mean we won't use the encoder values (because we will).
It just means to not FULLY depend on them, we will just prefer to use motor power instead.
*/
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
/*
To calculate power, we send the targetPosition value (how high we want the slider motors to
be based on motor 'ticks') to calculatePower function.
For every loop that occurs, it will constantly calculate power. So long as we're far away from
our "target" position, we will get as much power as possible. The closer we get, the lower the
power we will receive.
*/
public void update() {
double power = calculatePower();
liftSlideLeft.setPower(power);
liftSlideRight.setPower(power);
}
/*
Calculating power - To calculate the power, we determine the proportion, derivative and
integral of our closed loop system.
For more information, please visit:
- Introduction to Closed Loop System:
- https://www.ctrlaltftc.com/introduction-to-closed-loop-control
- The PID controller:
- https://www.ctrlaltftc.com/the-pid-controller
*/
private double calculatePower() {
double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
integralSum += error * timer.seconds();
double derivative = (error - lastError) / timer.seconds();
lastError = error;
timer.reset();
return (error * kp) + (derivative * kd) + (integralSum * ki);
}
public void toLowBucketPosition() {
setTargetPosition(1500);
}
public void toHighBucketPosition() {
setTargetPosition(3000);
}
}

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

View File

@ -8,7 +8,12 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -26,7 +31,7 @@ public class MotorsSubsystem {
public DcMotor backRightMotor;
public enum TravelState {
PARKED, BUCKET, SUBMARINE
STOPPED, MOVING
}
public TravelState travelState;
@ -36,7 +41,7 @@ public class MotorsSubsystem {
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = 1.0;
this.power = MAX_POWER;
}
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
@ -60,6 +65,8 @@ public class MotorsSubsystem {
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.setState(TravelState.STOPPED);
}
public void setFrontLeftMotorPower(double power) {
@ -78,46 +85,6 @@ public class MotorsSubsystem {
backRightMotor.setPower(power);
}
public void calculateTrajectory(Gamepad gamepad1) {
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double leftBackPower = axial - lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// Send calculated power to wheels
this.setFrontLeftMotorPower(leftFrontPower * this.power);
this.setFrontRightMotorPower(rightFrontPower * this.power);
this.setBackLeftMotorPower(leftBackPower * this.power);
this.setBackRightMotorPower(rightBackPower * this.power);
// Show the elapsed game time and wheel power.
this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.update();
}
public void setState(TravelState travelState) {
this.travelState = travelState;
@ -127,4 +94,12 @@ public class MotorsSubsystem {
return this.travelState;
}
public void setPower(DcMotor motor, double power) {
motor.setPower(power);
if (power < 0.05) {
this.setState(TravelState.MOVING);
} else {
this.setState(TravelState.STOPPED);
}
}
}

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

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

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

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

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

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

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

View File

@ -16,6 +16,9 @@ dependencies {
implementation 'org.firstinspires.ftc:Vision:10.1.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
implementation "com.acmerobotics.roadrunner:core:1.0.0"
implementation "com.acmerobotics.roadrunner:actions:1.0.0"
implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
}