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

Compare commits

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

1 Commits
branch-sil ... branch-ped

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

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java
View File

@ -1,24 +0,0 @@
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();
}
}

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

@ -0,0 +1,92 @@
/*
Copyright (c) 2024 Alan Smith
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted (subject to the limitations in the disclaimer below) provided that
the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.
Neither the name of Alan Smith nor the names of its contributors may be used to
endorse or promote products derived from this software without specific prior
written permission.
NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
/*
* This OpMode illustrates how to use the goBILDA Indicator Light
*
* To make this work, this Light uses a Servo motor and setPositions.
* Valid values are from .277 to .722.
*
* If it's less than .277 then no light
* If it's greater than .722 then white light
*
* Spec Sheet is here: https://cdn11.bigcommerce.com/s-x56mtydx1w/images/stencil/original/products/2275/13464/3118-0808-0002-Product-Insight-4__03940__01348.1728056113.png?c=1
*/
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name = "Demo: goBILDA Indicator Light", group = "Concept")
public class DemoGoBildaIndicatorLight extends LinearOpMode {
Servo front_led;
private final double buffer = .277;
@Override
public void runOpMode() {
front_led = hardwareMap.get(Servo.class, "test_servo");
waitForStart();
while (opModeIsActive()) {
double left_x = Math.abs(gamepad1.left_stick_x) / 4;
double left_y = Math.abs(gamepad1.left_stick_y) / 4;
double right_x = Math.abs(gamepad1.right_stick_x) / 4;
double right_y = Math.abs(gamepad1.right_stick_y) / 4;
double total_position = Math.min(left_x + left_y + right_x + right_y + buffer, 1.0);
telemetry.addData("left_x", left_x);
telemetry.addData("left_y", left_y);
telemetry.addData("right_x", right_x);
telemetry.addData("right_y", right_y);
telemetry.addData("total (plus buffer)", total_position);
telemetry.update();
front_led.setPosition(total_position);
if (gamepad1.cross) {
front_led.setPosition(.611);
sleep(2000);
}
if (gamepad1.triangle) {
front_led.setPosition(.444);
sleep(2000);
}
if (gamepad1.square) {
front_led.setPosition(.7);
sleep(2000);
}
if (gamepad1.circle) {
front_led.setPosition(.3);
sleep(2000);
}
}
}
}

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

@ -1,64 +0,0 @@
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);
}
}

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

@ -12,10 +12,10 @@ public class PedroConstants {
*/
// Robot motor configurations
public static final String FRONT_LEFT_MOTOR = "front-left";
public static final String BACK_LEFT_MOTOR = "back-left";
public static final String FRONT_RIGHT_MOTOR = "front-right";
public static final String BACK_RIGHT_MOTOR = "back-right";
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
public static final String BACK_LEFT_MOTOR = "Drive back lt";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
// Robot motor direction
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
@ -23,60 +23,48 @@ public class PedroConstants {
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
/*
Motor Max Power
*/
public static final double MAX_POWER = .75;
// Robot IMU configuration
public static final String IMU = "imu";
// Robot IMU placement
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
= RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
= RevHubOrientationOnRobot.UsbFacingDirection.UP;
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
// Robot encoders
// NOTE: Encoders are plugged into the same ports as motors hence the weird names
public static final String RIGHT_ENCODER = "back-right"; //0
public static final String BACK_ENCODER = "front-right"; //1
public static final String LEFT_ENCODER = "front-left"; //2
public static final String LEFT_ENCODER = "encoder left";
public static final String RIGHT_ENCODER = "encoder right";
public static final String BACK_ENCODER = "encoder back";
// Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
// 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";
public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
/*
Pedro's parameters
*/
// The weight of the robot in Kilograms
public static final double ROBOT_WEIGHT_IN_KG = 9;
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
// Maximum velocity of the robot going forward
public static final double ROBOT_SPEED_FORWARD = 51.5;
public static final double ROBOT_SPEED_FORWARD = 72.0693;
// Maximum velocity of the robot going right
public static final double ROBOT_SPEED_LATERAL = 28.7;
public static final double ROBOT_SPEED_LATERAL = 24.1401;
// Rate of deceleration when power is cut-off when the robot is moving forward
public static final double FORWARD_ZERO_POWER_ACCEL = -59.8;
public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
// Rate of deceleration when power is cut-off when the robot is moving to the right
public static final double LATERAL_ZERO_POWER_ACCEL = -99.7;
public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
public static final double ZERO_POWER_ACCEL_MULT = 4;
/* Centripetal force correction - increase if robot is correcting into the path
- decrease if robot is correcting away from the path */
public static final double CENTRIPETAL_SCALING = 0.0004;
public static final double CENTRIPETAL_SCALING = 0.0005;
}

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

@ -1,122 +0,0 @@
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
View File

@ -1,151 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "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);
}
}

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

