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

Compare commits

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

23 Commits
branch-rc- ... branch-sil

Author SHA1 Message Date
carlos
aa496b8237 Ollie's work committed 2024-12-08 12:48:30 -08:00
robotics1
66f3339e26 Working Pre loaded auto! Can score 11 points consistently! 2024-11-14 17:10:46 -08:00
robotics1
ad0a8d3374 Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493 2024-11-14 16:23:46 -08:00
Carlos
dc71eb4317 Feature where driver can override centricity (robot vs field). 2024-11-13 15:09:59 -08:00
Carlos
94144780b8 Tentative fix for robot/runBlocking problem as per issue #2 2024-11-13 09:13:24 -08:00
Carlos
a362d2e004 Massive upgrade and shift of files 2024-11-12 23:16:23 -08:00
robotics1
2008c3cd88 Working somehitng i don't know 2024-11-12 16:55:35 -08:00
robotics1
c5be3cd932 Silver Branch Code 2024-11-03 09:13:37 -08:00
Carlos Rivas
7d83b9c254 Add sample cometbot package 2024-10-31 15:37:09 -07:00
Carlos
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
robotics1
0cfb57c643 Merge remote-tracking branch 'origin/branch-silver-14493' into branch-silver-14493 2024-10-17 11:07:25 -07:00
robotics1
50db1f9175 Add new files (2nd try) 2024-10-17 11:07:10 -07:00
robotics1
552bb3e25a Add new files 2024-10-17 11:06:13 -07:00
carlos
c1076a832c Rebased changes 2024-10-17 11:05:12 -07:00
carlos
7bf8b0c357 Add files 2024-10-17 11:04:02 -07:00
Carlos
adfab3e8af Add sample code 2024-10-15 15:44:49 -07:00
Carlos
0f7ea50907 Updated constants for silver and it, somehow, works 2024-10-15 15:43:53 -07:00
Carlos
239f168540 Retuned to success, PIDF (P = .05) and translationalPIDFeedForward is back to 0 2024-10-15 13:59:02 -07:00
robotics2
f2bcdcc55a Add constants 2024-10-01 17:05:54 -07:00
carlos
e08aac773d Static entries for arm servos 2024-10-01 15:45:02 -07:00
Carlos Rivas
3950a83ac1 Added encoder naming and usage 2024-10-01 10:19:40 -07:00
Carlos
552ff3f339 Updated to 10.1 2024-09-30 22:37:16 -07:00
Carlos
4eef485dab Re-wired and reconfigured 2024-09-30 20:39:24 -07:00
65 changed files with 1975 additions and 1974 deletions
Expand all files Collapse all files

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