@ -1,142 +0,0 @@
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();
}
}
}

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

@ -1,135 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning")
public class AsherPathV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10.0, 40, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath(
// Line 1
new BezierCurve(
new Point(9.757, 84.983, Point.CARTESIAN),
new Point(33.000, 105.000, Point.CARTESIAN),
new Point(80.000, 118.000, Point.CARTESIAN),
new Point(55.000, 120.000, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierCurve(
new Point(55.000, 120.000, Point.CARTESIAN),
new Point(22.000, 106.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(75.000, 95.000, Point.CARTESIAN),
new Point(112.000, 132.000, Point.CARTESIAN),
new Point(61.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(61.000, 131.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 5
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(100.000, 118.000, Point.CARTESIAN),
new Point(103.000, 135.000, Point.CARTESIAN),
new Point(61.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 6
new BezierLine(
new Point(61.000, 135.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 7
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(113.000, 95.000, Point.CARTESIAN),
new Point(67.000, 95.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
/*
* End of only update this path
*/
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,79 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning")
public class AutoExample extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(0.0, 20.0, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.6);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(0.000, 20.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,106 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning")
public class AutoExampleFour extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(12,60, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(12.000, 60.000, Point.CARTESIAN),
new Point(60.000, 60.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.addPath(
// Line 2
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(40.000, 12.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.addPath(
// Line 3
new BezierCurve(
new Point(40.000, 12.000, Point.CARTESIAN),
new Point(35.000, 35.000, Point.CARTESIAN),
new Point(12.000, 35.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.addPath(
// Line 4
new BezierLine(
new Point(12.000, 35.000, Point.CARTESIAN),
new Point(12.000, 60.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,142 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
public class AutoExampleSeason2025V1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(15.0, 35, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.375);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(15.000, 35.000, Point.CARTESIAN),
new Point(60.000, 35.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 2
new BezierLine(
new Point(60.000, 35.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 3
new BezierLine(
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(15.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 4
new BezierLine(
new Point(15.000, 25.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 6
new BezierLine(
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(15.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 7
new BezierLine(
new Point(15.000, 15.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 8
new BezierLine(
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(60.000, 8.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 9
new BezierLine(
new Point(60.000, 8.000, Point.CARTESIAN),
new Point(15.000, 8.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,89 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning")
public class AutoExampleThree extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10,45, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(10.000, 45.000, Point.CARTESIAN),
new Point(45.000, 45.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.addPath(
// Line 2
new BezierLine(
new Point(50.000, 20.000, Point.CARTESIAN),
new Point(10.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,80 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning")
public class AutoExampleTwo extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10.0, 45, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(10.000, 45.000, Point.CARTESIAN),
new Point(45.000, 45.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

91
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java
View File

@ -1,91 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
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.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 = "Test", group = "Autonomous Pathing Tuning")
public class AutoTest extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain test;
@Override
public void init() {
follower = new Follower(hardwareMap);
test = follower.pathBuilder()
.addPath(
new BezierLine(
new Point(8.000, 60.000, Point.CARTESIAN),
new Point(18.000, 60.000, Point.CARTESIAN)
)
)
.addPath(
// 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 Point(48.000, 23.000, Point.CARTESIAN),
new Point(60.000, 36.000, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(60.000, 36.000, Point.CARTESIAN),
new Point(60.000, 49.000, Point.CARTESIAN)
)
).build();
follower.followPath(test);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(test);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,197 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.projects;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This file contains an example of a Linear "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
*
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
*
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
*
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")
public class BasicOmniOpMode_Linear extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
// Initialize the hardware variables. Note that the strings used here must correspond
// to the names assigned during the robot configuration step on the DS or RC devices.
DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
// TODO: replace these with your encoder ports
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
// ########################################################################################
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
// ########################################################################################
// Most robots need the motors on one side to be reversed to drive forward.
// The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft)
// If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure
// that your motors are turning in the correct direction. So, start out with the reversals here, BUT
// when you first test your robot, push the left joystick forward and observe the direction the wheels turn.
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.
leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
// Wait for the game to start (driver presses START)
telemetry.addData("Status", "Initialized");
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// This is test code:
//
// Uncomment the following code to test your motor directions.
// Each button should make the corresponding motor run FORWARD.
// 1) First get all the motors to take to correct positions on the robot
// by adjusting your Robot Configuration if necessary.
// 2) Then make sure they run in the correct direction by modifying the
// the setDirection() calls above.
// Once the correct motors move in the correct direction re-comment this code.
/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/
// Send calculated power to wheels
leftFrontDrive.setPower(leftFrontPower);
rightFrontDrive.setPower(rightFrontPower);
leftBackDrive.setPower(leftBackPower);
rightBackDrive.setPower(rightBackPower);
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
}
}}

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

@ -1,128 +0,0 @@
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();
}
}

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

@ -1,161 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75, 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 BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(37.000, 108.000, Point.CARTESIAN),
new Point(73.286, 111.536, Point.CARTESIAN),
new Point(67.821, 120.536, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(67.821, 120.536, Point.CARTESIAN),
new Point(28.000, 121.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(28.000, 121.500, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierCurve(
new Point(18.000, 130.179, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN),
new Point(68.700, 130.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(68.700, 130.500, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(63.804, 135.321, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(53.036, 135.161, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(72.300, 97.400, Point.CARTESIAN)
)
)
.addPath(
// Line 9
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(83.250, 95.464, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
follower.followPath(path, 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);
}
}

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

@ -1,246 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots.projects;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning")
public class BluebAutoV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(7.5, 72, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(7.5, 72, Point.CARTESIAN),
new Point(29.893, 38.250, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 2
new BezierLine(
new Point(29.893, 38.250, Point.CARTESIAN),
new Point(65.250, 32.143, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 3
new BezierLine(
new Point(65.250, 32.143, Point.CARTESIAN),
new Point(61.714, 24.429, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 4
new BezierLine(
new Point(61.714, 24.429, Point.CARTESIAN),
new Point(13.821, 22.821, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 5
new BezierLine(
new Point(13.821, 22.821, Point.CARTESIAN),
new Point(61.714, 24.429, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 6
new BezierLine(
new Point(61.714, 24.429, Point.CARTESIAN),
new Point(60.750, 12.696, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 7
new BezierLine(
new Point(60.750, 12.696, Point.CARTESIAN),
new Point(12.375, 13.179, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 8
new BezierLine(
new Point(12.375, 13.179, Point.CARTESIAN),
new Point(60.750, 12.536, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 9
new BezierLine(
new Point(60.750, 12.536, Point.CARTESIAN),
new Point(60.589, 9.321, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 10
new BezierLine(
new Point(60.589, 9.321, Point.CARTESIAN),
new Point(12.536, 8.357, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 11
new BezierLine(
new Point(12.536, 8.357, Point.CARTESIAN),
new Point(26.679, 8.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 12
new BezierLine(
new Point(26.679, 8.679, Point.CARTESIAN),
new Point(22.821, 109.446, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 13
new BezierLine(
new Point(22.821, 109.446, Point.CARTESIAN),
new Point(70.714, 109.446, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 14
new BezierLine(
new Point(70.714, 109.446, Point.CARTESIAN),
new Point(71.036, 120.214, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 15
new BezierLine(
new Point(71.036, 120.214, Point.CARTESIAN),
new Point(22.179, 120.214, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 16
new BezierLine(
new Point(22.179, 120.214, Point.CARTESIAN),
new Point(11.089, 130.821, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 17
new BezierLine(
new Point(11.089, 130.821, Point.CARTESIAN),
new Point(70.714, 112.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 18
new BezierLine(
new Point(70.714, 112.018, Point.CARTESIAN),
new Point(70.714, 128.250, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 19
new BezierLine(
new Point(70.714, 128.250, Point.CARTESIAN),
new Point(9.964, 130.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 20
new BezierLine(
new Point(9.964, 130.018, Point.CARTESIAN),
new Point(70.554, 130.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 21
new BezierLine(
new Point(70.554, 130.500, Point.CARTESIAN),
new Point(70.393, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 22
new BezierLine(
new Point(70.393, 135.000, Point.CARTESIAN),
new Point(13.821, 134.839, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

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

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

@ -1,109 +0,0 @@
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
View File

@ -1,99 +0,0 @@
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
View File

@ -1,185 +0,0 @@
/* 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
View File

@ -1,229 +0,0 @@
/* 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));
}
}

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

@ -1,172 +0,0 @@
/* Copyright (c) 2022 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.projects;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.PedroConstants;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This OpMode shows how to use the new universal IMU interface. This
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
* on the robot with the name "imu".
*
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
*
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
*
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
*
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
* the alternative SensorIMUNonOrthogonal sample in this folder.
*
* This "Orthogonal" requirement means that:
*
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* 2) The USB ports can only be pointing in one of the same six directions:<br>
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
* logoFacingDirection<br>
* usbFacingDirection
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
* to use those parameters.
*/
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
@Disabled // Comment this out to add to the OpMode list
public class SensorIMUOrthogonal extends LinearOpMode {
// The IMU sensor object
IMU imu;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override
public void runOpMode() throws InterruptedException {
// Retrieve and initialize the IMU.
// This sample expects the IMU to be in a REV Hub and named "imu".
imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
*
* Two input parameters are required to fully specify the Orientation.
* The first parameter specifies the direction the printed logo on the Hub is pointing.
* The second parameter specifies the direction the USB connector on the Hub is pointing.
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
*
* If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
* RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
*/
/* The next two lines define Hub orientation.
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
*
* To Do: EDIT these two lines to match YOUR mounting configuration.
*/
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
// Now initialize the IMU with this mounting orientation
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
imu.initialize(new IMU.Parameters(orientationOnRobot));
// Loop and update the dashboard
while (!isStopRequested()) {
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
// Check to see if heading reset is requested
if (gamepad1.y) {
telemetry.addData("Yaw", "Resetting\n");
imu.resetYaw();
} else {
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
}
// Retrieve Rotational Angles and Velocities
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
telemetry.update();
}
}
}

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

@ -1,237 +0,0 @@
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
View File

@ -1,174 +0,0 @@
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();
}
}

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

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

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

@ -69,7 +69,7 @@ public class PoseUpdater {
*/
public PoseUpdater(HardwareMap hardwareMap) {
// TODO: replace the second argument with your preferred localizer
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
}
/**

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

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

@ -57,9 +57,9 @@ public class ThreeWheelLocalizer extends Localizer {
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
/**
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
@ -80,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer {
*/
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(0, 6.19375, 0);
rightEncoderPose = new Pose(0, -6.19375, 0);
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0);
strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
hardwareMap = map;

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

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

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

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

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java
View File

@ -1,18 +0,0 @@
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;
}
}

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

@ -1,44 +0,0 @@
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
View File

@ -1,40 +0,0 @@
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
View File

@ -1,73 +0,0 @@
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();
}
}

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

@ -1,105 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class MotorsSubsystem {
public HardwareMap hardwareMap;
public Telemetry telemetry;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
public enum TravelState {
STOPPED, MOVING
}
public TravelState travelState;
public double power;
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = MAX_POWER;
}
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = power;
}
public void init() {
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.setState(TravelState.STOPPED);
}
public void setFrontLeftMotorPower(double power) {
frontLeftMotor.setPower(power);
}
public void setBackLeftMotorPower(double power) {
backLeftMotor.setPower(power);
}
public void setFrontRightMotorPower(double power) {
frontRightMotor.setPower(power);
}
public void setBackRightMotorPower(double power) {
backRightMotor.setPower(power);
}
public void setState(TravelState travelState) {
this.travelState = travelState;
}
public TravelState getState() {
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);
}
}
}

21
build.dependencies.gradle
View File

@ -6,19 +6,16 @@ repositories {
}
dependencies {
implementation 'org.firstinspires.ftc:Inspection:10.1.0'
implementation 'org.firstinspires.ftc:Blocks:10.1.0'
implementation 'org.firstinspires.ftc:RobotCore:10.1.0'
implementation 'org.firstinspires.ftc:RobotServer:10.1.0'
implementation 'org.firstinspires.ftc:OnBotJava:10.1.0'
implementation 'org.firstinspires.ftc:Hardware:10.1.0'
implementation 'org.firstinspires.ftc:FtcCommon:10.1.0'
implementation 'org.firstinspires.ftc:Vision:10.1.0'
implementation 'org.firstinspires.ftc:Inspection:10.0.0'
implementation 'org.firstinspires.ftc:Blocks:10.0.0'
implementation 'org.firstinspires.ftc:RobotCore:10.0.0'
implementation 'org.firstinspires.ftc:RobotServer:10.0.0'
implementation 'org.firstinspires.ftc:OnBotJava:10.0.0'
implementation 'org.firstinspires.ftc:Hardware:10.0.0'
implementation 'org.firstinspires.ftc:FtcCommon:10.0.0'
implementation 'org.firstinspires.ftc:Vision:10.0.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation "com.acmerobotics.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"
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
}