@ -1,133 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning")
public class AsherPathBlueV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(9.757, 84.983, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath(
// Line 1
new BezierCurve(
new Point(7.800, 87.5, Point.CARTESIAN),
new Point(19.000, 116.000, Point.CARTESIAN),
new Point(93.000, 118.000, Point.CARTESIAN),
new Point(45.000, 115.000, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierLine(
new Point(45.000, 115.000, Point.CARTESIAN),
new Point(14.000, 126.000, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierCurve(
new Point(14.000, 126.000, Point.CARTESIAN),
new Point(43.000, 112.500, Point.CARTESIAN),
new Point(64.000, 92.000, Point.CARTESIAN),
new Point(77.000, 117.000, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(77.000, 117.000, Point.CARTESIAN),
new Point(20.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 5
new BezierCurve(
new Point(20.000, 135.000, Point.CARTESIAN),
new Point(113.000, 95.000, Point.CARTESIAN),
new Point(69.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 6
new BezierLine(
new Point(69.000, 135.000, Point.CARTESIAN),
new Point(20.500, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 7
new BezierCurve(
new Point(20.500, 135.000, Point.CARTESIAN),
new Point(101.500, 95.500, Point.CARTESIAN),
new Point(72.500, 95.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
/*
* End of only update this path
*/
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,35 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.runmodes.Auto;
@Autonomous(name = "CometBot Auto", group = "Debug")
public class CometBotAuto extends OpMode {
public Auto auto;
@Override
public void init() {
auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap));
}
@Override
public void start() {
auto.start();
}
@Override
public void loop() {
auto.update();
telemetry.addData("Arm State", auto.arm.getState());
telemetry.addData("Arm Position", auto.arm.getPosition());
telemetry.addData("Claw State", auto.claw.getState());
telemetry.addData("Wrist State", auto.wrist.getState());
telemetry.addData("Wrist Position", auto.wrist.getPosition());
telemetry.update();
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java Normal file
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", group = "Development")
public class CometBotDevAuto extends OpMode {
public CometBotAutoDevelopment runMode;
@Override
public void init() {
this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2);
this.runMode.init();
}
@Override
public void loop() {
this.runMode.update();
telemetry.update();
}
}

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

@ -1,31 +0,0 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
@TeleOp(name="ComeBot Drive", group="Debug")
@Disabled
public class CometBotDrive extends OpMode {
private Teleop teleop;
@Override
public void init() {
teleop = new Teleop(hardwareMap,
telemetry,
new Follower(hardwareMap),
gamepad1);
teleop.start();
}
@Override
public void loop() {
teleop.update();
}
}

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

@ -1,97 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix", group = "Debug")
@Disabled
public class DevTeleOpRemix extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
motors = new MotorsSubsystem(hardwareMap, telemetry, power);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
motors.calculateTrajectory(gamepad1);
}
}

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

@ -1,107 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
@Disabled
public class DevTeleOpRemixDeux extends OpMode {
private Follower follower;
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
follower = new Follower(hardwareMap);
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
motors = new MotorsSubsystem(hardwareMap, telemetry);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
follower.setMaxPower(this.power);
follower.startTeleopDrive();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
}

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

@ -1,196 +0,0 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import android.graphics.Point;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop", group = "Debug")
public class DevTeleop extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
private double MAX_POWER = .45;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
//pick up
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
//claw open close
if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
claw.switchState();
}
}
/* public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
*/
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
//low bucket
if (currentGamepad2.a && !previousGamepad2.a) {
lift.toLowBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
//high basket
if (currentGamepad2.b && !previousGamepad2.b) {
lift.toHighBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
//
if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){
lift.toFloor();
arm.bucketArm();
wrist.floorWrist();
}
}
public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){
if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){
lift.toHover();
wrist.floorWrist();
arm.engageArm();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
// theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
theHighBucketScore(lift, wrist, arm);
theTravel(lift, arm, wrist);
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// Send calculated power to wheels
frontLeftMotor.setPower(leftFrontPower * MAX_POWER);
frontRightMotor.setPower(rightFrontPower * MAX_POWER);
backLeftMotor.setPower(leftBackPower * MAX_POWER);
backRightMotor.setPower(rightBackPower * MAX_POWER);
// Show the elapsed game time and wheel power.
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Current Lift Position", lift.getPosition());
telemetry.update();
}
}

64
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java Normal file
View File

@ -0,0 +1,64 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
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.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine3;
@Autonomous(name = "BlueNetAuto", group = "Dev")
public class NetAuto extends OpMode {
public Follower follower;
public AutoLine1 myFirstPath = new AutoLine1();
public AutoLine2 mySecondPath = new AutoLine2();
public int pathState = 0;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(0.65);
myFirstPath.moveToAutoLine1(follower);
}
@Override
public void loop() {
follower.update();
switch(pathState) {
case 0:
if (!follower.isBusy()) {
pathState = 1;
mySecondPath.moveToAutoLine2(follower);
}
case 1:
if (!follower.isBusy()) {
System.out.println("Finished");
}
}
// switch(pathState) {
// case 0:
// if (!follower.isBusy()) {
// mySecondPath.moveToAutoLine2(follower);
// pathState = 1;
// }
// case 1:
// if (!follower.isBusy()) {
// pathState = 2;
// }
// case 2:
// // set path 3
// // as if busy, if not, set path 4 and so on.
// System.out.print("we're at the end");
//
// }
follower.telemetryDebug(telemetry);
}
}

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

@ -10,15 +10,12 @@ public class PedroConstants {
/* /*
Robot parameters Robot parameters
*/ */
// Turn localizer - -0.003
// Robot motor configurations // 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 = "front-left";
public static final String FRONT_LEFT_MOTOR = "Drive front lt"; public static final String BACK_LEFT_MOTOR = "back-left";
public static final String BACK_LEFT_MOTOR = "Drive back lt"; public static final String FRONT_RIGHT_MOTOR = "front-right";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt"; public static final String BACK_RIGHT_MOTOR = "back-right";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
// Robot motor direction // Robot motor direction
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE; public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
@ -26,43 +23,55 @@ public class PedroConstants {
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_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 // Robot IMU configuration
public static final String IMU = "imu"; public static final String IMU = "imu";
// Robot IMU placement // Robot IMU placement
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN; = RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT; = RevHubOrientationOnRobot.UsbFacingDirection.UP;
// Robot encoders // Robot encoders
public static final String LEFT_ENCODER = "encoder left"; // NOTE: Encoders are plugged into the same ports as motors hence the weird names
public static final String RIGHT_ENCODER = "encoder right"; public static final String RIGHT_ENCODER = "back-right"; //0
public static final String BACK_ENCODER = "encoder back"; public static final String BACK_ENCODER = "front-right"; //1
public static final String LEFT_ENCODER = "front-left"; //2
// Robot encoder direction // Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD; 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.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD; public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
// Arm config
public static final String SLIDE_MOTOR = "SlideMotor";
public static final String Claw_Servo = "ClawServo";
public static final String Wrist_Servo = "WristServo";
public static final String Arm_Servo = "ArmServo";
/* /*
Pedro's parameters Pedro's parameters
*/ */
// The weight of the robot in Kilograms // 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 // Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 51.4598; public static final double ROBOT_SPEED_FORWARD = 51.5;
// Maximum velocity of the robot going right // Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 28.7119; public static final double ROBOT_SPEED_LATERAL = 28.7;
// Rate of deceleration when power is cut-off when the robot is moving forward // Rate of deceleration when power is cut-off when the robot is moving forward
public static final double FORWARD_ZERO_POWER_ACCEL = -59.805; public static final double FORWARD_ZERO_POWER_ACCEL = -59.8;
// Rate of deceleration when power is cut-off when the robot is moving to the right // Rate of deceleration when power is cut-off when the robot is moving to the right
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672; public static final double LATERAL_ZERO_POWER_ACCEL = -99.7;
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop // 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 = 3.5;

122
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java Normal file
View File

@ -0,0 +1,122 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
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;
@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(7.875, 89.357);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
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(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 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 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 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(15.107, 130.339, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
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(57.857, 133.071, Point.CARTESIAN),
new Point(18.964, 134.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
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();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

151
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java Normal file
View File

@ -0,0 +1,151 @@
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 = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning")
public class BlueNonBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10.929, 55.446, 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(10.929, 55.446, Point.CARTESIAN),
new Point(42.429, 46.446, Point.CARTESIAN),
new Point(36.321, 38.089, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(36.321, 38.089, Point.CARTESIAN),
new Point(59.786, 36.643, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(59.786, 36.643, Point.CARTESIAN),
new Point(59.304, 24.750, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(59.304, 24.750, Point.CARTESIAN),
new Point(13.982, 23.946, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(13.982, 23.946, Point.CARTESIAN),
new Point(59.464, 24.429, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(59.464, 24.429, Point.CARTESIAN),
new Point(58.982, 15.268, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierLine(
new Point(58.982, 15.268, Point.CARTESIAN),
new Point(13.821, 14.464, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(13.821, 14.464, Point.CARTESIAN),
new Point(58.661, 13.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(58.661, 13.500, Point.CARTESIAN),
new Point(58.339, 8.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierLine(
new Point(58.339, 8.679, Point.CARTESIAN),
new Point(14.625, 8.518, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
follower.followPath(path, true);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* 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, true);
}
follower.telemetryDebug(telemetryA);
}
}

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

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

142
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java Normal file
View File

@ -0,0 +1,142 @@
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
public class CometBotAutoDevelopment {
/*
Subsystems
*/
private MotorsSubsystem motors;
/*
Controllers
*/
public Gamepad GP1;
public Gamepad GP2;
public Gamepad currentGP1;
public Gamepad previousGP1;
public Gamepad currentGP2;
public Gamepad previousGP2;
private Telemetry telemetry;
public FieldStates fieldStates;
private boolean centricity = false;
private Follower follower;
private HardwareMap hardwareMap;
public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry);
this.GP1 = gp1;
this.GP2 = gp2;
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.currentGP1 = new Gamepad();
this.currentGP2 = new Gamepad();
this.previousGP1 = new Gamepad();
this.previousGP2 = new Gamepad();
this.fieldStates = new FieldStates();
this.follower = new Follower(hardwareMap);
}
public class ZeroOutPower implements Action {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
follower = new Follower(hardwareMap);
follower.setMaxPower(0);
System.out.println("Running ZeroOutPower");
return follower.isBusy();
}
}
public class ReturnToMaxPower implements Action {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
follower = new Follower(hardwareMap);
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
System.out.println("Running ReturnToMaxPower");
return follower.isBusy();
}
}
public Action zeroOutPower() {
return new ZeroOutPower();
}
public Action returnToMaxPower() {
return new ReturnToMaxPower();
}
public void init() {
this.motors.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
public void update() {
this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.toFixMotorBlockingIssueFirstMethod();
this.toFixMotorBlockingIssueSecondMethod();
this.changeCentricity();
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity);
follower.update();
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
}
public void changeCentricity() {
if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
this.centricity = !centricity;
this.follower.breakFollowing();
this.follower.startTeleopDrive();
}
}
public void toFixMotorBlockingIssueFirstMethod() {
if (this.currentGP1.cross && !this.previousGP1.cross) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.zeroOutPower(),
new SleepAction(3),
this.returnToMaxPower()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
}
}
public void toFixMotorBlockingIssueSecondMethod() {
if (this.currentGP1.circle && !this.previousGP1.circle) {
this.follower.breakFollowing();
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
new SleepAction(3)
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@ -8,7 +8,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; 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.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
@ -26,68 +25,67 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
* @version 1.0, 3/12/2024 * @version 1.0, 3/12/2024
*/ */
@Config @Config
@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning") @Autonomous (name = "Test", group = "Autonomous Pathing Tuning")
public class AsherOrientBlue extends OpMode { public class AutoTest extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
private Follower follower; private Follower follower;
private PathChain path; private PathChain test;
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 @Override
public void init() { public void init() {
follower = new Follower(hardwareMap); follower = new Follower(hardwareMap);
follower.setMaxPower(.4); test = follower.pathBuilder()
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath( .addPath(
// Line 1
new BezierLine( new BezierLine(
new Point(20.500, 7.800, Point.CARTESIAN), new Point(8.000, 60.000, Point.CARTESIAN),
new Point(20.500, 87.500, Point.CARTESIAN) new Point(18.000, 60.000, Point.CARTESIAN)
) )
) )
.addPath( .addPath(
// Line 2 // Line 2
new BezierCurve(
new Point(18.000, 60.000, Point.CARTESIAN),
new Point(18.000, 23.000, Point.CARTESIAN),
new Point(48.000, 23.000, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierLine( new BezierLine(
new Point(20.500, 87.500, Point.CARTESIAN), new Point(48.000, 23.000, Point.CARTESIAN),
new Point(7.800, 87.500, Point.CARTESIAN) new Point(60.000, 36.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(90)).build(); .addPath(
/* // Line 4
* End of only update this path new BezierLine(
*/ new Point(60.000, 36.000, Point.CARTESIAN),
new Point(60.000, 49.000, Point.CARTESIAN)
)
).build();
follower.followPath(path);
follower.followPath(test);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update(); 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 @Override
public void loop() { public void loop() {
follower.update(); follower.update();
if (follower.atParametricEnd()) { if (follower.atParametricEnd()) {
follower.followPath(path); follower.followPath(test);
} }
follower.telemetryDebug(telemetryA); follower.telemetryDebug(telemetryA);
} }
} }

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BasicOmniOpMode_Linear.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * 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;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;

128
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java Normal file
View File

@ -0,0 +1,128 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
public class BlueAuto {
public void GeneratedPath() {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(9.757, 84.983, Point.CARTESIAN),
new Point(8.442, 129.227, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 2
new BezierLine(
new Point(8.442, 129.227, Point.CARTESIAN),
new Point(52.762, 101.628, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 3
new BezierLine(
new Point(52.762, 101.628, Point.CARTESIAN),
new Point(79.224, 116.564, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 4
new BezierLine(
new Point(79.224, 116.564, Point.CARTESIAN),
new Point(54.548, 130.525, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 5
new BezierLine(
new Point(54.548, 130.525, Point.CARTESIAN),
new Point(12.338, 133.772, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 6
new BezierLine(
new Point(12.338, 133.772, Point.CARTESIAN),
new Point(52.437, 101.628, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 7
new BezierLine(
new Point(52.437, 101.628, Point.CARTESIAN),
new Point(71.594, 120.947, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(71.594, 120.947, Point.CARTESIAN),
new Point(52.275, 120.785, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 9
new BezierLine(
new Point(52.275, 120.785, Point.CARTESIAN),
new Point(11.039, 131.012, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 10
new BezierLine(
new Point(11.039, 131.012, Point.CARTESIAN),
new Point(70.782, 120.460, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 11
new BezierLine(
new Point(70.782, 120.460, Point.CARTESIAN),
new Point(50.327, 142.377, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 12
new BezierLine(
new Point(50.327, 142.377, Point.CARTESIAN),
new Point(13.799, 134.422, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 13
new BezierLine(
new Point(13.799, 134.422, Point.CARTESIAN),
new Point(13.799, 134.422, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 14
new BezierLine(
new Point(13.799, 134.422, Point.CARTESIAN),
new Point(71.919, 103.901, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
@ -35,7 +35,7 @@ public class BlueBasketAuto extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75); private final Pose startPose = new Pose(11.25, 95.75, 0);
/** /**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
@ -140,7 +140,7 @@ public class BlueBasketAuto extends OpMode {
) )
) )
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
follower.followPath(path); follower.followPath(path, true);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update(); telemetryA.update();
@ -154,7 +154,7 @@ public class BlueBasketAuto extends OpMode {
public void loop() { public void loop() {
follower.update(); follower.update();
if (follower.atParametricEnd()) { if (follower.atParametricEnd()) {
follower.followPath(path); follower.followPath(path, true);
} }
follower.telemetryDebug(telemetryA); follower.telemetryDebug(telemetryA);
} }

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots; package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;

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

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

109
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java Normal file
View File

@ -0,0 +1,109 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
public class GeneratedPath {
public GeneratedPath() {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(9.757, 84.983, Point.CARTESIAN),
new Point(28.573, 76.302, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 2
new BezierLine(
new Point(28.573, 76.302, Point.CARTESIAN),
new Point(36.203, 76.140, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 3
new BezierLine(
new Point(36.203, 76.140, Point.CARTESIAN),
new Point(35.067, 35.716, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 4
new BezierLine(
new Point(35.067, 35.716, Point.CARTESIAN),
new Point(73.705, 34.742, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 5
new BezierLine(
new Point(73.705, 34.742, Point.CARTESIAN),
new Point(73.705, 24.839, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 6
new BezierLine(
new Point(73.705, 24.839, Point.CARTESIAN),
new Point(7.630, 26.462, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 7
new BezierLine(
new Point(7.630, 26.462, Point.CARTESIAN),
new Point(64.126, 22.728, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(64.126, 22.728, Point.CARTESIAN),
new Point(63.964, 13.150, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 9
new BezierLine(
new Point(63.964, 13.150, Point.CARTESIAN),
new Point(12.338, 15.260, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 10
new BezierLine(
new Point(12.338, 15.260, Point.CARTESIAN),
new Point(63.802, 13.150, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 11
new BezierLine(
new Point(63.802, 13.150, Point.CARTESIAN),
new Point(63.639, 11.689, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 12
new BezierLine(
new Point(63.639, 11.689, Point.CARTESIAN),
new Point(12.014, 11.689, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
}
}

99
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java Normal file
View File

@ -0,0 +1,99 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
public class RedAuto {
public class GeneratedPath {
public GeneratedPath() {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(131.499, 58.931, Point.CARTESIAN),
new Point(131.986, 18.183, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 2
new BezierLine(
new Point(131.986, 18.183, Point.CARTESIAN),
new Point(90.264, 40.911, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 3
new BezierLine(
new Point(90.264, 40.911, Point.CARTESIAN),
new Point(83.445, 26.300, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 4
new BezierLine(
new Point(83.445, 26.300, Point.CARTESIAN),
new Point(136.207, 14.286, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 5
new BezierLine(
new Point(136.207, 14.286, Point.CARTESIAN),
new Point(81.497, 24.352, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 6
new BezierLine(
new Point(81.497, 24.352, Point.CARTESIAN),
new Point(82.634, 12.988, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 7
new BezierLine(
new Point(82.634, 12.988, Point.CARTESIAN),
new Point(133.935, 11.364, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(133.935, 11.364, Point.CARTESIAN),
new Point(82.309, 11.689, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 9
new BezierLine(
new Point(82.309, 11.689, Point.CARTESIAN),
new Point(83.445, 2.598, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 10
new BezierLine(
new Point(83.445, 2.598, Point.CARTESIAN),
new Point(132.149, 10.390, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
}
}
}

185
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java Normal file
View File

@ -0,0 +1,185 @@
/* Copyright (c) 2017 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.projects;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import java.util.Locale;
/*
* This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit.
*
* Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
* Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
*
* 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
*
* @see <a href="http://www.adafruit.com/products/2472">Adafruit IMU</a>
*/
@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
public class SensorBNO055IMU extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
// State
//----------------------------------------------------------------------------------------------
// The IMU sensor object
BNO055IMU imu;
// State used for updating telemetry
Orientation angles;
Acceleration gravity;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override public void runOpMode() {
// Set up the parameters with which we will use our IMU. Note that integration
// algorithm here just reports accelerations to the logcat log; it doesn't actually
// provide positional information.
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode
parameters.loggingEnabled = true;
parameters.loggingTag = "IMU";
parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
// Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
// on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
// and named "imu".
imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
imu.initialize(parameters);
// Set up our telemetry dashboard
composeTelemetry();
// Wait until we're told to go
waitForStart();
// Start the logging of measured acceleration
imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
// Loop and update the dashboard
while (opModeIsActive()) {
telemetry.update();
}
}
//----------------------------------------------------------------------------------------------
// Telemetry Configuration
//----------------------------------------------------------------------------------------------
void composeTelemetry() {
// At the beginning of each telemetry update, grab a bunch of data
// from the IMU that we will then display in separate lines.
telemetry.addAction(new Runnable() { @Override public void run()
{
// Acquiring the angles is relatively expensive; we don't want
// to do that in each of the three items that need that info, as that's
// three times the necessary expense.
angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
gravity = imu.getGravity();
}
});
telemetry.addLine()
.addData("status", new Func<String>() {
@Override public String value() {
return imu.getSystemStatus().toShortString();
}
})
.addData("calib", new Func<String>() {
@Override public String value() {
return imu.getCalibrationStatus().toString();
}
});
telemetry.addLine()
.addData("heading", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.firstAngle);
}
})
.addData("roll", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.secondAngle);
}
})
.addData("pitch", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.thirdAngle);
}
});
telemetry.addLine()
.addData("grvty", new Func<String>() {
@Override public String value() {
return gravity.toString();
}
})
.addData("mag", new Func<String>() {
@Override public String value() {
return String.format(Locale.getDefault(), "%.3f",
Math.sqrt(gravity.xAccel*gravity.xAccel
+ gravity.yAccel*gravity.yAccel
+ gravity.zAccel*gravity.zAccel));
}
});
}
//----------------------------------------------------------------------------------------------
// Formatting
//----------------------------------------------------------------------------------------------
String formatAngle(AngleUnit angleUnit, double angle) {
return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
}
String formatDegrees(double degrees){
return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
}
}

229
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java Normal file
View File

@ -0,0 +1,229 @@
/* Copyright (c) 2017 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.projects;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ReadWriteFile;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import java.io.File;
import java.util.Locale;
/*
* This OpMode calibrates a BNO055 IMU per
* "Section 3.11 Calibration" of the BNO055 specification.
*
* Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
* Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
*
* Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the
* default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without
* manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them
* again at each run can help reduce the time that automatic calibration requires.
*
* This summary of the calibration process from Intel is informative:
* http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html
*
* "This device requires calibration in order to operate accurately. [...] Calibration data is
* lost on a power cycle. See one of the examples for a description of how to calibrate the device,
* but in essence:
*
* There is a calibration status register available [...] that returns the calibration status
* of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS).
* Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally]
* involves certain motions to get all 4 values at 3. The motions are as follows (though see the
* datasheet for more information):
*
* 1. GYR: Simply let the sensor sit flat for a few seconds.</ol>
* 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45
* degrees, hold for a few seconds, then continue rotating another 45 degrees and
* hold, etc. 6 or more movements of this type may be required. You can move through
* any axis you desire, but make sure that the device is lying at least once
* perpendicular to the x, y, and z axis.</ol>
* 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.</ol>
* 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue
* slowly moving the device though various axes until it does."</ol>
*
* To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station.
* Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A'
* button on the gamepad to write the calibration to a file. That file can then be indicated
* later when running an OpMode which uses the IMU.
*
* Note: if your intended uses of the IMU do not include use of all its sensors (for example,
* you might not use the magnetometer), then it makes little sense for you to wait for full
* calibration of the sensors you are not using before saving the calibration data. Indeed,
* it appears that in a SensorMode that doesn't use the magnetometer (for example), the
* magnetometer cannot actually be calibrated.
*
* References:
* The AdafruitBNO055IMU Javadoc
* The BNO055IMU.Parameters.calibrationDataFile Javadoc
* The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055
* The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
*/
@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
public class SensorBNO055IMUCalibration extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
// State
//----------------------------------------------------------------------------------------------
// Our sensors, motors, and other devices go here, along with other long term state
BNO055IMU imu;
// State used for updating telemetry
Orientation angles;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override public void runOpMode() {
telemetry.log().setCapacity(12);
telemetry.log().add("");
telemetry.log().add("Please refer to the calibration instructions");
telemetry.log().add("contained in the Adafruit IMU calibration");
telemetry.log().add("sample OpMode.");
telemetry.log().add("");
telemetry.log().add("When sufficient calibration has been reached,");
telemetry.log().add("press the 'A' button to write the current");
telemetry.log().add("calibration data to a file.");
telemetry.log().add("");
// We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu".
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.loggingEnabled = true;
parameters.loggingTag = "IMU";
imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
imu.initialize(parameters);
composeTelemetry();
telemetry.log().add("Waiting for start...");
// Wait until we're told to go
while (!isStarted()) {
telemetry.update();
idle();
}
telemetry.log().add("...started...");
while (opModeIsActive()) {
if (gamepad1.a) {
// Get the calibration data
BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData();
// Save the calibration data to a file. You can choose whatever file
// name you wish here, but you'll want to indicate the same file name
// when you initialize the IMU in an OpMode in which it is used. If you
// have more than one IMU on your robot, you'll of course want to use
// different configuration file names for each.
String filename = "AdafruitIMUCalibration.json";
File file = AppUtil.getInstance().getSettingsFile(filename);
ReadWriteFile.writeFile(file, calibrationData.serialize());
telemetry.log().add("saved to '%s'", filename);
// Wait for the button to be released
while (gamepad1.a) {
telemetry.update();
idle();
}
}
telemetry.update();
}
}
void composeTelemetry() {
// At the beginning of each telemetry update, grab a bunch of data
// from the IMU that we will then display in separate lines.
telemetry.addAction(new Runnable() { @Override public void run()
{
// Acquiring the angles is relatively expensive; we don't want
// to do that in each of the three items that need that info, as that's
// three times the necessary expense.
angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
}
});
telemetry.addLine()
.addData("status", new Func<String>() {
@Override public String value() {
return imu.getSystemStatus().toShortString();
}
})
.addData("calib", new Func<String>() {
@Override public String value() {
return imu.getCalibrationStatus().toString();
}
});
telemetry.addLine()
.addData("heading", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.firstAngle);
}
})
.addData("roll", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.secondAngle);
}
})
.addData("pitch", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.thirdAngle);
}
});
}
//----------------------------------------------------------------------------------------------
// Formatting
//----------------------------------------------------------------------------------------------
String formatAngle(AngleUnit angleUnit, double angle) {
return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
}
String formatDegrees(double degrees){
return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * 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;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;

237
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java Normal file
View File

@ -0,0 +1,237 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
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.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "org.firstinspires.ftc.teamcode.cometbots.projects.bBlueAutoV1", group = "V1")
public class bBlueAutoV1 extends OpMode {
public Telemetry telemetry;
public Follower robot;
public PathChain path;
@Override
public void init() {
robot = new Follower(hardwareMap);
PathBuilder builder = new PathBuilder();
path = builder
.addPath(
// Line 1
new BezierLine(
new Point(9.757, 84.983, Point.CARTESIAN),
new Point(28.573, 76.302, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierLine(
new Point(28.573, 76.302, Point.CARTESIAN),
new Point(36.203, 76.140, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierLine(
new Point(36.203, 76.140, Point.CARTESIAN),
new Point(35.067, 35.716, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(35.067, 35.716, Point.CARTESIAN),
new Point(73.705, 34.742, Point.CARTESIAN)
)
)
.addPath(
// Line 5
new BezierLine(
new Point(73.705, 34.742, Point.CARTESIAN),
new Point(73.705, 24.839, Point.CARTESIAN)
)
)
.addPath(
// Line 6
new BezierLine(
new Point(73.705, 24.839, Point.CARTESIAN),
new Point(7.630, 26.462, Point.CARTESIAN)
)
)
.addPath(
// Line 7
new BezierLine(
new Point(7.630, 26.462, Point.CARTESIAN),
new Point(64.126, 22.728, Point.CARTESIAN)
)
)
.addPath(
// Line 8
new BezierLine(
new Point(64.126, 22.728, Point.CARTESIAN),
new Point(63.964, 13.150, Point.CARTESIAN)
)
)
.addPath(
// Line 9
new BezierLine(
new Point(63.964, 13.150, Point.CARTESIAN),
new Point(12.338, 15.260, Point.CARTESIAN)
)
)
.addPath(
// Line 10
new BezierLine(
new Point(12.338, 15.260, Point.CARTESIAN),
new Point(63.802, 13.150, Point.CARTESIAN)
)
)
.addPath(
// Line 11
new BezierLine(
new Point(63.802, 13.150, Point.CARTESIAN),
new Point(63.639, 11.689, Point.CARTESIAN)
)
)
.addPath(
// Line 12
new BezierLine(
new Point(63.639, 11.689, Point.CARTESIAN),
new Point(12.014, 11.689, Point.CARTESIAN)
)
)
.addPath(
// Line 13
new BezierLine(
new Point(12.014, 11.689, Point.CARTESIAN),
new Point(62.665, 30.196, Point.CARTESIAN)
)
)
.addPath(
// Line 14
new BezierLine(
new Point(62.665, 30.196, Point.CARTESIAN),
new Point(13.312, 51.463, Point.CARTESIAN)
)
)
.addPath(
// Line 15
new BezierLine(
new Point(13.312, 51.463, Point.CARTESIAN),
new Point(16.234, 103.738, Point.CARTESIAN)
)
)
.addPath(
// Line 16
new BezierLine(
new Point(16.234, 103.738, Point.CARTESIAN),
new Point(68.023, 108.284, Point.CARTESIAN)
)
)
.addPath(
// Line 17
new BezierLine(
new Point(68.023, 108.284, Point.CARTESIAN),
new Point(68.185, 121.109, Point.CARTESIAN)
)
)
.addPath(
// Line 18
new BezierLine(
new Point(68.185, 121.109, Point.CARTESIAN),
new Point(21.754, 119.811, Point.CARTESIAN)
)
)
.addPath(
// Line 19
new BezierLine(
new Point(21.754, 119.811, Point.CARTESIAN),
new Point(11.526, 129.227, Point.CARTESIAN)
)
)
.addPath(
// Line 20
new BezierLine(
new Point(11.526, 129.227, Point.CARTESIAN),
new Point(72.568, 111.856, Point.CARTESIAN)
)
)
.addPath(
// Line 21
new BezierLine(
new Point(72.568, 111.856, Point.CARTESIAN),
new Point(58.607, 128.902, Point.CARTESIAN)
)
)
.addPath(
// Line 22
new BezierLine(
new Point(58.607, 128.902, Point.CARTESIAN),
new Point(11.364, 130.850, Point.CARTESIAN)
)
)
.addPath(
// Line 23
new BezierLine(
new Point(11.364, 130.850, Point.CARTESIAN),
new Point(58.931, 128.577, Point.CARTESIAN)
)
)
.addPath(
// Line 24
new BezierLine(
new Point(58.931, 128.577, Point.CARTESIAN),
new Point(58.769, 133.123, Point.CARTESIAN)
)
)
.addPath(
// Line 25
new BezierLine(
new Point(58.769, 133.123, Point.CARTESIAN),
new Point(13.475, 133.935, Point.CARTESIAN)
)
).build();
;
}
@Override
public void loop() {
robot.update();
if (robot.atParametricEnd())
robot.followPath(path);
robot.telemetryDebug(telemetry);
}
}

174
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java Normal file
View File

@ -0,0 +1,174 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
public class bRedAutoV1 {
public bRedAutoV1() {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(133.935, 83.770, Point.CARTESIAN),
new Point(79.874, 117.213, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 2
new BezierLine(
new Point(79.874, 117.213, Point.CARTESIAN),
new Point(79.874, 120.785, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 3
new BezierLine(
new Point(79.874, 120.785, Point.CARTESIAN),
new Point(131.824, 118.349, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 4
new BezierLine(
new Point(131.824, 118.349, Point.CARTESIAN),
new Point(79.549, 120.460, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 5
new BezierLine(
new Point(79.549, 120.460, Point.CARTESIAN),
new Point(79.549, 128.740, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 6
new BezierLine(
new Point(79.549, 128.740, Point.CARTESIAN),
new Point(131.337, 128.090, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 7
new BezierLine(
new Point(131.337, 128.090, Point.CARTESIAN),
new Point(79.549, 128.740, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(79.549, 128.740, Point.CARTESIAN),
new Point(79.549, 133.610, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 9
new BezierLine(
new Point(79.549, 133.610, Point.CARTESIAN),
new Point(130.850, 133.285, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 10
new BezierLine(
new Point(130.850, 133.285, Point.CARTESIAN),
new Point(130.201, 36.528, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 11
new BezierLine(
new Point(130.201, 36.528, Point.CARTESIAN),
new Point(84.095, 36.203, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 12
new BezierLine(
new Point(84.095, 36.203, Point.CARTESIAN),
new Point(84.095, 23.378, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 13
new BezierLine(
new Point(84.095, 23.378, Point.CARTESIAN),
new Point(119.811, 23.378, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 14
new BezierLine(
new Point(119.811, 23.378, Point.CARTESIAN),
new Point(127.603, 13.475, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 15
new BezierLine(
new Point(127.603, 13.475, Point.CARTESIAN),
new Point(88.640, 28.248, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 16
new BezierLine(
new Point(88.640, 28.248, Point.CARTESIAN),
new Point(87.666, 15.910, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 17
new BezierLine(
new Point(87.666, 15.910, Point.CARTESIAN),
new Point(127.603, 12.014, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 18
new BezierLine(
new Point(127.603, 12.014, Point.CARTESIAN),
new Point(86.692, 12.825, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 19
new BezierLine(
new Point(86.692, 12.825, Point.CARTESIAN),
new Point(86.692, 10.390, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 20
new BezierLine(
new Point(86.692, 10.390, Point.CARTESIAN),
new Point(126.467, 9.903, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
}
}

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

@ -1,95 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
@TeleOp(name = "Arm Test", group = "Debug")
public class ArmTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Arm
*/
ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
arm.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.circle && !previousGamepad1.circle) {
arm.parkArm();
}
if (currentGamepad1.square && !previousGamepad1.square) {
arm.engageArm();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
arm.switchState();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
arm.setPosition(arm.getPosition() + .05);
}
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Arm State", arm.getState());
telemetry.addData("Arm Position", arm.getPosition());
telemetry.update();
}
}
}

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

@ -1,78 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
@TeleOp(name = "Claw Test", group = "Debug")
public class ClawTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Claw
*/
ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.cross && !previousGamepad1.cross) {
claw.switchState();
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Claw State", claw.getState());
telemetry.update();
}
}
}

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

@ -1,124 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
@TeleOp(name = "Lift Raw Test", group = "Debug")
public class LiftRawTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
private final int MIN_POINT = 0;
private final int MAX_POINT = 3700;
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor");
liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
liftDrive.setPower(.5);
// Max position is 6800, safely setting to 6500
if (currentGamepad1.square && !previousGamepad1.square) {
liftDrive.setTargetPosition(MIN_POINT);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
liftDrive.setTargetPosition(1500);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
liftDrive.setTargetPosition(2750);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
liftDrive.setTargetPosition(MAX_POINT);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
int newPosition = liftDrive.getCurrentPosition() - 125;
if (newPosition < MIN_POINT) {
liftDrive.setTargetPosition(MIN_POINT);
} else {
liftDrive.setTargetPosition(newPosition);
}
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
int newPosition = liftDrive.getCurrentPosition() + 125;
if (newPosition > MAX_POINT) {
liftDrive.setTargetPosition(MAX_POINT);
} else {
liftDrive.setTargetPosition(newPosition);
}
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition());
telemetry.update();
}
}
}

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

@ -1,98 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
@TeleOp(name = "Lift Test", group = "Debug")
public class LiftTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
private final int MIN_POINT = 0;
// 2000 ~ 2500
// 3750 max
private final int MAX_POINT = 6500;
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
LiftSubsystem lift = new LiftSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
lift.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
lift.toFloor();
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
lift.toHighBucket();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
lift.toLowBucket();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
lift.switchState();
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());
telemetry.update();
}
}
}

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

@ -1,96 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Wrist Test", group = "Debug")
public class WristTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Wrist
*/
WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
wrist.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
wrist.bucketWrist();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
wrist.floorWrist();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
wrist.switchState();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
wrist.setPosition(wrist.getPosition() + .05);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Wrist State", wrist.getState());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.update();
}
}
}

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

@ -1,22 +0,0 @@
package org.firstinspires.ftc.teamcode.configs;
import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public static double clawClose = 1.00;
public static double clawOpen = 0.05;
public static double armEngage = 0.5;
public static double armPark = 0.125;
public static double armBucket = 0.175;
public static double wristFloor = 0.625;
public static double wristBucket = 0.215;
public static int liftToFloorPos = 20;
public static int liftToFloatPos = 150;
public static int liftToLowBucketPos = 2250;
public static int liftToHighBucketPos = 3900;
public static double liftPower = .45;
public static int liftToHoverState = 60;
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md
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 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 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 oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
* sensitive than the others. For reference,
* 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 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 thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`

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

@ -5,7 +5,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; 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.ThreeWheelIMULocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer;
@ -71,7 +70,6 @@ public class PoseUpdater {
public PoseUpdater(HardwareMap hardwareMap) { public PoseUpdater(HardwareMap hardwareMap) {
// TODO: replace the second argument with your preferred localizer // TODO: replace the second argument with your preferred localizer
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
} }
/** /**

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

@ -38,9 +38,9 @@ public class DriveEncoderLocalizer extends Localizer {
private Encoder leftRear; private Encoder leftRear;
private Encoder rightRear; private Encoder rightRear;
private double totalHeading; private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = -0.6308; public static double FORWARD_TICKS_TO_INCHES = 1;
public static double STRAFE_TICKS_TO_INCHES = 46.4839; public static double STRAFE_TICKS_TO_INCHES = 1;
public static double TURN_TICKS_TO_RADIANS = -0.002; public static double TURN_TICKS_TO_RADIANS = 1;
public static double ROBOT_WIDTH = 1; public static double ROBOT_WIDTH = 1;
public static double ROBOT_LENGTH = 1; public static double ROBOT_LENGTH = 1;

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java
View File

@ -65,9 +65,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
private double previousIMUOrientation; private double previousIMUOrientation;
private double deltaRadians; private double deltaRadians;
private double totalHeading; private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.004;//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.0036;//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.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5; public static double TURN_TICKS_TO_RADIANS = 0.0022;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static boolean useIMU = true; public static boolean useIMU = true;
@ -96,9 +96,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION))); imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
// TODO: replace these with your encoder positions // TODO: replace these with your encoder positions
leftEncoderPose = new Pose(-7.625, 6.19375, 0); leftEncoderPose = new Pose(0, 6.19375, 0);
rightEncoderPose = new Pose(-7.625, -6.19375, 0); rightEncoderPose = new Pose(0, -6.19375, 0);
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90)); strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
// TODO: replace these with your encoder ports // TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));

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

@ -57,11 +57,8 @@ public class ThreeWheelLocalizer extends Localizer {
private Pose rightEncoderPose; private Pose rightEncoderPose;
private Pose strafeEncoderPose; private Pose strafeEncoderPose;
private double totalHeading; 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 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 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 TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
/** /**
@ -95,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary // TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION); //leftEncoder.setDirection(Encoder.REVERSE);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); // rightEncoder.setDirection(Encoder.REVERSE);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); //strafeEncoder.setDirection(Encoder.FORWARD);
setStartPose(setStartPose); setStartPose(setStartPose);
timer = new NanoTimer(); timer = new NanoTimer();

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

@ -63,7 +63,6 @@ public class LateralTuner extends OpMode {
telemetryA.addData("distance moved", poseUpdater.getPose().getY()); telemetryA.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.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.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
telemetryA.update(); telemetryA.update();
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");

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

@ -53,14 +53,14 @@ public class FollowerConstants {
0); 0);
// Feed forward constant added on to the translational PIDF // 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 // Heading error PIDF coefficients
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
2, 2,
0, 0,
.025, 0.025,
0); 0);
// Feed forward constant added on to the heading PIDF // Feed forward constant added on to the heading PIDF

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java
View File

@ -93,11 +93,13 @@ public class ForwardVelocityTuner extends OpMode {
} }
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); 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("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("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("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.addLine("Press CROSS or A on game pad 1 to stop.");
telemetryA.update(); //
//
// telemetryA.update();
} }
@ -138,6 +140,13 @@ public class ForwardVelocityTuner extends OpMode {
velocities.add(currentVelocity); velocities.add(currentVelocity);
velocities.remove(0); 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 { } else {
double average = 0; double average = 0;
for (Double velocity : velocities) { for (Double velocity : velocities) {
@ -146,7 +155,12 @@ public class ForwardVelocityTuner extends OpMode {
average /= (double) velocities.size(); average /= (double) velocities.size();
telemetryA.addData("forward velocity:", average); 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();
} }
} }
} }

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

@ -119,13 +119,6 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
*/ */
@Override @Override
public void loop() { 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) { if (gamepad1.cross || gamepad1.a) {
requestOpModeStop(); requestOpModeStop();
} }

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

@ -1,114 +0,0 @@
package org.firstinspires.ftc.teamcode.runmodes;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.util.action.Action;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
import org.firstinspires.ftc.teamcode.util.action.SleepAction;
public class Auto {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public Timer clawTimer = new Timer();
public Timer armTimer = new Timer();
public Timer wristTimer = new Timer();
public Follower follower;
public Telemetry telemetry;
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
this.follower = follower;
this.telemetry = telemetry;
init();
}
public void init() {
claw.init();
arm.init();
wrist.init();
}
public void start() {
clawTimer.resetTimer();
armTimer.resetTimer();
wristTimer.resetTimer();
claw.start();
arm.start();
wrist.start();
}
public void update() {
this.telemetry.addData("Current State", caseState);
this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
this.telemetry.update();
switch(caseState) {
case 1:
claw.openClaw();
caseState = 2;
break;
case 2:
if (clawTimer.getElapsedTimeSeconds() > 2) {
arm.engageArm();
caseState = 3;
}
break;
case 3:
if (armTimer.getElapsedTimeSeconds() > 4) {
wrist.floorWrist();
caseState = 4;
}
break;
case 4:
if (clawTimer.getElapsedTimeSeconds() > 6) {
claw.closeClaw();
caseState = 5;
}
break;
case 5:
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
caseState = 6;
}
break;
case 6:
if (clawTimer.getElapsedTimeSeconds() > 10) {
claw.openClaw();
caseState = 7;
}
break;
case 7:
this.init();
break;
}
}
}

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

@ -1,89 +0,0 @@
package org.firstinspires.ftc.teamcode.runmodes;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class Teleop {
private ClawSubsystem claw;
private Follower follower;
private DcMotorEx leftFront;
private DcMotorEx leftRear;
private DcMotorEx rightFront;
private DcMotorEx rightRear;
private Telemetry telemetry;
private Gamepad gamepad1;
private Gamepad currentGamepad1;
private Gamepad previousGamepad1;
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
initMotors(hardwareMap);
this.follower = follower;
this.telemetry = telemetry;
this.gamepad1 = gamepad1;
this.currentGamepad1 = new Gamepad();
this.previousGamepad1 = new Gamepad();
}
public void start() {
claw.start();
follower.startTeleopDrive();
}
public void update() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.a && !previousGamepad1.a)
claw.switchState();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
telemetry.addData("X", follower.getPose().getX());
telemetry.addData("Y", follower.getPose().getY());
telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading()));
telemetry.addData("Claw State", claw.getState());
telemetry.update();
}
private void initMotors(HardwareMap hardwareMap) {
rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java Normal file
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;
}
}

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

@ -1,89 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class ArmSubsystem {
public enum ArmState {
PARK, ENGAGE, BUCKET
}
public ServoImplEx arm;
public ArmState state;
public RunAction engageArm, parkArm, bucketArm;
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
arm.resetDeviceConfigurationForOpMode();
this.state = armState;
parkArm = new RunAction(this::parkArm);
engageArm = new RunAction(this::engageArm);
bucketArm = new RunAction(this::bucketArm);
}
public void setState(ArmState armState) {
if (armState == ArmState.ENGAGE) {
arm.setPosition(armEngage);
this.state = ArmState.ENGAGE;
} else if (armState == ArmState.PARK) {
arm.setPosition(armPark);
this.state = ArmState.PARK;
} else if (armState == ArmState.BUCKET) {
arm.setPosition(armBucket);
this.state = ArmState.BUCKET;
}
}
public void engageArm() {
setState(ArmState.ENGAGE);
}
public void parkArm() {
setState(ArmState.PARK);
}
public void bucketArm() {
setState(ArmState.BUCKET);
}
public void switchState() {
if (state == ArmState.ENGAGE) {
setState(ArmState.PARK);
} else if (state == ArmState.PARK) {
setState(ArmState.BUCKET);
} else if (state == ArmState.BUCKET) {
setState(ArmState.ENGAGE);
}
}
public ArmState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(parkArm);
}
public void start() {
Actions.runBlocking(parkArm);
}
public double getPosition() {
return this.arm.getPosition();
}
public void setPosition(double position) {
this.arm.setPosition(position);
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java Normal file
View File

@ -0,0 +1,44 @@
package org.firstinspires.ftc.teamcode.subsystem;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine1 {
private PathChain pathChain;
private Pose autoLin1StartPose = new Pose(8,65);
public void moveToAutoLine1(Follower robot) {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(30.000, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0));
pathChain = builder.build();
robot.setStartingPose(autoLin1StartPose);
robot.followPath(pathChain);
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java Normal file
View File

@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.subsystem;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine2 {
private PathChain pathChain;
public void moveToAutoLine2(Follower robot) {
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 2
new BezierCurve(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(24.000, 24.000, Point.CARTESIAN),
new Point(72.000, 36.000, Point.CARTESIAN),
new Point(56.000, 24.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

73
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java Normal file
View File

@ -0,0 +1,73 @@
package org.firstinspires.ftc.teamcode.subsystem;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class AutoLine3 implements Action {
private Follower actionRobot;
private PathChain pathChain;
private Pose startPose = new Pose(56,24);
public AutoLine3(Follower robot) {
this.actionRobot = robot;
this.actionRobot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
/* .addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(30.000, 72.000, Point.CARTESIAN)
// This is the origional new Point(36.000, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(24.000, 24.000, Point.CARTESIAN),
new Point(72.000, 36.000, Point.CARTESIAN),
new Point(56.000, 24.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) */
.addPath(
// Line 3
new BezierLine(
new Point(56.000, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation();
pathChain = builder.build();
this.actionRobot.followPath(this.pathChain);
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
this.actionRobot.update();
return this.actionRobot.isBusy();
}
}

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

@ -1,67 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
public class ClawSubsystem {
public enum ClawState {
CLOSED, OPEN
}
private Servo claw;
private ClawState state;
public RunAction openClaw, closeClaw;
public ClawSubsystem(HardwareMap hardwareMap, ClawState clawState) {
claw = hardwareMap.get(Servo.class, "claw-servo");
this.state = clawState;
openClaw = new RunAction(this::openClaw);
closeClaw = new RunAction(this::closeClaw);
}
public void setState(ClawState clawState) {
if (clawState == ClawState.CLOSED) {
claw.setPosition(clawClose);
this.state = ClawState.CLOSED;
} else if (clawState == ClawState.OPEN) {
claw.setPosition(clawOpen);
this.state = ClawState.OPEN;
}
}
public ClawState getState() {
return this.state;
}
public void switchState() {
if (state == ClawState.CLOSED) {
setState(ClawState.OPEN);
} else if (state == ClawState.OPEN) {
setState(ClawState.CLOSED);
}
}
public void openClaw() {
setState(ClawState.OPEN);
}
public void closeClaw() {
setState(ClawState.CLOSED);
}
public void init() {
Actions.runBlocking(closeClaw);
}
public void start() {
Actions.runBlocking(closeClaw);
}
}

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

@ -1,94 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHoverState;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class LiftSubsystem {
public DcMotor lift;
public RunAction toFloor, toLowBucket, toHighBucket;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER
}
private LiftState liftState;
public LiftSubsystem(HardwareMap hardwareMap) {
lift = hardwareMap.get(DcMotor.class, "lift-motor");
toFloor = new RunAction(this::toFloor);
toLowBucket = new RunAction(this::toLowBucket);
toHighBucket = new RunAction(this::toHighBucket);
}
public void setTarget(int b) {
lift.setTargetPosition(b);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void switchState() {
if (this.liftState == LiftState.FLOOR) {
this.toFloor();
} else if (this.liftState == LiftState.FLOAT) {
this.toLowBucket();
} else if (this.liftState == LiftState.LOW_BUCKET) {
this.toHighBucket();
} else if (this.liftState == LiftState.HIGH_BUCKET) {
this.toFloor();
}
}
public void toHover() {
this.setTarget(liftToHoverState);
this.setState(LiftState.HOVER);
}
public void toFloor() {
this.setTarget(liftToFloorPos);
this.setState(LiftState.FLOOR);
}
public void toFloat() {
this.setTarget(liftToFloatPos);
this.setState(LiftState.FLOAT);
}
public void toLowBucket() {
this.setTarget(liftToLowBucketPos);
this.setState(LiftState.LOW_BUCKET);
}
public void toHighBucket() {
this.setTarget(liftToHighBucketPos);
this.setState(LiftState.HIGH_BUCKET);
}
public void init() {
lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
lift.setDirection(DcMotorSimple.Direction.REVERSE);
lift.setPower(liftPower);
}
private void setState(LiftState liftState) {
this.liftState = liftState;
}
public int getPosition() {
return lift.getCurrentPosition();
}
public void start() {
this.toFloor();
}
}

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

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

@ -1,79 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class WristSubsystem {
public enum WristState {
FLOOR, BUCKET
}
public ServoImplEx wrist;
public WristState state;
public RunAction floorWrist, bucketWrist;
public WristSubsystem(HardwareMap hardwareMap, WristState wristState) {
wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
wrist.resetDeviceConfigurationForOpMode();
this.state = wristState;
bucketWrist = new RunAction(this::bucketWrist);
floorWrist = new RunAction(this::floorWrist);
}
public void setState(WristState wristState) {
if (wristState == WristState.FLOOR) {
wrist.setPosition(wristFloor);
this.state = WristState.FLOOR;
} else if (wristState == WristState.BUCKET) {
wrist.setPosition(wristBucket);
this.state = WristState.BUCKET;
}
}
public void floorWrist() {
setState(WristState.FLOOR);
}
public void bucketWrist() {
setState(WristState.BUCKET);
}
public void switchState() {
if (state == WristState.FLOOR) {
setState(WristState.BUCKET);
} else if (state == WristState.BUCKET) {
setState(WristState.FLOOR);
}
}
public WristState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(floorWrist);
}
public void start() {
Actions.runBlocking(floorWrist);
}
public double getPosition() {
return this.wrist.getPosition();
}
public void setPosition(double position) {
this.wrist.setPosition(position);
}
}

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

@ -1,11 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public interface Action {
boolean run(TelemetryPacket p);
default void preview(Canvas fieldOverlay) {
}
}

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

@ -1,24 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public class Actions {
public static void runBlocking(Action a) {
FtcDashboard dash = FtcDashboard.getInstance();
Canvas c = new Canvas();
a.preview(c);
boolean b = true;
while (b && !Thread.currentThread().isInterrupted()) {
TelemetryPacket p = new TelemetryPacket();
p.fieldOverlay().getOperations().addAll(c.getOperations());
b = a.run(p);
dash.sendTelemetryPacket(p);
}
}
}

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

@ -1,39 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.config.Config;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
@Config
public class FieldConstants {
public enum RobotStart {
BLUE_BUCKET,
BLUE_OBSERVATION,
RED_BUCKET,
RED_OBSERVATION
}
public static final Pose blueBucketStartPose = new Pose(8, 79.5, Math.toRadians(180));
public static final Pose blueObservationStartPose = new Pose(8, 36, Math.toRadians(180));
public static final Pose redBucketStartPose = new Pose(144-8, 79.5, 0);
public static final Pose redObservationStartPose = new Pose(144-8, 36, 0);
// Blue Preload Poses
public static final Pose blueBucketPreloadPose = new Pose(34.5, 79.5, Math.toRadians(180));
// Blue Bucket Sample Poses
public static final Pose blueBucketLeftSamplePose = new Pose(34.75, 113.5, Math.toRadians(66));
public static final Pose blueBucketLeftSampleControlPose = new Pose(32, 108);
public static final Pose blueBucketMidSamplePose = new Pose(33, 125.5, Math.toRadians(73));
public static final Pose blueBucketMidSampleControlPose = new Pose(47.5, 110);
public static final Pose blueBucketRightSamplePose = new Pose(33, 133, Math.toRadians(74));
public static final Pose blueBucketRightSampleControlPose = new Pose(46, 101);
public static final Pose blueBucketScorePose = new Pose(16, 128, Math.toRadians(-45));
public static final Pose blueBucketParkPose = new Pose(65, 97.75, Math.toRadians(90));
public static final Pose blueBucketParkControlPose = new Pose(60.25, 123.5);
}

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

@ -1,30 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public class RunAction implements Action {
private final Runnable runnable;
private Runnable callback;
public RunAction(Runnable runnable) {
this.runnable = runnable;
}
public void runAction() {
runnable.run();
if (callback != null) {
callback.run();
}
}
public void setCallback(Runnable callback) {
this.callback = callback;
}
// Adapter to make Action compatible with the Action interface
public boolean run(TelemetryPacket p) {
runAction();
return false; // Regular actions complete after one execution
}
}

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

@ -1,43 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public class SequentialAction implements Action {
private List<Action> actions;
public SequentialAction(List<Action> actions) {
this.actions = new ArrayList<>(actions);
}
public SequentialAction(Action... actions) {
this(Arrays.asList(actions));
}
@Override
public boolean run(TelemetryPacket p) {
if (actions.isEmpty()) {
return false;
}
if (actions.get(0).run(p)) {
return true;
} else {
actions.remove(0);
return run(p);
}
}
@Override
public void preview(Canvas fieldOverlay) {
for (Action a : actions) {
a.preview(fieldOverlay);
}
}
}

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

@ -1,32 +0,0 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import java.util.Timer;
import java.util.TimerTask;
public class SleepAction implements Action {
private final double dt;
private boolean isFinished = false;
public SleepAction(double dt) {
this.dt = dt;
}
@Override
public boolean run(TelemetryPacket p) {
if (!isFinished) {
Timer timer = new Timer();
timer.schedule(new TimerTask() {
@Override
public void run() {
isFinished = true;
}
}, (long) (dt * 1000));
} else {
isFinished = false; // Reset the flag after sleep is complete
return false; // Indicate that the action is finished
}
return true; // Indicate that the action is still running
}
}

5
build.dependencies.gradle
View File

@ -16,6 +16,9 @@ dependencies {
implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0'
implementation 'androidx.appcompat:appcompat:1.2.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"
} }