diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 1d14dfb76035..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -36,26 +36,26 @@ import com.qualcomm.robotcore.util.ElapsedTime; /* - * 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. + * This file contains an example of a Linear线性的 "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop(teleoperation遥控的) 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 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. + * Note that a Mecanum drive must display an X roller-pattern when viewed from above.注意，麦克纳姆轮正确安装时，从上往下看起来应该像X * - * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * 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. + * 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 + * 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. + * 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). * @@ -67,7 +67,7 @@ @Disabled public class BasicOmniOpMode_Linear extends LinearOpMode { - // Declare OpMode members for each of the 4 motors. + // Declare声明 OpMode members for each of the 4 motors. private ElapsedTime runtime = new ElapsedTime(); private DcMotor leftFrontDrive = null; private DcMotor leftBackDrive = null; diff --git a/MeepMeep/.gitignore b/MeepMeep/.gitignore new file mode 100644 index 000000000000..42afabfd2abe --- /dev/null +++ b/MeepMeep/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle new file mode 100644 index 000000000000..cb8c19758d0b --- /dev/null +++ b/MeepMeep/build.gradle @@ -0,0 +1,17 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +repositories { + maven { url = 'https://jitpack.io' } + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'com.github.rh-robotics:MeepMeep:v1.0.0' +} \ No newline at end of file diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java new file mode 100644 index 000000000000..e851a14aeeb6 --- /dev/null +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -0,0 +1,55 @@ +package com.example.meepmeep; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + +import org.rowlandhall.meepmeep.MeepMeep; +import org.rowlandhall.meepmeep.core.colorscheme.scheme.ColorSchemeRedDark; +import org.rowlandhall.meepmeep.roadrunner.DefaultBotBuilder; +import org.rowlandhall.meepmeep.roadrunner.entity.RoadRunnerBotEntity; + +public class MeepTurbo { + public static void main(String[] args) { + // Declare a MeepMeep instance + // With a field size of 800 pixels + MeepMeep meepMeep = new MeepMeep(800); + + RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) + // Required: Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(500, 60, Math.toRadians(1000), Math.toRadians(1000), 339/2.54) + // Option: Set theme. Default = ColorSchemeRedDark() + .setColorScheme(new ColorSchemeRedDark()) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(59.71, 36.96, Math.toRadians(153.15))) +// .forward(50) +// .turn(Math.toRadians(90)) +// .forward(50) +// .addDisplacementMarker(() -> { +// /* Everything in the marker callback should be commented out */ +// +// // bot.shooter.shoot() +// // bot.wobbleArm.lower() +// }) +// .turn(Math.toRadians(90)) +// .splineTo(new Vector2d(50, 15), 0) +// .turn(Math.toRadians(90)) +// .build() + //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(59.71, 36.96, Math.toRadians(153.15))) + .splineTo(new Vector2d(10.98, 59.92), Math.toRadians(180.00)) + .splineTo(new Vector2d(-36.35, 37.36), Math.toRadians(223.03)) + .build()); + + + + + + // Set field image + meepMeep.setBackground(MeepMeep.Background.FIELD_FREIGHTFRENZY_ADI_DARK) + .setDarkMode(true) + // Background opacity from 0-1 + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start(); + } +} + diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 878287a7c712..8a4e34ccc19d 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -25,4 +25,7 @@ android { dependencies { implementation project(':FtcRobotController') + implementation 'org.apache.commons:commons-math3:3.6.1' + implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' + implementation 'com.acmerobotics.roadrunner:core:0.5.6' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java new file mode 100644 index 000000000000..937238c1d2de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -0,0 +1,313 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; + +@TeleOp +public class ManualOpMode extends LinearOpMode { + RobotChassis robotChassis; + RobotTop robotTop; + + // servoName(port): positionRange[a, b]; defaultPos + // control hub: + // top(3): [0-idle, 0.62-out] up: 0.05; default: 0 + // container(2): [0.35-open, 1-close]; default: 1 + // lift(5): [0.2-open, 0.6-close];default: 0.6 + // -------------------------------------------- + // expansion hub: + // stretch(1): [0-back, 0.3-out]; default: 0 + // spinX(2): [0, 1]; default: 0.5 + // turn(3): [0.38-back, 0.71-out]; default: 0.38 + // spinY(4): [0, 1]; default: 0.07 + // grab(5): [0.2-open, 0.83-close]; default: 0.2 + + final int LIFT_TOP_POSITION = 1250; + final int LIFT_HANGING_POSITION = 925; + final int LIFT_BOTTOM_POSITION = 50; + final double STRETCH_BACK_POSITION = 0.03; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.22; + final double SPIN_X_HOVERING_POSITION = 0.53; + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_HOVERING_POSITION = 0.64; + final double TURN_OUT_DOWN_POSITION = 0.68; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.9; + + @Override + public void runOpMode() { + robotChassis = new RobotChassis(this); + robotTop = new RobotTop(this); + RobotVisionAngle robotVisionAngle = new RobotVisionAngle(); + robotVisionAngle.initialize(hardwareMap); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + Gamepad previousGamepad2 = new Gamepad(); + previousGamepad2.copy(gamepad2); + + int liftPosition; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; + boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoOut = false; + boolean containerRelease = true; + boolean backGrabOpen = false; + double recognitionAngle = 0; + LiftState liftState = LiftState.BOTTOM; + ArmState armState = ArmState.IDLE; + + waitForStart(); + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmTurnPosition(armTurnPos); + robotTop.setContainerServoPosition(0.35); + robotTop.setLiftServoPosition(0.6); + while (opModeIsActive()) { + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + + // robotLift + liftPosition = robotTop.getLiftPosition(); +// if (liftState == LiftState.BOTTOM) { +// if (gamepad1.y) { +// liftState = LiftState.GOING_UP; +// } +// } else if (liftState == LiftState.GOING_UP) { +// robotTop.setTopServoPosition(0.05); +// if (liftPosition >= LIFT_TOP_POSITION) { +// robotTop.setLeftPower(0); +// liftState = LiftState.TOP; +// } else if (liftPosition >= LIFT_TOP_POSITION - 150) { +// robotTop.setLeftPower(0.3); +// } else if (liftPosition <= LIFT_TOP_POSITION - 150) { +// robotTop.setLeftPower(0.7); +// } +// } else if (liftState == LiftState.TOP) { +// if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { +// robotTop.setLeftPower(-0.6); +// liftState = LiftState.GOING_DOWN; +// } +// if (liftPosition <= LIFT_TOP_POSITION) { +// robotTop.setLeftPower(0.3); +// } +// } else if (liftState == LiftState.GOING_DOWN) { +// if (liftPosition <= LIFT_BOTTOM_POSITION) { +// robotTop.setLeftPower(0); +// robotTop.setTopServoPosition(0.05); +// sleep(500); +// liftState = LiftState.BOTTOM; +// } else if (liftPosition <= LIFT_BOTTOM_POSITION + 150) { +// robotTop.setLeftPower(-0.2); +// } +// } +// if (gamepad1.y && !previousGamepad1.y) { +// if (armState == ArmState.IDLE) { +// robotChassis.stopMotor(); +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); +// sleep(1000); +// armState = ArmState.LOCKED; +// liftState = LiftState.GOING_UP; +// } else if (armState == ArmState.LOCKED) { +// robotChassis.stopMotor(); +// sleep(1500); +// armStretchPos = STRETCH_BACK_POSITION; +// armState = ArmState.WITHDRAWING; +// } +// } + if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { + if (gamepad1.left_trigger != 0) { + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + armState = ArmState.STRETCHED; + sleep(1000); + } + if (gamepad1.right_trigger != 0) { + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + armState = ArmState.STRETCHED; + sleep(1000); + } + } else { + if (gamepad1.left_trigger != 0) { + if(!topServoOut){ + robotTop.setTopServoPosition(0.05); + } + robotTop.setLeftPower(-0.3); + } else if (gamepad1.right_trigger != 0) { + if(!topServoOut){ + robotTop.setTopServoPosition(0.05); + } + robotTop.setLeftPower(0.5); + } else { + if(LIFT_HANGING_POSITION - 100 <= liftPosition && liftPosition <= LIFT_HANGING_POSITION){ + robotTop.setLeftPower(0.03); + }else{ + robotTop.setLeftPower(0); + } + } + } + if (gamepad1.left_bumper && !previousGamepad1.left_bumper && liftPosition > 1100) { + if (topServoOut) { + robotTop.setTopServoPosition(0.05); + } else { + robotTop.setTopServoPosition(0.6); + } + topServoOut = !topServoOut; + } + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + if (containerRelease) { + robotTop.setContainerServoPosition(1); + } else { + robotTop.setContainerServoPosition(0.35); + } + containerRelease = !containerRelease; + } + + // robotArm + if (gamepad1.x && !previousGamepad1.x) { + if (armState == ArmState.IDLE) { + robotTop.setTopServoPosition(0); + armStretchPos = STRETCH_OUT_POSITION; + armState = ArmState.TURNING_OUT; + robotTop.setArmStretchPosition(armStretchPos); + } else if (armState == ArmState.TURNED) { + armState = ArmState.TURNING_BACK; + robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); + } + } + if (armState == ArmState.WITHDRAWING) { + if (armStretchPos <= STRETCH_BACK_POSITION) { + armState = ArmState.IDLE; + } else { + armStretchPos -= 0.03; + robotTop.setArmStretchPosition(armStretchPos); + } + } + if (armState == ArmState.TURNING_OUT) { + if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { + armTurnPos = TURN_OUT_HOVERING_POSITION; + robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + armState = ArmState.TURNED; + } else { + armTurnPos += 0.03; + robotTop.setArmTurnPosition(armTurnPos); + } + } + if (armState == ArmState.TURNING_BACK) { + if (armTurnPos <= TURN_BACK_POSITION + 0.05) { + armTurnPos = TURN_BACK_POSITION; + robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); + armState = ArmState.WITHDRAWING; + } else { + armTurnPos -= 0.03; + robotTop.setArmTurnPosition(armTurnPos); + } + } + if (armState == ArmState.STRETCHED) { + if (gamepad1.x && !previousGamepad1.x) { + robotTop.setTopServoPosition(0); + armState = ArmState.WITHDRAWING; + } + } + if (gamepad1.b && !previousGamepad1.b) { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + sleep(500); + if (!armGrabbing && armState == ArmState.TURNED) { + robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); + robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); + grabbingFlag = true; + } else { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + armGrabbing = false; + } + } + if (grabbingFlag && !gamepad1.b) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + sleep(500); + robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); + robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + armGrabbing = true; + grabbingFlag = false; + } + robotTop.setArmStretchPosition(armStretchPos); + if (gamepad1.dpad_up) { + armSpinXPos = Math.min(1, armSpinXPos + 0.02); + robotTop.setArmSpinXPosition(armSpinXPos); + } else if (gamepad1.dpad_down) { + armSpinXPos = Math.max(0, armSpinXPos - 0.02); + robotTop.setArmSpinXPosition(armSpinXPos); + } + if (gamepad1.dpad_right) { + armSpinYPos = Math.min(1, armSpinYPos + 0.05); + robotTop.setArmSpinYPosition(armSpinYPos); + } else if (gamepad1.dpad_left) { + armSpinYPos = Math.max(0, armSpinYPos - 0.05); + robotTop.setArmSpinYPosition(armSpinYPos); + } + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } + + //vision + if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 + ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { + //robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); + recognitionAngle = robotVisionAngle.getDetectedAngle(); + robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); + } + + // telemetry + telemetry.addData("liftPos", liftPosition); + telemetry.addData("state", liftState); + telemetry.addData("ArmState", armState); + telemetry.addData("XPos", armSpinXPos); + telemetry.addData("YPos", armSpinYPos); + telemetry.addData("angle", recognitionAngle); + telemetry.update(); + previousGamepad1.copy(gamepad1); + previousGamepad2.copy(gamepad2); + sleep(50); + } + } + + protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + return 0; + } + + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 180 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 180; + } + + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + } + + + enum LiftState { + BOTTOM, GOING_UP, TOP, GOING_DOWN + } + + enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED, STRETCHED + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java new file mode 100644 index 000000000000..efb7ac891d29 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.RobotAuto; + +@Autonomous +public class RedLeftAuto extends LinearOpMode { + RobotAuto robotAuto; + @Override + public void runOpMode(){ + robotAuto = new RobotAuto(this); + waitForStart(); + resetRuntime(); + + robotAuto.grab() + .setLiftPower(0.5) + .sleep(400) + .setLiftPower(0) + .fastBackward(24) + .leftShift(24) + .fastSpin(180) + .setLiftPower(0.3) + .sleep(800) + .fastBackward(10) + .setLiftPower(-0.5) + .sleep(200) + .setLiftPower(0) + .release(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java new file mode 100644 index 000000000000..d5071362c671 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; + + +public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java new file mode 100644 index 000000000000..64d3df5bd8f0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; + +import java.util.Collections; +import java.util.List; + +public class TrajectorySequence { + private final List sequenceList; + + public TrajectorySequence(List sequenceList) { + if (sequenceList.size() == 0) throw new EmptySequenceException(); + + this.sequenceList = Collections.unmodifiableList(sequenceList); + } + + public Pose2d start() { + return sequenceList.get(0).getStartPose(); + } + + public Pose2d end() { + return sequenceList.get(sequenceList.size() - 1).getEndPose(); + } + + public double duration() { + double total = 0.0; + + for (SequenceSegment segment : sequenceList) { + total += segment.getDuration(); + } + + return total; + } + + public SequenceSegment get(int i) { + return sequenceList.get(i); + } + + public int size() { + return sequenceList.size(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java new file mode 100644 index 000000000000..0b153bbe7b34 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java @@ -0,0 +1,679 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.PathContinuityViolationException; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.DisplacementMarker; +import com.acmerobotics.roadrunner.trajectory.DisplacementProducer; +import com.acmerobotics.roadrunner.trajectory.MarkerCallback; +import com.acmerobotics.roadrunner.trajectory.SpatialMarker; +import com.acmerobotics.roadrunner.trajectory.TemporalMarker; +import com.acmerobotics.roadrunner.trajectory.TimeProducer; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.acmerobotics.roadrunner.util.Angle; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; + +public class TrajectorySequenceBuilder { + private final double resolution = 0.25; + + private final TrajectoryVelocityConstraint baseVelConstraint; + private final TrajectoryAccelerationConstraint baseAccelConstraint; + + private TrajectoryVelocityConstraint currentVelConstraint; + private TrajectoryAccelerationConstraint currentAccelConstraint; + + private final double baseTurnConstraintMaxAngVel; + private final double baseTurnConstraintMaxAngAccel; + + private double currentTurnConstraintMaxAngVel; + private double currentTurnConstraintMaxAngAccel; + + private final List sequenceSegments; + + private final List temporalMarkers; + private final List displacementMarkers; + private final List spatialMarkers; + + private Pose2d lastPose; + + private double tangentOffset; + + private boolean setAbsoluteTangent; + private double absoluteTangent; + + private TrajectoryBuilder currentTrajectoryBuilder; + + private double currentDuration; + private double currentDisplacement; + + private double lastDurationTraj; + private double lastDisplacementTraj; + + public TrajectorySequenceBuilder( + Pose2d startPose, + Double startTangent, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this.baseVelConstraint = baseVelConstraint; + this.baseAccelConstraint = baseAccelConstraint; + + this.currentVelConstraint = baseVelConstraint; + this.currentAccelConstraint = baseAccelConstraint; + + this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + sequenceSegments = new ArrayList<>(); + + temporalMarkers = new ArrayList<>(); + displacementMarkers = new ArrayList<>(); + spatialMarkers = new ArrayList<>(); + + lastPose = startPose; + + tangentOffset = 0.0; + + setAbsoluteTangent = (startTangent != null); + absoluteTangent = startTangent != null ? startTangent : 0.0; + + currentTrajectoryBuilder = null; + + currentDuration = 0.0; + currentDisplacement = 0.0; + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + } + + public TrajectorySequenceBuilder( + Pose2d startPose, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this( + startPose, null, + baseVelConstraint, baseAccelConstraint, + baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel + ); + } + + public TrajectorySequenceBuilder lineTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder forward(double distance) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder forward( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder back(double distance) { + return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder back( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineTo( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + private TrajectorySequenceBuilder addPath(AddPathCallback callback) { + if (currentTrajectoryBuilder == null) newPath(); + + try { + callback.run(); + } catch (PathContinuityViolationException e) { + newPath(); + callback.run(); + } + + Trajectory builtTraj = currentTrajectoryBuilder.build(); + + double durationDifference = builtTraj.duration() - lastDurationTraj; + double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj; + + lastPose = builtTraj.end(); + currentDuration += durationDifference; + currentDisplacement += displacementDifference; + + lastDurationTraj = builtTraj.duration(); + lastDisplacementTraj = builtTraj.getPath().length(); + + return this; + } + + public TrajectorySequenceBuilder setTangent(double tangent) { + setAbsoluteTangent = true; + absoluteTangent = tangent; + + pushPath(); + + return this; + } + + private TrajectorySequenceBuilder setTangentOffset(double offset) { + setAbsoluteTangent = false; + + this.tangentOffset = offset; + this.pushPath(); + + return this; + } + + public TrajectorySequenceBuilder setReversed(boolean reversed) { + return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0); + } + + public TrajectorySequenceBuilder setConstraints( + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + this.currentVelConstraint = velConstraint; + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetConstraints() { + this.currentVelConstraint = this.baseVelConstraint; + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) { + this.currentVelConstraint = velConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetVelConstraint() { + this.currentVelConstraint = this.baseVelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) { + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetAccelConstraint() { + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) { + this.currentTurnConstraintMaxAngVel = maxAngVel; + this.currentTurnConstraintMaxAngAccel = maxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder resetTurnConstraint() { + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) { + return this.addTemporalMarker(currentDuration, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) { + return this.addTemporalMarker(currentDuration + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) { + return this.addTemporalMarker(0.0, time, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) { + return this.addTemporalMarker(time -> scale * time + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) { + this.temporalMarkers.add(new TemporalMarker(time, callback)); + return this; + } + + public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) { + this.spatialMarkers.add(new SpatialMarker(point, callback)); + return this; + } + + public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement + offset, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) { + return this.addDisplacementMarker(0.0, displacement, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) { + return addDisplacementMarker((displacement -> scale * displacement + offset), callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) { + displacementMarkers.add(new DisplacementMarker(displacement, callback)); + + return this; + } + + public TrajectorySequenceBuilder turn(double angle) { + return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel); + } + + public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) { + pushPath(); + + MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( + new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0), + new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0), + maxAngVel, + maxAngAccel + ); + + sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList())); + + lastPose = new Pose2d( + lastPose.getX(), lastPose.getY(), + Angle.norm(lastPose.getHeading() + angle) + ); + + currentDuration += turnProfile.duration(); + + return this; + } + + public TrajectorySequenceBuilder waitSeconds(double seconds) { + pushPath(); + sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList())); + + currentDuration += seconds; + return this; + } + + public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) { + pushPath(); + + sequenceSegments.add(new TrajectorySegment(trajectory)); + return this; + } + + private void pushPath() { + if (currentTrajectoryBuilder != null) { + Trajectory builtTraj = currentTrajectoryBuilder.build(); + sequenceSegments.add(new TrajectorySegment(builtTraj)); + } + + currentTrajectoryBuilder = null; + } + + private void newPath() { + if (currentTrajectoryBuilder != null) + pushPath(); + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + + double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset); + + currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution); + } + + public TrajectorySequence build() { + pushPath(); + + List globalMarkers = convertMarkersToGlobal( + sequenceSegments, + temporalMarkers, displacementMarkers, spatialMarkers + ); + projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments); + + return new TrajectorySequence(sequenceSegments); + } + + private List convertMarkersToGlobal( + List sequenceSegments, + List temporalMarkers, + List displacementMarkers, + List spatialMarkers + ) { + ArrayList trajectoryMarkers = new ArrayList<>(); + + // Convert temporal markers + for (TemporalMarker marker : temporalMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback()) + ); + } + + // Convert displacement markers + for (DisplacementMarker marker : displacementMarkers) { + double time = displacementToTime( + sequenceSegments, + marker.getProducer().produce(currentDisplacement) + ); + + trajectoryMarkers.add( + new TrajectoryMarker( + time, + marker.getCallback() + ) + ); + } + + // Convert spatial markers + for (SpatialMarker marker : spatialMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker( + pointToTime(sequenceSegments, marker.getPoint()), + marker.getCallback() + ) + ); + } + + return trajectoryMarkers; + } + + private void projectGlobalMarkersToLocalSegments(List markers, List sequenceSegments) { + if (sequenceSegments.isEmpty()) return; + + markers.sort(Comparator.comparingDouble(TrajectoryMarker::getTime)); + + double timeOffset = 0.0; + int markerIndex = 0; + for (SequenceSegment segment : sequenceSegments) { + while (markerIndex < markers.size()) { + TrajectoryMarker marker = markers.get(markerIndex); + if (marker.getTime() >= timeOffset + segment.getDuration()) { + break; + } + + segment.getMarkers().add(new TrajectoryMarker( + Math.max(0.0, marker.getTime()) - timeOffset, marker.getCallback())); + ++markerIndex; + } + + timeOffset += segment.getDuration(); + } + + SequenceSegment segment = sequenceSegments.get(sequenceSegments.size() - 1); + while (markerIndex < markers.size()) { + TrajectoryMarker marker = markers.get(markerIndex); + segment.getMarkers().add(new TrajectoryMarker(segment.getDuration(), marker.getCallback())); + ++markerIndex; + } + } + + // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private + // note: this assumes that the profile position is monotonic increasing + private Double motionProfileDisplacementToTime(MotionProfile profile, double s) { + double tLo = 0.0; + double tHi = profile.duration(); + while (!(Math.abs(tLo - tHi) < 1e-6)) { + double tMid = 0.5 * (tLo + tHi); + if (profile.get(tMid).getX() > s) { + tHi = tMid; + } else { + tLo = tMid; + } + } + return 0.5 * (tLo + tHi); + } + + private Double displacementToTime(List sequenceSegments, double s) { + double currentTime = 0.0; + double currentDisplacement = 0.0; + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double segmentLength = thisSegment.getTrajectory().getPath().length(); + + if (currentDisplacement + segmentLength > s) { + double target = s - currentDisplacement; + double timeInSegment = motionProfileDisplacementToTime( + thisSegment.getTrajectory().getProfile(), + target + ); + + return currentTime + timeInSegment; + } else { + currentDisplacement += segmentLength; + } + } + + currentTime += segment.getDuration(); + } + + return currentTime; + } + + private Double pointToTime(List sequenceSegments, Vector2d point) { + class ComparingPoints { + private final double distanceToPoint; + private final double totalDisplacement; + private final double thisPathDisplacement; + + public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) { + this.distanceToPoint = distanceToPoint; + this.totalDisplacement = totalDisplacement; + this.thisPathDisplacement = thisPathDisplacement; + } + } + + List projectedPoints = new ArrayList<>(); + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25); + Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec(); + double distanceToPoint = point.minus(projectedPoint).norm(); + + double totalDisplacement = 0.0; + + for (ComparingPoints comparingPoint : projectedPoints) { + totalDisplacement += comparingPoint.totalDisplacement; + } + + totalDisplacement += displacement; + + projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement)); + } + } + + ComparingPoints closestPoint = null; + + for (ComparingPoints comparingPoint : projectedPoints) { + if (closestPoint == null) { + closestPoint = comparingPoint; + continue; + } + + if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) + closestPoint = comparingPoint; + } + + return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement); + } + + private interface AddPathCallback { + void run(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java new file mode 100644 index 000000000000..bcfa8698a008 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java @@ -0,0 +1,306 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; + +import androidx.annotation.Nullable; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.DashboardUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LogFiles; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedList; +import java.util.List; + +@Config +public class TrajectorySequenceRunner { + public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a"; + public static String COLOR_INACTIVE_TURN = "#7c4dff7a"; + public static String COLOR_INACTIVE_WAIT = "#dd2c007a"; + + public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50"; + public static String COLOR_ACTIVE_TURN = "#7c4dff"; + public static String COLOR_ACTIVE_WAIT = "#dd2c00"; + + public static int POSE_HISTORY_LIMIT = 100; + + private final TrajectoryFollower follower; + + private final PIDFController turnController; + + private final NanoClock clock; + + private TrajectorySequence currentTrajectorySequence; + private double currentSegmentStartTime; + private int currentSegmentIndex; + private int lastSegmentIndex; + + private Pose2d lastPoseError = new Pose2d(); + + List remainingMarkers = new ArrayList<>(); + + private final FtcDashboard dashboard; + private final LinkedList poseHistory = new LinkedList<>(); + + private VoltageSensor voltageSensor; + + private List lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels; + + public TrajectorySequenceRunner( + TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients, VoltageSensor voltageSensor, + List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels + ) { + this.follower = follower; + + turnController = new PIDFController(headingPIDCoefficients); + turnController.setInputBounds(0, 2 * Math.PI); + + this.voltageSensor = voltageSensor; + + this.lastDriveEncPositions = lastDriveEncPositions; + this.lastDriveEncVels = lastDriveEncVels; + this.lastTrackingEncPositions = lastTrackingEncPositions; + this.lastTrackingEncVels = lastTrackingEncVels; + + clock = NanoClock.system(); + + dashboard = FtcDashboard.getInstance(); + dashboard.setTelemetryTransmissionInterval(25); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + currentTrajectorySequence = trajectorySequence; + currentSegmentStartTime = clock.seconds(); + currentSegmentIndex = 0; + lastSegmentIndex = -1; + } + + public @Nullable + DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) { + Pose2d targetPose = null; + DriveSignal driveSignal = null; + + TelemetryPacket packet = new TelemetryPacket(); + Canvas fieldOverlay = packet.fieldOverlay(); + + SequenceSegment currentSegment = null; + + if (currentTrajectorySequence != null) { + if (currentSegmentIndex >= currentTrajectorySequence.size()) { + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + currentTrajectorySequence = null; + } + + if (currentTrajectorySequence == null) + return new DriveSignal(); + + double now = clock.seconds(); + boolean isNewTransition = currentSegmentIndex != lastSegmentIndex; + + currentSegment = currentTrajectorySequence.get(currentSegmentIndex); + + if (isNewTransition) { + currentSegmentStartTime = now; + lastSegmentIndex = currentSegmentIndex; + + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + remainingMarkers.addAll(currentSegment.getMarkers()); + Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime())); + } + + double deltaTime = now - currentSegmentStartTime; + + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + if (isNewTransition) + follower.followTrajectory(currentTrajectory); + + if (!follower.isFollowing()) { + currentSegmentIndex++; + + driveSignal = new DriveSignal(); + } else { + driveSignal = follower.update(poseEstimate, poseVelocity); + lastPoseError = follower.getLastError(); + } + + targetPose = currentTrajectory.get(deltaTime); + } else if (currentSegment instanceof TurnSegment) { + MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime); + + turnController.setTargetPosition(targetState.getX()); + + double correction = turnController.update(poseEstimate.getHeading()); + + double targetOmega = targetState.getV(); + double targetAlpha = targetState.getA(); + + lastPoseError = new Pose2d(0, 0, turnController.getLastError()); + + Pose2d startPose = currentSegment.getStartPose(); + targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX()); + + driveSignal = new DriveSignal( + new Pose2d(0, 0, targetOmega + correction), + new Pose2d(0, 0, targetAlpha) + ); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + driveSignal = new DriveSignal(); + } + } else if (currentSegment instanceof WaitSegment) { + lastPoseError = new Pose2d(); + + targetPose = currentSegment.getStartPose(); + driveSignal = new DriveSignal(); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + } + } + + while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) { + remainingMarkers.get(0).getCallback().onMarkerReached(); + remainingMarkers.remove(0); + } + } + + poseHistory.add(poseEstimate); + + if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) { + poseHistory.removeFirst(); + } + + final double NOMINAL_VOLTAGE = 12.0; + double voltage = voltageSensor.getVoltage(); + if (driveSignal != null && !DriveConstants.RUN_USING_ENCODER) { + driveSignal = new DriveSignal( + driveSignal.getVel().times(NOMINAL_VOLTAGE / voltage), + driveSignal.getAccel().times(NOMINAL_VOLTAGE / voltage) + ); + } + + if (targetPose != null) { + LogFiles.record( + targetPose, poseEstimate, voltage, + lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels + ); + } + + packet.put("x", poseEstimate.getX()); + packet.put("y", poseEstimate.getY()); + packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading())); + + packet.put("xError", getLastPoseError().getX()); + packet.put("yError", getLastPoseError().getY()); + packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading())); + + draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate); + + dashboard.sendTelemetryPacket(packet); + + return driveSignal; + } + + private void draw( + Canvas fieldOverlay, + TrajectorySequence sequence, SequenceSegment currentSegment, + Pose2d targetPose, Pose2d poseEstimate + ) { + if (sequence != null) { + for (int i = 0; i < sequence.size(); i++) { + SequenceSegment segment = sequence.get(i); + + if (segment instanceof TrajectorySegment) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath()); + } else if (segment instanceof TurnSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setFill(COLOR_INACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2); + } else if (segment instanceof WaitSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + } + + if (currentSegment != null) { + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath()); + } else if (currentSegment instanceof TurnSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setFill(COLOR_ACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3); + } else if (currentSegment instanceof WaitSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + + if (targetPose != null) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke("#4CAF50"); + DashboardUtil.drawRobot(fieldOverlay, targetPose); + } + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory); + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawRobot(fieldOverlay, poseEstimate); + } + + public Pose2d getLastPoseError() { + return lastPoseError; + } + + public boolean isBusy() { + return currentTrajectorySequence != null; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java new file mode 100644 index 000000000000..5608ac17e836 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +/* + * Constants shared between multiple drive types. + * + * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final + * fields may also be edited through the dashboard (connect to the robot's WiFi network and + * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you + * adjust them in the dashboard; **config variable changes don't persist between app restarts**. + * + * These are not the only parameters; some are located in the localizer classes, drive base classes, + * and op modes themselves. + */ +@Config +public class DriveConstants { + + /* + * These are motor constants that should be listed online for your motors. + */ + public static final double TICKS_PER_REV = 500; + public static final double MAX_RPM = 300; + + /* + * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. + * Set this flag to false if drive encoders are not present and an alternative localization + * method is in use (e.g., tracking wheels). + * + * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients + * from DriveVelocityPIDTuner. + */ + public static final boolean RUN_USING_ENCODER = false; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); + + /* + * These are physical constants that can be determined from your robot (including the track + * width; it will be tune empirically later although a rough estimate is important). Users are + * free to chose whichever linear distance unit they would like so long as it is consistently + * used. The default values were selected with inches in mind. Road runner uses radians for + * angular distances although most angular parameters are wrapped in Math.toRadians() for + * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. + */ + public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) + public static double GEAR_RATIO = 1.0/17.515 + + ; // (齿轮比) output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 339/2.54; // (轮距 单位：英寸) in + + /* * These are the feedforward parameters used to model the drive motor behavior. If you are using + * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive + * motor encoders or have elected not to use them for velocity control, these values should be + * empirically tuned. + */ + public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + public static double kA = 0.01; + public static double kStatic = 0.05; + + /* + * These values are used to generate the trajectories for you robot. To ensure proper operation, + * the constraints should never exceed ~80% of the robot's actual capabilities. While Road + * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start + * small and gradually increase them later after everything is working. All distance units are + * inches. + */ + //TODO:ADJUST THESE NUMBERS. + public static double MAX_VEL = 50; + public static double MAX_ACCEL = 30; + public static double MAX_ANG_VEL = Math.toRadians(180); + public static double MAX_ANG_ACCEL = Math.toRadians(180); + + /* + * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. + */ + public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + public static double rpmToVelocity(double rpm) { + return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0; + } + + public static double getMotorVelocityF(double ticksPerSecond) { + // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx + return 32767 / ticksPerSecond; + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java new file mode 100644 index 000000000000..67ce1abaf000 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java @@ -0,0 +1,299 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.MecanumDrive; +import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LynxModuleUtil; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/* + * Simple mecanum drive hardware implementation for REV hardware. + */ +@Config +public class SampleMecanumDrive extends MecanumDrive { + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.1, 0.01, 0.1); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.1, 0, 0.1); + + public static double LATERAL_MULTIPLIER = 0.357124; + + public static double VX_WEIGHT = 1; + public static double VY_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1.3; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL); + + private TrajectoryFollower follower; + + private DcMotorEx leftFront, leftRear, rightRear, rightFront; + private List motors; + + private IMU imu; + private VoltageSensor batteryVoltageSensor; + + private List lastEncPositions = new ArrayList<>(); + private List lastEncVels = new ArrayList<>(); + + public SampleMecanumDrive(HardwareMap hardwareMap) { + super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER); + + follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(IMU.class, "imu"); + IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); + imu.initialize(parameters); + + leftFront = hardwareMap.get(DcMotorEx.class, "FL"); + leftRear = hardwareMap.get(DcMotorEx.class, "BL"); + rightRear = hardwareMap.get(DcMotorEx.class, "BR"); + rightFront = hardwareMap.get(DcMotorEx.class, "FR"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (DriveConstants.RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); + } + + // reverse any motors using DcMotor.setDirection() + + List lastTrackingEncPositions = new ArrayList<>(); + List lastTrackingEncVels = new ArrayList<>(); + + // TODO: if desired, use setLocalizer() to change the localization method + // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); + + trajectorySequenceRunner = new TrajectorySequenceRunner( + follower, HEADING_PID, batteryVoltageSensor, + lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels + ); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, ACCEL_CONSTRAINT, + DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY()) + + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + VY_WEIGHT * Math.abs(drivePower.getY()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + VY_WEIGHT * drivePower.getY(), + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + lastEncPositions.clear(); + + List wheelPositions = new ArrayList<>(); + for (DcMotorEx motor : motors) { + int position = motor.getCurrentPosition(); + lastEncPositions.add(position); + wheelPositions.add(DriveConstants.encoderTicksToInches(position)); + } + return wheelPositions; + } + + @Override + public List getWheelVelocities() { + lastEncVels.clear(); + + List wheelVelocities = new ArrayList<>(); + for (DcMotorEx motor : motors) { + int vel = (int) motor.getVelocity(); + lastEncVels.add(vel); + wheelVelocities.add(DriveConstants.encoderTicksToInches(vel)); + } + return wheelVelocities; + } + + @Override + public void setMotorPowers(double v, double v1, double v2, double v3) { + leftFront.setPower(v); + leftRear.setPower(v1); + rightRear.setPower(v2); + rightFront.setPower(v3); + } + + @Override + public double getRawExternalHeading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + @Override + public Double getExternalHeadingVelocity() { + return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new MecanumVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java new file mode 100644 index 000000000000..a9ee7df54da3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java @@ -0,0 +1,293 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.TankDrive; +import com.acmerobotics.roadrunner.followers.TankPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LynxModuleUtil; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/* + * Simple tank drive hardware implementation for REV hardware. + */ +@Config +public class SampleTankDrive extends TankDrive { + public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + + public static double VX_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(DriveConstants.MAX_ACCEL); + + private TrajectoryFollower follower; + + private List motors, leftMotors, rightMotors; + private IMU imu; + + private VoltageSensor batteryVoltageSensor; + + public SampleTankDrive(HardwareMap hardwareMap) { + super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH); + + follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(IMU.class, "imu"); + IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); + imu.initialize(parameters); + + // add/remove motors depending on your robot (e.g., 6WD) + DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + leftMotors = Arrays.asList(leftFront, leftRear); + rightMotors = Arrays.asList(rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (DriveConstants.RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); + } + + // TODO: reverse any motors using DcMotor.setDirection() + + // TODO: if desired, use setLocalizer() to change the localization method + // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + + trajectorySequenceRunner = new TrajectorySequenceRunner( + follower, HEADING_PID, batteryVoltageSensor, + new ArrayList<>(), new ArrayList<>(), new ArrayList<>(), new ArrayList<>() + ); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, accelConstraint, + DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + 0, + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } else { + // Ensure the y axis is zeroed out. + vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading()); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += DriveConstants.encoderTicksToInches(leftMotor.getCurrentPosition()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += DriveConstants.encoderTicksToInches(rightMotor.getCurrentPosition()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + public List getWheelVelocities() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += DriveConstants.encoderTicksToInches(leftMotor.getVelocity()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += DriveConstants.encoderTicksToInches(rightMotor.getVelocity()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + @Override + public void setMotorPowers(double v, double v1) { + for (DcMotorEx leftMotor : leftMotors) { + leftMotor.setPower(v); + } + for (DcMotorEx rightMotor : rightMotors) { + rightMotor.setPower(v1); + } + } + + @Override + public double getRawExternalHeading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + @Override + public Double getExternalHeadingVelocity() { + return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new TankVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java new file mode 100644 index 000000000000..0b59fd321f5d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +@Config +public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 0; + public static double WHEEL_RADIUS = 2; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel + + private Encoder leftEncoder, rightEncoder, frontEncoder; + + private List lastEncPositions, lastEncVels; + + public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right + new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front + )); + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftEncoder.getCurrentPosition(); + int rightPos = rightEncoder.getCurrentPosition(); + int frontPos = frontEncoder.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos), + encoderTicksToInches(rightPos), + encoderTicksToInches(frontPos) + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = (int) leftEncoder.getCorrectedVelocity(); + int rightVel = (int) rightEncoder.getCorrectedVelocity(); + int frontVel = (int) frontEncoder.getCorrectedVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel), + encoderTicksToInches(rightVel), + encoderTicksToInches(frontVel) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java new file mode 100644 index 000000000000..b3a0ec77e2e9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java @@ -0,0 +1,221 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_RPM; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.rpmToVelocity; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LoggingUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.RegressionUtil; + +import java.util.ArrayList; +import java.util.List; + +/* + * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an + * outline of the procedure: + * 1. Slowly ramp the motor power and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) + * and an optional intercept (kStatic). + * 3. Accelerate the robot (apply constant power) and record the encoder counts. + * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear + * regression. + */ + +@Autonomous(group = "drive") +public class AutomaticFeedforwardTuner extends LinearOpMode { + public static double MAX_POWER = 0.7; + public static double DISTANCE = 100; // in + + @Override + public void runOpMode() throws InterruptedException { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Press play to begin the feedforward tuning routine"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Would you like to fit kStatic?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitIntercept = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitIntercept = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + telemetry.clearAll(); + telemetry.addLine(Misc.formatInvariant( + "Place your robot on the field with at least %.2f in of room in front", DISTANCE)); + telemetry.addLine("Press (Y/Δ) to begin"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxVel = rpmToVelocity(MAX_RPM); + double finalVel = MAX_POWER * maxVel; + double accel = (finalVel * finalVel) / (2.0 * DISTANCE); + double rampTime = Math.sqrt(2.0 * DISTANCE / accel); + + List timeSamples = new ArrayList<>(); + List positionSamples = new ArrayList<>(); + List powerSamples = new ArrayList<>(); + + drive.setPoseEstimate(new Pose2d()); + + double startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > rampTime) { + break; + } + double vel = accel * elapsedTime; + double power = vel / maxVel; + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(power); + + drive.setDrivePower(new Pose2d(power, 0.0, 0.0)); + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData( + timeSamples, positionSamples, powerSamples, fitIntercept, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveRampRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Quasi-static ramp up test complete"); + if (fitIntercept) { + telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)", + rampResult.kV, rampResult.kStatic, rampResult.rSquare)); + } else { + telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)", + rampResult.kStatic, rampResult.rSquare)); + } + telemetry.addLine("Would you like to fit kA?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitAccelFF = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitAccelFF = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + if (fitAccelFF) { + telemetry.clearAll(); + telemetry.addLine("Place the robot back in its starting position"); + telemetry.addLine("Press (Y/Δ) to continue"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxPowerTime = DISTANCE / maxVel; + + timeSamples.clear(); + positionSamples.clear(); + powerSamples.clear(); + + drive.setPoseEstimate(new Pose2d()); + drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0)); + + startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > maxPowerTime) { + break; + } + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(MAX_POWER); + + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData( + timeSamples, positionSamples, powerSamples, rampResult, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveAccelRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Constant power test complete"); + telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)", + accelResult.kA, accelResult.rSquare)); + telemetry.update(); + } + + while (!isStopRequested()) { + idle(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java new file mode 100644 index 000000000000..5571ba48330f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +@Config +@Autonomous(group = "drive") +public class BackAndForth extends LinearOpMode { + + public static double DISTANCE = 50; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) + .back(DISTANCE) + .build(); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.followTrajectory(trajectoryForward); + drive.followTrajectory(trajectoryBackward); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java new file mode 100644 index 000000000000..66abd1a5b4ee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java @@ -0,0 +1,171 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +import java.util.List; + +/* + * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- + * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as + * important as the positional parameters. Like the other manual tuning routines, this op mode + * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's + * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC + * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully + * connected, start the program, and your robot will begin moving forward and backward according to + * a motion profile. Your job is to graph the velocity errors over time and adjust the PID + * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). + * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the + * MOTOR_VELO_PID field. + * + * Recommended tuning process: + * + * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to + * mitigate oscillations. + * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. + * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class DriveVelocityPIDTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + if (!RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" + + "PID is not in use", getClass().getSimpleName()); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Mode mode = Mode.TUNING_MODE; + + double lastKp = MOTOR_VELO_PID.p; + double lastKi = MOTOR_VELO_PID.i; + double lastKd = MOTOR_VELO_PID.d; + double lastKf = MOTOR_VELO_PID.f; + + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = kV * motionState.getV(); + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + + List velocities = drive.getWheelVelocities(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData("measuredVelocity" + i, velocities.get(i)); + telemetry.addData( + "error" + i, + motionState.getV() - velocities.get(i) + ); + } + break; + case DRIVER_MODE: + if (gamepad1.b) { + drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d + || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + lastKp = MOTOR_VELO_PID.p; + lastKi = MOTOR_VELO_PID.i; + lastKd = MOTOR_VELO_PID.d; + lastKf = MOTOR_VELO_PID.f; + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java new file mode 100644 index 000000000000..c1961726bfd5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin driving in a square. + * You should observe the target position (green) and your pose estimate (blue) and adjust your + * follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + */ +@Config +@Autonomous(group = "drive") +public class FollowerPIDTuner extends LinearOpMode { + public static double DISTANCE = 48; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); + + drive.setPoseEstimate(startPose); + + waitForStart(); + + if (isStopRequested()) return; + + while (!isStopRequested()) { + TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .build(); + drive.followTrajectorySequence(trajSeq); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java new file mode 100644 index 000000000000..be5aa14ad4cf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for testing localization. Drive the robot around like a normal + * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight + * errors are not out of the ordinary, especially with sudden drive motions). The goal of this + * exercise is to ascertain whether the localizer has been configured properly (note: the pure + * encoder localizer heading may be significantly off if the track width has not been tuned). + */ +@TeleOp(group = "drive") +public class LocalizationTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java new file mode 100644 index 000000000000..9bf139e48b80 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java @@ -0,0 +1,152 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.kinematics.Kinematics; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +import java.util.Objects; + +/* + * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, + * tuning these coefficients is just as important as the positional parameters. Like the other + * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if + * you are using the Control Hub. Once you've successfully connected, start the program, and your + * robot will begin moving forward and backward according to a motion profile. Your job is to graph + * the velocity errors over time and adjust the feedforward coefficients. Once you've found a + * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class ManualFeedforwardTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + private SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private Mode mode; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry()); + + drive = new SampleMecanumDrive(hardwareMap); + + final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); + + mode = Mode.TUNING_MODE; + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic); + + final double NOMINAL_VOLTAGE = 12.0; + final double voltage = voltageSensor.getVoltage(); + drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0)); + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + double currentVelo = poseVelo.getX(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + telemetry.addData("measuredVelocity", currentVelo); + telemetry.addData("error", motionState.getV() - currentVelo); + break; + case DRIVER_MODE: + if (gamepad1.b) { + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java new file mode 100644 index 000000000000..54223ab82192 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java @@ -0,0 +1,73 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. + *

+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds. + *

+ * Further fine tuning of MAX_ANG_VEL may be desired. + */ + +@Config +@Autonomous(group = "drive") +public class MaxAngularVeloTuner extends LinearOpMode { + public static double RUNTIME = 4.0; + + private ElapsedTime timer; + private double maxAngVelocity = 0.0; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(0, 0, 1)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); + } + + drive.setDrivePower(new Pose2d()); + + telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); + telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); + telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8); + telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8)); + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java new file mode 100644 index 000000000000..ff9211b0c34a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *

+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *

+ * Further fine tuning of kF may be desired. + */ +@Config +@Autonomous(group = "drive") +public class MaxVelocityTuner extends LinearOpMode { + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8); + telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java new file mode 100644 index 000000000000..9b73a1de7a75 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for debugging your motor configuration. + * Pressing each of the buttons will power its respective motor. + * + * Button Mappings: + * + * Xbox/PS4 Button - Motor + * X / ▢ - Front Left + * Y / Δ - Front Right + * B / O - Rear Right + * A / X - Rear Left + * The buttons are mapped to match the wheels spatially if you + * were to rotate the gamepad 45deg°. x/square is the front left + * ________ and each button corresponds to the wheel as you go clockwise + * / ______ \ + * ------------.-' _ '-..+ Front of Bot + * / _ ( Y ) _ \ ^ + * | ( X ) _ ( B ) | Front Left \ Front Right + * ___ '. ( A ) /| Wheel \ Wheel + * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) + * | | | \ + * '.___.' '. | Rear Left \ Rear Right + * '. / Wheel \ Wheel + * \. .' (A/X) \ (B/O) + * \________/ + * + * Uncomment the @Disabled tag below to use this opmode. + */ +@Disabled +@Config +@TeleOp(group = "drive") +public class MotorDirectionDebugger extends LinearOpMode { + public static double MOTOR_POWER = 0.7; + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + telemetry.addLine("Press play to begin the debugging opmode"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + while (!isStopRequested()) { + telemetry.addLine("Press each button to turn on its respective motor"); + telemetry.addLine(); + telemetry.addLine("Xbox/PS4 Button - Motor"); + telemetry.addLine("  X / ▢         - Front Left"); + telemetry.addLine("  Y / Δ         - Front Right"); + telemetry.addLine("  B / O         - Rear  Right"); + telemetry.addLine("  A / X         - Rear  Left"); + telemetry.addLine(); + + if(gamepad1.x) { + drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); + telemetry.addLine("Running Motor: Front Left"); + } else if(gamepad1.y) { + drive.setMotorPowers(0, 0, 0, MOTOR_POWER); + telemetry.addLine("Running Motor: Front Right"); + } else if(gamepad1.b) { + drive.setMotorPowers(0, 0, MOTOR_POWER, 0); + telemetry.addLine("Running Motor: Rear Right"); + } else if(gamepad1.a) { + drive.setMotorPowers(0, MOTOR_POWER, 0, 0); + telemetry.addLine("Running Motor: Rear Left"); + } else { + drive.setMotorPowers(0, 0, 0, 0); + telemetry.addLine("Running Motor: None"); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java new file mode 100644 index 000000000000..e1d36ffa39b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Autonomous(group = "drive") +public class SplineTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + Trajectory traj = drive.trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(30, 30), 0) + .build(); + + drive.followTrajectory(traj); + + sleep(2000); + + drive.followTrajectory( + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java new file mode 100644 index 000000000000..77df5b71fa4d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StrafeTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .strafeRight(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java new file mode 100644 index 000000000000..28254cbb5bd6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StraightTest extends LinearOpMode { + public static double DISTANCE = 20; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java new file mode 100644 index 000000000000..d0f301b9596d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +@Config +@Autonomous(group = "drive") +public class TrackWidthTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + // TODO: if you haven't already, set the localizer to something that doesn't depend on + // drive encoders for computing the heading + + telemetry.addLine("Press play to begin the track width tuner routine"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.normDelta(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; + trackWidthStats.add(trackWidth); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)", + trackWidthStats.getMean(), + trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java new file mode 100644 index 000000000000..d6c2dc649e90 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; + +/** + * This routine determines the effective forward offset for the lateral tracking wheel. + * The procedure executes a point turn at a given angle for a certain number of trials, + * along with a specified delay in milliseconds. The purpose of this is to track the + * change in the y position during the turn. The offset, or distance, of the lateral tracking + * wheel from the center or rotation allows the wheel to spin during a point turn, leading + * to an incorrect measurement for the y position. This creates an arc around around + * the center of rotation with an arc length of change in y and a radius equal to the forward + * offset. We can compute this offset by calculating (change in y position) / (change in heading) + * which returns the radius if the angle (change in heading) is in radians. This is based + * on the arc length formula of length = theta * radius. + * + * To run this routine, simply adjust the desired angle and specify the number of trials + * and the desired delay. Then, run the procedure. Once it finishes, it will print the + * average of all the calculated forward offsets derived from the calculation. This calculated + * forward offset is then added onto the current forward offset to produce an overall estimate + * for the forward offset. You can run this procedure as many times as necessary until a + * satisfactory result is produced. + */ +@Config +@Autonomous(group="drive") +public class TrackingWheelForwardOffsetTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Press play to begin the forward offset tuner"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.norm(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + + drive.getPoseEstimate().getY() / headingAccumulator; + forwardOffsetStats.add(forwardOffset); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)", + forwardOffsetStats.getMean(), + forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java new file mode 100644 index 000000000000..e0b22b087a96 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + * + * Tuning Routine: + * + * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + * + * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + * + * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + * + * 4. Press play to begin the tuning routine. + * + * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + * + * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + * + * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + * + * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + * + * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + * + * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + * + * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the + * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the + * effective center of rotation. You can ignore this effect. The center of rotation will be offset + * slightly but your heading will still be fine. This does not affect your overall tracking + * precision. The heading should still line up. + */ +@Config +@TeleOp(group = "drive") +public class TrackingWheelLateralDistanceTuner extends LinearOpMode { + public static int NUM_TURNS = 10; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Prior to beginning the routine, please read the directions " + + "located in the comments of the opmode file."); + telemetry.addLine("Press play to begin the tuning routine."); + telemetry.addLine(""); + telemetry.addLine("Press Y/△ to stop the routine."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.update(); + + double headingAccumulator = 0; + double lastHeading = 0; + + boolean tuningFinished = false; + + while (!isStopRequested() && !tuningFinished) { + Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + drive.setDrivePower(vel); + + drive.update(); + + double heading = drive.getPoseEstimate().getHeading(); + double deltaHeading = heading - lastHeading; + + headingAccumulator += Angle.normDelta(deltaHeading); + lastHeading = heading; + + telemetry.clearAll(); + telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); + telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addLine(); + telemetry.addLine("Press Y/△ to conclude routine"); + telemetry.update(); + + if (gamepad1.y) + tuningFinished = true; + } + + telemetry.clearAll(); + telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); + telemetry.addLine("Effective LATERAL_DISTANCE: " + + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE); + + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java new file mode 100644 index 000000000000..38beecf9dac5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test turning capabilities. + */ +@Config +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + public static double ANGLE = 90; // deg + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(Math.toRadians(ANGLE)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java new file mode 100644 index 000000000000..5c8e74c3d0b2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.ArrayList; +import java.util.List; + +public abstract class SequenceSegment { + private final double duration; + private final Pose2d startPose; + private final Pose2d endPose; + private final List markers; + + protected SequenceSegment( + double duration, + Pose2d startPose, Pose2d endPose, + List markers + ) { + this.duration = duration; + this.startPose = startPose; + this.endPose = endPose; + this.markers = new ArrayList<>(markers); + } + + public double getDuration() { + return this.duration; + } + + public Pose2d getStartPose() { + return startPose; + } + + public Pose2d getEndPose() { + return endPose; + } + + public List getMarkers() { + return markers; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java new file mode 100644 index 000000000000..cfbfa49b5188 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; + +import java.util.Collections; + +public final class TrajectorySegment extends SequenceSegment { + private final Trajectory trajectory; + + public TrajectorySegment(Trajectory trajectory) { + // Note: Markers are already stored in the `Trajectory` itself. + // This class should not hold any markers + super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList()); + this.trajectory = trajectory; + } + + public Trajectory getTrajectory() { + return this.trajectory; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java new file mode 100644 index 000000000000..c46c54769b3c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.Angle; + +import java.util.List; + +public final class TurnSegment extends SequenceSegment { + private final double totalRotation; + private final MotionProfile motionProfile; + + public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List markers) { + super( + motionProfile.duration(), + startPose, + new Pose2d( + startPose.getX(), startPose.getY(), + Angle.norm(startPose.getHeading() + totalRotation) + ), + markers + ); + + this.totalRotation = totalRotation; + this.motionProfile = motionProfile; + } + + public final double getTotalRotation() { + return this.totalRotation; + } + + public final MotionProfile getMotionProfile() { + return this.motionProfile; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java new file mode 100644 index 000000000000..18337d05e416 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.List; + +public final class WaitSegment extends SequenceSegment { + public WaitSegment(Pose2d pose, double seconds, List markers) { + super(seconds, pose, pose, markers); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java new file mode 100644 index 000000000000..8d2d4b2469fa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import androidx.annotation.Nullable; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.IOException; +import java.io.InputStream; + +/** + * Set of utilities for loading trajectories from assets (the plugin save location). + */ +public class AssetsTrajectoryManager { + + /** + * Loads the group config. + */ + public static @Nullable + TrajectoryGroupConfig loadGroupConfig() { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME); + return TrajectoryConfigManager.loadGroupConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory config with the given name. + */ + public static @Nullable TrajectoryConfig loadConfig(String name) { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + name + ".yaml"); + return TrajectoryConfigManager.loadConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory builder with the given name. + */ + public static @Nullable TrajectoryBuilder loadBuilder(String name) { + TrajectoryGroupConfig groupConfig = loadGroupConfig(); + TrajectoryConfig config = loadConfig(name); + if (groupConfig == null || config == null) { + return null; + } + return config.toTrajectoryBuilder(groupConfig); + } + + /** + * Loads a trajectory with the given name. + */ + public static @Nullable Trajectory load(String name) { + TrajectoryBuilder builder = loadBuilder(name); + if (builder == null) { + return null; + } + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java new file mode 100644 index 000000000000..a7f3bd409a7f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +/** + * IMU axes signs in the order XYZ (after remapping). + */ +public enum AxesSigns { + PPP(0b000), + PPN(0b001), + PNP(0b010), + PNN(0b011), + NPP(0b100), + NPN(0b101), + NNP(0b110), + NNN(0b111); + + public final int bVal; + + AxesSigns(int bVal) { + this.bVal = bVal; + } + + public static AxesSigns fromBinaryValue(int bVal) { + int maskedVal = bVal & 0x07; + switch (maskedVal) { + case 0b000: + return AxesSigns.PPP; + case 0b001: + return AxesSigns.PPN; + case 0b010: + return AxesSigns.PNP; + case 0b011: + return AxesSigns.PNN; + case 0b100: + return AxesSigns.NPP; + case 0b101: + return AxesSigns.NPN; + case 0b110: + return AxesSigns.NNP; + case 0b111: + return AxesSigns.NNN; + default: + throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java new file mode 100644 index 000000000000..5a205bda5513 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +/** + * A direction for an axis to be remapped to + */ +public enum AxisDirection { + POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java new file mode 100644 index 000000000000..17265ab2fc7c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.Path; + +import java.util.List; + +/** + * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases. + */ +public class DashboardUtil { + private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches + private static final double ROBOT_RADIUS = 9; // in + + + public static void drawPoseHistory(Canvas canvas, List poseHistory) { + double[] xPoints = new double[poseHistory.size()]; + double[] yPoints = new double[poseHistory.size()]; + for (int i = 0; i < poseHistory.size(); i++) { + Pose2d pose = poseHistory.get(i); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path, double resolution) { + int samples = (int) Math.ceil(path.length() / resolution); + double[] xPoints = new double[samples]; + double[] yPoints = new double[samples]; + double dx = path.length() / (samples - 1); + for (int i = 0; i < samples; i++) { + double displacement = i * dx; + Pose2d pose = path.get(displacement); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path) { + drawSampledPath(canvas, path, DEFAULT_RESOLUTION); + } + + public static void drawRobot(Canvas canvas, Pose2d pose) { + canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS); + Vector2d v = pose.headingVec().times(ROBOT_RADIUS); + double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2; + double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY(); + canvas.strokeLine(x1, y1, x2, y2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java new file mode 100644 index 000000000000..908b90375dc9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java @@ -0,0 +1,125 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class Encoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + // convert to uint16 + int real = (int) input & 0xffff; + // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits + // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window + real += ((real % 20) / 4) * CPS_STEP; + // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by + real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private int velocityEstimateIdx; + private double[] velocityEstimates; + private double lastUpdateTime; + + public Encoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimates = new double[3]; + this.lastUpdateTime = clock.seconds(); + } + + public Encoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + /** + * Gets the position from the underlying motor and adjusts for the set direction. + * Additionally, this method updates the velocity estimates used for compensated velocity + * + * @return encoder position + */ + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; + velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + /** + * Gets the velocity directly from the underlying motor and compensates for the direction + * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) + * + * @return raw velocity + */ + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + /** + * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity + * that are lost in overflow due to velocity being transmitted as 16 bits. + * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. + * + * @return corrected velocity + */ + public double getCorrectedVelocity() { + double median = velocityEstimates[0] > velocityEstimates[1] + ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) + : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); + return inverseOverflow(getRawVelocity(), median); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java new file mode 100644 index 000000000000..e28fa31ca282 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java @@ -0,0 +1,273 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import android.annotation.SuppressLint; +import android.content.Context; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.fasterxml.jackson.core.JsonFactory; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.ObjectWriter; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier; +import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.WebHandlerManager; + +import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleTankDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; + +import java.io.File; +import java.io.FileInputStream; +import java.io.IOException; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Date; +import java.util.List; +import java.util.Objects; + +import fi.iki.elonen.NanoHTTPD; + +public final class LogFiles { + private static final File ROOT = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/"); + + public static LogFile log = new LogFile("uninitialized"); + + public static class LogFile { + public String version = "quickstart1 v2"; + + public String opModeName; + public long msInit = System.currentTimeMillis(); + public long nsInit = System.nanoTime(); + public long nsStart, nsStop; + + public double ticksPerRev = DriveConstants.TICKS_PER_REV; + public double maxRpm = DriveConstants.MAX_RPM; + public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER; + public double motorP = DriveConstants.MOTOR_VELO_PID.p; + public double motorI = DriveConstants.MOTOR_VELO_PID.i; + public double motorD = DriveConstants.MOTOR_VELO_PID.d; + public double motorF = DriveConstants.MOTOR_VELO_PID.f; + public double wheelRadius = DriveConstants.WHEEL_RADIUS; + public double gearRatio = DriveConstants.GEAR_RATIO; + public double trackWidth = DriveConstants.TRACK_WIDTH; + public double kV = DriveConstants.kV; + public double kA = DriveConstants.kA; + public double kStatic = DriveConstants.kStatic; + public double maxVel = DriveConstants.MAX_VEL; + public double maxAccel = DriveConstants.MAX_ACCEL; + public double maxAngVel = DriveConstants.MAX_ANG_VEL; + public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL; + + public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP; + public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI; + public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD; + public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP; + public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI; + public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD; + public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER; + + public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; + public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; + public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; + public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; + public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; + public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; + public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; + public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; + public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; + + public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; + public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; + public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO; + public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE; + public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET; + + public RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = DriveConstants.LOGO_FACING_DIR; + public RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = DriveConstants.USB_FACING_DIR; + + public List nsTimes = new ArrayList<>(); + + public List targetXs = new ArrayList<>(); + public List targetYs = new ArrayList<>(); + public List targetHeadings = new ArrayList<>(); + + public List xs = new ArrayList<>(); + public List ys = new ArrayList<>(); + public List headings = new ArrayList<>(); + + public List voltages = new ArrayList<>(); + + public List> driveEncPositions = new ArrayList<>(); + public List> driveEncVels = new ArrayList<>(); + public List> trackingEncPositions = new ArrayList<>(); + public List> trackingEncVels = new ArrayList<>(); + + public LogFile(String opModeName) { + this.opModeName = opModeName; + } + } + + public static void record( + Pose2d targetPose, Pose2d pose, double voltage, + List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels + ) { + long nsTime = System.nanoTime(); + if (nsTime - log.nsStart > 3 * 60 * 1_000_000_000L) { + return; + } + + log.nsTimes.add(nsTime); + + log.targetXs.add(targetPose.getX()); + log.targetYs.add(targetPose.getY()); + log.targetHeadings.add(targetPose.getHeading()); + + log.xs.add(pose.getX()); + log.ys.add(pose.getY()); + log.headings.add(pose.getHeading()); + + log.voltages.add(voltage); + + while (log.driveEncPositions.size() < lastDriveEncPositions.size()) { + log.driveEncPositions.add(new ArrayList<>()); + } + while (log.driveEncVels.size() < lastDriveEncVels.size()) { + log.driveEncVels.add(new ArrayList<>()); + } + while (log.trackingEncPositions.size() < lastTrackingEncPositions.size()) { + log.trackingEncPositions.add(new ArrayList<>()); + } + while (log.trackingEncVels.size() < lastTrackingEncVels.size()) { + log.trackingEncVels.add(new ArrayList<>()); + } + + for (int i = 0; i < lastDriveEncPositions.size(); i++) { + log.driveEncPositions.get(i).add(lastDriveEncPositions.get(i)); + } + for (int i = 0; i < lastDriveEncVels.size(); i++) { + log.driveEncVels.get(i).add(lastDriveEncVels.get(i)); + } + for (int i = 0; i < lastTrackingEncPositions.size(); i++) { + log.trackingEncPositions.get(i).add(lastTrackingEncPositions.get(i)); + } + for (int i = 0; i < lastTrackingEncVels.size(); i++) { + log.trackingEncVels.get(i).add(lastTrackingEncVels.get(i)); + } + } + + private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() { + @SuppressLint("SimpleDateFormat") + final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS"); + + final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory()) + .writerWithDefaultPrettyPrinter(); + + @Override + public void onOpModePreInit(OpMode opMode) { + log = new LogFile(opMode.getClass().getCanonicalName()); + + // clean up old files + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified())); + long totalSizeBytes = 0; + for (File f : fs) { + totalSizeBytes += f.length(); + } + + int i = 0; + while (i < fs.length && totalSizeBytes >= 32 * 1000 * 1000) { + totalSizeBytes -= fs[i].length(); + if (!fs[i].delete()) { + RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath()); + } + ++i; + } + } + + @Override + public void onOpModePreStart(OpMode opMode) { + log.nsStart = System.nanoTime(); + } + + @Override + public void onOpModePostStop(OpMode opMode) { + log.nsStop = System.nanoTime(); + + if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) { + //noinspection ResultOfMethodCallIgnored + ROOT.mkdirs(); + + String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json"; + File file = new File(ROOT, filename); + try { + jsonWriter.writeValue(file, log); + } catch (IOException e) { + RobotLog.setGlobalErrorMsg(new RuntimeException(e), + "Unable to write data to " + file.getAbsolutePath()); + } + } + } + }; + + @WebHandlerRegistrar + public static void registerRoutes(Context context, WebHandlerManager manager) { + //noinspection ResultOfMethodCallIgnored + ROOT.mkdirs(); + + // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves + // don't use @OnCreateEventLoop because it's unreliable + OpModeManagerImpl.getOpModeManagerOfActivity( + AppUtil.getInstance().getActivity() + ).registerListener(notifHandler); + + manager.register("/logs", session -> { + final StringBuilder sb = new StringBuilder(); + sb.append("Logs

    "); + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified())); + for (File f : fs) { + sb.append("
  • "); + sb.append(f.getName()); + sb.append("
    • "); + } + sb.append("
"); + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK, + NanoHTTPD.MIME_HTML, sb.toString()); + }); + + manager.register("/logs/download", session -> { + final String[] pairs = session.getQueryParameterString().split("&"); + if (pairs.length != 1) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length); + } + + final String[] parts = pairs[0].split("="); + if (!parts[0].equals("file")) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]); + } + + File f = new File(ROOT, parts[1]); + if (!f.exists()) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND, + NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist"); + } + + return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK, + "application/json", new FileInputStream(f)); + }); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java new file mode 100644 index 000000000000..b7776cfcc5f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * Utility functions for log files. + */ +public class LoggingUtil { + public static final File ROAD_RUNNER_FOLDER = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/"); + + private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now + + private static void buildLogList(List logFiles, File dir) { + for (File file : dir.listFiles()) { + if (file.isDirectory()) { + buildLogList(logFiles, file); + } else { + logFiles.add(file); + } + } + } + + private static void pruneLogsIfNecessary() { + List logFiles = new ArrayList<>(); + buildLogList(logFiles, ROAD_RUNNER_FOLDER); + Collections.sort(logFiles, (lhs, rhs) -> + Long.compare(lhs.lastModified(), rhs.lastModified())); + + long dirSize = 0; + for (File file: logFiles) { + dirSize += file.length(); + } + + while (dirSize > LOG_QUOTA) { + if (logFiles.size() == 0) break; + File fileToRemove = logFiles.remove(0); + dirSize -= fileToRemove.length(); + //noinspection ResultOfMethodCallIgnored + fileToRemove.delete(); + } + } + + /** + * Obtain a log file with the provided name + */ + public static File getLogFile(String name) { + //noinspection ResultOfMethodCallIgnored + ROAD_RUNNER_FOLDER.mkdirs(); + + pruneLogsIfNecessary(); + + return new File(ROAD_RUNNER_FOLDER, name); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java new file mode 100644 index 000000000000..b9de338346ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java @@ -0,0 +1,124 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; + +import java.util.HashMap; +import java.util.Map; + +/** + * Collection of utilites for interacting with Lynx modules. + */ +public class LynxModuleUtil { + + private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2); + + /** + * Parsed representation of a Lynx module firmware version. + */ + public static class LynxFirmwareVersion implements Comparable { + public final int major; + public final int minor; + public final int eng; + + public LynxFirmwareVersion(int major, int minor, int eng) { + this.major = major; + this.minor = minor; + this.eng = eng; + } + + @Override + public boolean equals(Object other) { + if (other instanceof LynxFirmwareVersion) { + LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other; + return major == otherVersion.major && minor == otherVersion.minor && + eng == otherVersion.eng; + } else { + return false; + } + } + + @Override + public int compareTo(LynxFirmwareVersion other) { + int majorComp = Integer.compare(major, other.major); + if (majorComp == 0) { + int minorComp = Integer.compare(minor, other.minor); + if (minorComp == 0) { + return Integer.compare(eng, other.eng); + } else { + return minorComp; + } + } else { + return majorComp; + } + } + + @Override + public String toString() { + return Misc.formatInvariant("%d.%d.%d", major, minor, eng); + } + } + + /** + * Retrieve and parse Lynx module firmware version. + * @param module Lynx module + * @return parsed firmware version + */ + public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) { + String versionString = module.getNullableFirmwareVersionString(); + if (versionString == null) { + return null; + } + + String[] parts = versionString.split("[ :,]+"); + try { + // note: for now, we ignore the hardware entry + return new LynxFirmwareVersion( + Integer.parseInt(parts[3]), + Integer.parseInt(parts[5]), + Integer.parseInt(parts[7]) + ); + } catch (NumberFormatException e) { + return null; + } + } + + /** + * Exception indicating an outdated Lynx firmware version. + */ + public static class LynxFirmwareVersionException extends RuntimeException { + public LynxFirmwareVersionException(String detailMessage) { + super(detailMessage); + } + } + + /** + * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement. + * @param hardwareMap hardware map containing Lynx modules + */ + public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) { + Map outdatedModules = new HashMap<>(); + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + LynxFirmwareVersion version = getFirmwareVersion(module); + if (version == null || version.compareTo(MIN_VERSION) < 0) { + for (String name : hardwareMap.getNamesOf(module)) { + outdatedModules.put(name, version); + } + } + } + if (outdatedModules.size() > 0) { + StringBuilder msgBuilder = new StringBuilder(); + msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n"); + msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n", + MIN_VERSION.toString())); + for (Map.Entry entry : outdatedModules.entrySet()) { + msgBuilder.append(Misc.formatInvariant( + "\t%s: %s\n", entry.getKey(), + entry.getValue() == null ? "Unknown" : entry.getValue().toString())); + } + throw new LynxFirmwareVersionException(msgBuilder.toString()); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java new file mode 100644 index 000000000000..ca227dfda7d7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java @@ -0,0 +1,156 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; + +import androidx.annotation.Nullable; + +import com.acmerobotics.roadrunner.kinematics.Kinematics; + +import org.apache.commons.math3.stat.regression.SimpleRegression; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.List; + +/** + * Various regression utilities. + */ +public class RegressionUtil { + + /** + * Feedforward parameter estimates from the ramp regression and additional summary statistics + */ + public static class RampResult { + public final double kV, kStatic, rSquare; + + public RampResult(double kV, double kStatic, double rSquare) { + this.kV = kV; + this.kStatic = kStatic; + this.rSquare = rSquare; + } + } + + /** + * Feedforward parameter estimates from the ramp regression and additional summary statistics + */ + public static class AccelResult { + public final double kA, rSquare; + + public AccelResult(double kA, double rSquare) { + this.kA = kA; + this.rSquare = rSquare; + } + } + + /** + * Numerically compute dy/dx from the given x and y values. The returned list is padded to match + * the length of the original sequences. + * + * @param x x-values + * @param y y-values + * @return derivative values + */ + private static List numericalDerivative(List x, List y) { + List deriv = new ArrayList<>(x.size()); + for (int i = 1; i < x.size() - 1; i++) { + deriv.add( + (y.get(i + 1) - y.get(i - 1)) / + (x.get(i + 1) - x.get(i - 1)) + ); + } + // copy endpoints to pad output + deriv.add(0, deriv.get(0)); + deriv.add(deriv.get(deriv.size() - 1)); + return deriv; + } + + /** + * Run regression to compute velocity and static feedforward from ramp test data. + * + * Here's the general procedure for gathering the requisite data: + * 1. Slowly ramp the motor power/voltage and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope + * (kV) and an optional intercept (kStatic). + * + * @param timeSamples time samples + * @param positionSamples position samples + * @param powerSamples power samples + * @param fitStatic fit kStatic + * @param file log file + */ + public static RampResult fitRampData(List timeSamples, List positionSamples, + List powerSamples, boolean fitStatic, + @Nullable File file) { + if (file != null) { + try (PrintWriter pw = new PrintWriter(file)) { + pw.println("time,position,power"); + for (int i = 0; i < timeSamples.size(); i++) { + double time = timeSamples.get(i); + double pos = positionSamples.get(i); + double power = powerSamples.get(i); + pw.println(time + "," + pos + "," + power); + } + } catch (FileNotFoundException e) { + // ignore + } + } + + List velSamples = numericalDerivative(timeSamples, positionSamples); + + SimpleRegression rampReg = new SimpleRegression(fitStatic); + for (int i = 0; i < timeSamples.size(); i++) { + double vel = velSamples.get(i); + double power = powerSamples.get(i); + + rampReg.addData(vel, power); + } + + return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()), + rampReg.getRSquare()); + } + + /** + * Run regression to compute acceleration feedforward. + * + * @param timeSamples time samples + * @param positionSamples position samples + * @param powerSamples power samples + * @param rampResult ramp result + * @param file log file + */ + public static AccelResult fitAccelData(List timeSamples, List positionSamples, + List powerSamples, RampResult rampResult, + @Nullable File file) { + if (file != null) { + try (PrintWriter pw = new PrintWriter(file)) { + pw.println("time,position,power"); + for (int i = 0; i < timeSamples.size(); i++) { + double time = timeSamples.get(i); + double pos = positionSamples.get(i); + double power = powerSamples.get(i); + pw.println(time + "," + pos + "," + power); + } + } catch (FileNotFoundException e) { + // ignore + } + } + + List velSamples = numericalDerivative(timeSamples, positionSamples); + List accelSamples = numericalDerivative(timeSamples, velSamples); + + SimpleRegression accelReg = new SimpleRegression(false); + for (int i = 0; i < timeSamples.size(); i++) { + double vel = velSamples.get(i); + double accel = accelSamples.get(i); + double power = powerSamples.get(i); + + double powerFromVel = Kinematics.calculateMotorFeedforward( + vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic); + double powerFromAccel = power - powerFromVel; + + accelReg.addData(accel, powerFromAccel); + } + + return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java deleted file mode 100644 index 47ec4f901bdd..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ /dev/null @@ -1,110 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import android.util.ArrayMap; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.task.Task; - -import java.util.List; -import java.util.Map; -import java.util.stream.Collectors; - -public abstract class TaskOpMode extends LinearOpMode { - - protected Map tasks; - protected Map taskLinks; - - public TaskOpMode() { - tasks = new ArrayMap<>(); - taskLinks = new ArrayMap<>(); - } - - public abstract void linearInit(); - - public void linearStart() { - } - - public abstract void linearLoop(); - - public void linearStop() { - } - - @Override - public void runOpMode() { - linearInit(); - waitForStart(); - linearStart(); - while (opModeIsActive()) { - for (Map.Entry taskEntry : tasks.entrySet()) { - int taskId = taskEntry.getKey(); - Task task = taskEntry.getValue(); - if (!task.hasNext()) { - task.finish(); - tasks.remove(taskId, task); - taskLinks.remove(taskId); - taskLinks.entrySet().stream() - .filter(e -> e.getValue() == taskId) - .forEach(e -> { - Task nextTask = tasks.get(e.getKey()); - if (nextTask != null) nextTask.init(); - }); - continue; - } - Integer linkedTaskId = taskLinks.get(taskId); - if (tasks.containsKey(linkedTaskId)) continue; - task.iterate(); - } - linearLoop(); - sleep(Task.TICK_MS); - } - linearStop(); - } - - public int registerTask(Task task) { - task.init(); - return registerTask(task, -1); - } - - /** - * Register a task in the task map. All the tasks in the map will be iterated in - * the main loop when it's the right time to do. - * - * @param task the task that need registering. - * @param linkedTaskId the taskId of the linked task. The task being registered will - * be run after the linked task is finished. If nothing provided, - * the registered task will start to run immediately. - * @return the taskId of the registered task. - */ - public int registerTask(Task task, int linkedTaskId) { - int taskId = findMinFreeTaskId(); - tasks.put(taskId, task); - taskLinks.put(taskId, linkedTaskId); - return taskId; - } - - /** - * Cancel the task and all the linked-after tasks if necessary. - * - * @param taskId the taskId of the task needs canceling. - */ - public void cancelTask(int taskId) { - Task task = tasks.get(taskId); - if (task == null) return; - task.cancel(); - tasks.remove(taskId, task); - taskLinks.remove(taskId); - taskLinks.entrySet().stream() - .filter(e -> e.getValue() == taskId) - .forEach(e -> cancelTask(e.getKey())); - } - - protected int findMinFreeTaskId() { - List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); - int maxTaskId = taskIdList.get(taskIdList.size() - 1); - for (int i = 0; i <= maxTaskId; i++) { - if (taskIdList.contains(i)) return 1; - } - return maxTaskId + 1; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java new file mode 100644 index 000000000000..b7726e8508b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; + +@Disabled +// The ManualOp currently used is a piece of shit. +// I'll write a better one here (if the time is enough). +public class AdvancedManual extends LinearOpMode { + @Override + public void runOpMode(){ + TaskManager servoMgr = new TaskManager(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + //code + + servoMgr.updateServos(); + previousGamepad1.copy(gamepad1); + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java new file mode 100644 index 000000000000..2941740faa0f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +public class ArmStateMachine { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + RobotTop robotTop; + ArmState armState; + Gamepad gamepad1; + Gamepad previousGamepad1; + TaskManager taskMgr; + + final double STRETCH_BACK_POSITION = 0.03; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.22; + final double SPIN_X_HOVERING_POSITION = 0.53; + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_HOVERING_POSITION = 0.64; + final double TURN_OUT_DOWN_POSITION = 0.71; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.9; + + double armStretchPos; + double armTurnPos; + double armSpinXPos; + double armSpinYPos; + public ArmStateMachine(RobotTop robotTop){ + this.robotTop = robotTop; + this.armState = ArmState.IDLE; + this.taskMgr = new TaskManager(); + } + protected void init(){ + int liftPosition; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; + boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoState = false; + boolean containerRelease = false; + double recognitionAngle = 0; + } + public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1){ + this.gamepad1 = gamepad1; + this.previousGamepad1 = previousGamepad1; + } + + public void update(){ + switch (armState){ + case IDLE: + handleIdleState(); + case TURNING_OUT: + handleTurningOutState(); + case TURNED: + handleTurnedState(); + case TURNING_BACK: + handleTurningBackState(); + case WITHDRAWING: + handleWithdrawingState(); + } + } + protected void handleIdleState(){ + if(gamepad1.x && !previousGamepad1.x){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setTopServoPosition(0); + } + } + protected void handleTurningOutState(){} + protected void handleTurnedState(){} + protected void handleTurningBackState(){} + protected void handleWithdrawingState(){} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java index 6776ae6a1b33..f72d16f4cf59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.task; +package org.firstinspires.ftc.teamcode.advancedManual; public interface Task { int TICK_MS = 50; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java new file mode 100644 index 000000000000..30566ecd3e6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java @@ -0,0 +1,92 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + + +import com.qualcomm.robotcore.hardware.Servo; + +import java.util.HashMap; +import java.util.HashSet; +import java.util.Set; + +public class TaskManager { + protected int currentMaxId = 0; + protected HashMap tasks; + protected HashMap taskState; + + public TaskManager() { + tasks = new HashMap<>(); + taskState = new HashMap<>(); + } + + public void updateServos(){ + Set tasksToRemove = new HashSet<>(); + for (HashMap.Entry taskEntry : tasks.entrySet()) { + int taskId = taskEntry.getKey(); + Task task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + taskState.replace(taskId, false); + tasksToRemove.add(taskId); + continue; + } + task.iterate(); + } + for (int taskId : tasksToRemove) { + tasks.remove(taskId); + } + } + + public int setTimedServoPosition(Servo servo, double position, long timeMs) { + final double deltaPos = position - servo.getPosition(); + final long iterationCount = timeMs / Task.TICK_MS; + final double positionPerIteration = deltaPos / iterationCount; + Task task = new Task() { + long remainingIterations = iterationCount; + final double targetPosition = position; + + @Override + public void iterate() { + servo.setPosition(servo.getPosition() + positionPerIteration); + remainingIterations--; + } + + @Override + public void finish() { + servo.setPosition(targetPosition); + } + + @Override + public boolean hasNext() { + return remainingIterations > 0; + } + }; + return registerTask(task); + } + public int await(long timeMs){ + Task task = new Task() { + long iteration = timeMs / 50; + @Override + public void iterate() { + iteration--; + } + + @Override + public boolean hasNext() { + return iteration > 0; + } + }; + return registerTask(task); + } + public boolean getTaskState(int taskId){ + return Boolean.TRUE.equals(taskState.get(taskId)); + } + private int registerTask(Task task){ + int taskId = findMinFreeTaskId(); + tasks.put(taskId, task); + taskState.put(taskId, true); + return taskId; + } + private int findMinFreeTaskId() { + currentMaxId += 1; + return currentMaxId; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java new file mode 100644 index 000000000000..8ace175db1c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.advancedManual.TaskManager; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +public class RobotStateMachine { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + enum LiftState { + BOTTOM, GOING_UP, TOP, GOING_DOWN + } + RobotTop robotTop; + ArmState armState; + LiftState liftState; + Gamepad gamepad1; + Gamepad previousGamepad1; + TaskManager taskMgr; + + final double STRETCH_BACK_POSITION = 0.03; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.22; + final double SPIN_X_HOVERING_POSITION = 0.53; + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_HOVERING_POSITION = 0.64; + final double TURN_OUT_DOWN_POSITION = 0.71; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.9; + final double CONTAINER_OPEN_POSITION = 0.35; + final double CONTAINER_CLOSE_POSITION = 1; + final double LIFT_SERVO_CLOSE = 0.6; + final double LIFT_SERVO_OPEN = 0.2; + + double armStretchPos; + double armTurnPos; + double armSpinXPos; + double armSpinYPos; + public RobotStateMachine(RobotTop robotTop){ + this.robotTop = robotTop; + this.armState = ArmState.IDLE; + this.liftState = LiftState.BOTTOM; + this.taskMgr = new TaskManager(); + } + protected void init(){ + int liftPosition; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; + boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoState = false; + boolean containerRelease = false; + double recognitionAngle = 0; + } + + private void initializeRobotPositions() { + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmTurnPosition(armTurnPos); + robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); + robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); + } + + public void update(){ + switch (armState){ + case IDLE: + handleArmIdle(); + case TURNING_OUT: + handleArmTurningOut(); + case TURNED: + handleArmTurned(); + case TURNING_BACK: + handleArmTurningBack(); + case WITHDRAWING: + handleWithdrawingState(); + } + } + protected void handleArmIdle(){ + if(gamepad1.x && !previousGamepad1.x){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setTopServoPosition(0); + } + } + protected void handleArmTurningOut(){} + protected void handleArmTurned(){} + protected void handleArmTurningBack(){} + protected void handleWithdrawingState(){} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java new file mode 100644 index 000000000000..7a4333668fad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import android.util.ArrayMap; + +import org.firstinspires.ftc.teamcode.advancedManual.Task; + +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; + +class ServoStimulator{ + double position = 0; + + public void setPosition(double position) { + this.position = position; + System.out.println(this.position); + } + + public double getPosition() { + return position; + } +} + +class ServoMgrTest { + protected Map tasks; + protected Map taskState; + + public ServoMgrTest() { + tasks = new ArrayMap<>(); + } + + public void updateServos(){ + for (Map.Entry taskEntry : tasks.entrySet()) { + int taskId = taskEntry.getKey(); + Task task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + taskState.replace(taskId, false); + tasks.remove(taskId, task); + continue; + } + task.iterate(); + } + } + + private Task timedSetPosition(ServoStimulator servo, double position, long timeMs) { + final double deltaPos = position - servo.getPosition(); + final long iterationCount = timeMs / Task.TICK_MS; + final double positionPerIteration = deltaPos / iterationCount; + return new Task() { + long remainingIterations = iterationCount; + final double targetPosition = position; + + @Override + public void iterate() { + servo.setPosition(servo.getPosition() + positionPerIteration); + remainingIterations--; + } + + @Override + public void finish() { + servo.setPosition(targetPosition); + } + + @Override + public boolean hasNext() { + return remainingIterations > 0; + } + }; + } + public int setTimedServoPosition(ServoStimulator servo, double position, long timeMs){ + Task task = timedSetPosition(servo, position, timeMs); + int taskId = findMinFreeTaskId(); + tasks.put(taskId, task); + return taskId; + } + private int findMinFreeTaskId() { + List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); + int maxTaskId = taskIdList.get(taskIdList.size() - 1); + for (int i = 0; i <= maxTaskId; i++) { + if (taskIdList.contains(i)) return 1; + } + return maxTaskId + 1; + } +} + +class Main{ + public static void main(String[] args) { + ServoMgrTest servoMgr = new ServoMgrTest(); + ServoStimulator servo1 = new ServoStimulator(); + ServoStimulator servo2 = new ServoStimulator(); + ServoStimulator servo3 = new ServoStimulator(); + servoMgr.setTimedServoPosition(servo1, 0.7, 1000); + servoMgr.setTimedServoPosition(servo2, 0.2, 5000); + servoMgr.setTimedServoPosition(servo3, 1, 10000); + while (true){ + servoMgr.updateServos(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java new file mode 100644 index 000000000000..24d386da3c73 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.advancedManual.TaskManager; +@Disabled +// The ManualOp currently used is a piece of shit. +// I'll write a better one here (if the time is enough). +public class TimedServoTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo servo = hardwareMap.get(Servo.class, "servo"); + TaskManager servoMgr = new TaskManager(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + if(gamepad1.a && !previousGamepad1.a){ + servoMgr.setTimedServoPosition(servo, 1,2000); + } + if(gamepad1.b && !previousGamepad1.b){ + servoMgr.setTimedServoPosition(servo, 0,2000); + } + + servoMgr.updateServos(); + previousGamepad1.copy(gamepad1); + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java new file mode 100644 index 000000000000..5757ad6f73d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -0,0 +1,368 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.util.Range; + +@SuppressWarnings(value = "unused") +public class RobotAuto { + LinearOpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + RobotChassis robotChassis; + RobotTop robotTop; + private double headingError = 0; + @SuppressWarnings("FieldCanBeLocal") + private double targetHeading = 0; + @SuppressWarnings("FieldMayBeFinal") + private double driveSpeed = 0; + private double turnSpeed = 0; + + IMU imu; + SparkFunOTOS otos; + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.LEFT; + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + public RobotAuto(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.robotChassis = new RobotChassis(opMode); + this.robotTop = new RobotTop(opMode); + otos = opMode.hardwareMap.get(SparkFunOTOS.class, "otos"); + configureOtos(); + imu = opMode.hardwareMap.get(IMU.class, "imu"); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + imu.resetYaw(); + } + + static final double COUNTS_PER_MOTOR_REV = 560; + + // gearing UP (will affect the direction of wheel rotation) < 1 < gearing DOWN + // eg. for a 12-tooth driving a 24-tooth, the value is 24/12=2.0 + static final double DRIVE_GEAR_REDUCTION = 1.0; + + // wheel diameter in inches + static final double WHEEL_DIAMETER_INCHES = 5.31; + + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + + static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable + static final double P_STRAFE_GAIN = 0.03; // Strafe Speed Control "Gain". + static final double P_TURN_GAIN = 0.1; // Larger is more responsive, but also less stable + static final double HEADING_THRESHOLD = 0.5; + + public double getSteeringCorrection(double desiredHeading, double proportionalGain) { + + // Determine the heading current error + headingError = desiredHeading - getHeading(); + + // Normalize the error to be within +/- 180 degrees + while (headingError > 180) headingError -= 360; + while (headingError <= -180) headingError += 360; + + // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 + return Range.clip(headingError * proportionalGain, -1, 1); + } + /** + * Read the robot heading directly from the IMU. + * + * @return The heading of the robot in degrees. + */ + public double getHeading() { + return getHeading(AngleUnit.DEGREES); + } + /** + * read the Robot heading directly from the IMU + * + * @param unit The desired angle unit (degrees or radians) + * @return The heading of the robot in desired units. + */ + public double getHeading(AngleUnit unit) { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + telemetry.addData("Yaw/Pitch/Roll", orientation.toString()); + 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)); + return orientation.getYaw(unit); + } + + public RobotAuto driveStraight(double maxDriveSpeed, + double distance, + double heading) { + + // Ensure that the OpMode is still active + if (opMode.opModeIsActive()) { + + // Determine new target position, and pass to motor controller + int moveCounts = (int) (distance * COUNTS_PER_INCH); + setStraightTargetPosition(moveCounts); + + robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + robotChassis.driveRobot(maxDriveSpeed, 0, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { + + // Determine required steering to keep on heading + double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + robotChassis.driveRobot(maxDriveSpeed, 0, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f, %4d",maxDriveSpeed,distance,heading,turnSpeed,moveCounts); + telemetry.update(); + } + + // Stop all motion & Turn off RUN_TO_POSITION + robotChassis.stopMotor(); + robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return this; + } + + public RobotAuto driveStrafe(double maxDriveSpeed, + double distance, + double heading + ) { + + // Ensure that the OpMode is still active + if (opMode.opModeIsActive()) { + + // Determine new target position, and pass to motor controller + int moveCounts = (int) (distance * COUNTS_PER_INCH); + setStrafeTargetPosition(moveCounts); + + robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + robotChassis.driveRobot(0, maxDriveSpeed, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { + + // Determine required steering to keep on heading + double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + robotChassis.driveRobot(0, maxDriveSpeed, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f, %4d",maxDriveSpeed,distance,heading,turnSpeed,moveCounts); + telemetry.update(); + } + + // Stop all motion & Turn off RUN_TO_POSITION + robotChassis.stopMotor(); + robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return this; + } + public void setStraightTargetPosition(int moveCounts) { + int[] motorPos = robotChassis.getTargetPosition(); + for (int i = 0; i < motorPos.length; i++) { + motorPos[i] += moveCounts; + } + robotChassis.setTargetPosition(motorPos); + } + public void setStrafeTargetPosition(int moveCounts) { + int[] motorPos = robotChassis.getTargetPosition(); + motorPos[0] = motorPos[0] + moveCounts; + motorPos[1] = motorPos[1] - moveCounts; + motorPos[2] = motorPos[2] - moveCounts; + motorPos[3] = motorPos[3] + moveCounts; + robotChassis.setTargetPosition(motorPos); + } + public RobotAuto turnToHeading(double maxTurnSpeed, double heading) { + + // Run getSteeringCorrection() once to pre-calculate the current error + getSteeringCorrection(heading, P_DRIVE_GAIN); + + // keep looping while we are still active, and not on heading. + //绝对值的问题？ + while (opMode.opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + robotChassis.driveRobot(0, 0, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f",maxTurnSpeed,turnSpeed,heading,getHeading()); + telemetry.update(); + } + + // Stop all motion; + robotChassis.stopMotor(); + return this; + } + + private void configureOtos() { + otos.setLinearUnit(DistanceUnit.INCH); + otos.setAngularUnit(AngleUnit.DEGREES); + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0); + otos.setOffset(offset); + + otos.setLinearScalar(1.0); + otos.setAngularScalar(1.0); + + otos.calibrateImu(); + + otos.resetTracking(); + + SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); + otos.setPosition(currentPosition); + + // Get the hardware and firmware version + SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); + SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); + otos.getVersionInfo(hwVersion, fwVersion); + + telemetry.addLine("OTOS configured! Press start to get position data!"); + telemetry.addLine(); + telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); + telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); + telemetry.update(); + } + + public SparkFunOTOS.Pose2D getPosition() { + return otos.getPosition(); + } + + public RobotAuto forward(double d) { + return driveStraight(0.6, -d, getHeading()); + } + + public RobotAuto fastForward(double d) { + return driveStraight(0.9, -d, getHeading()); + } + + + public RobotAuto backward(double d) { + return driveStraight(0.6, -d, getHeading()); + } + + public RobotAuto fastBackward(double d) { + return driveStraight(0.9, -d, getHeading()); + } + + public RobotAuto rightShift(double d) { + return driveStrafe(0.9, -d, getHeading()); + } + + public RobotAuto leftShift(double d) { + return driveStrafe(0.9, d, getHeading()); + } + + public RobotAuto spin(double h) { + return turnToHeading(0.6, h); + } + + public RobotAuto fastSpin(double h) { + return turnToHeading(0.9, h); + } + + public RobotAuto sleep(long milliseconds) { + opMode.sleep(milliseconds); + return this; + } + + protected double[] SpinVector(double[] vector, double angle) { + double x = vector[0] * Math.cos(Math.toRadians(angle)) - vector[1] * Math.sin(Math.toRadians(angle)); + double y = vector[0] * Math.sin(Math.toRadians(angle)) + vector[1] * Math.cos(Math.toRadians(angle)); + return new double[]{x, y}; + } + protected double[] getDisplacement(double[] CurrentPos, double[] DesiredPos) { + double CurrentX = CurrentPos[0]; + double CurrentY = CurrentPos[1]; + double CurrentHeading = CurrentPos[2]; + double DesiredX = DesiredPos[0]; + double DesiredY = DesiredPos[1]; + double DesiredHeading = DesiredPos[2]; + double[] Displacement = {DesiredX - CurrentX, DesiredY - CurrentY, CurrentHeading}; + Displacement = SpinVector(Displacement, -CurrentHeading); + return Displacement; + } + /** + * Go to the position given (track only include left/right and forward/backward) + * Go forward/backward first,then move left/right. + * + * @param CurrentPos The current position.(position{axial,lateral,heading}) + * @param DesiredPos The desired position.(position{axial,lateral,heading}) + * @return RobotHardware class. + */ + public RobotAuto gotoPosition(double[] CurrentPos, double[] DesiredPos) { + double[] Displacement = getDisplacement(CurrentPos, DesiredPos); + double DesiredHeading = DesiredPos[2]; + return fastForward(-Displacement[0]) + .leftShift(-Displacement[1]) + .fastSpin(DesiredHeading); + } + public RobotAuto gotoPosition(double x, double y, double h) { + SparkFunOTOS.Pose2D CurrentPos = getPosition(); + double[] DesiredPos = {x, y, h}; + return gotoPosition(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); + } + + /** + * Go to the position given (track only include left/right and forward/backward) + * Move left/right first,then go forward/backward. + * + * @param CurrentPos The current position.(position{axial,lateral,heading}) + * @param DesiredPos The desired position.(position{axial,lateral,heading}) + * @return RobotHardware class. + */ + public RobotAuto gotoPosition2(double[] CurrentPos, double[] DesiredPos) { + double[] Displacement = getDisplacement(CurrentPos, DesiredPos); + double DesiredHeading = DesiredPos[2]; + return leftShift(Displacement[1]) + .fastForward(Displacement[0]) + .fastSpin(DesiredHeading); + } + public RobotAuto gotoPosition2(double x, double y, double h) { + SparkFunOTOS.Pose2D CurrentPos = getPosition(); + double[] DesiredPos = {x, y, h}; + return gotoPosition2(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); + } + + public RobotAuto stretchArm(){ + robotTop.setArmStretchPosition(0.3); + return this; + } + public RobotAuto setLiftPower(double power){ + robotTop.setLeftPower(power); + return this; + } + public RobotAuto grab(){ + robotTop.setLiftServoPosition(0.6); + return this; + } + public RobotAuto release(){ + robotTop.setLiftServoPosition(0.2); + return this; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java new file mode 100644 index 000000000000..34892e780196 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -0,0 +1,150 @@ +//底盘代码 +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@SuppressWarnings(value = "unused") +public class RobotChassis { + LinearOpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + protected DcMotor leftFrontDrive; + protected DcMotor leftBackDrive; + protected DcMotor rightFrontDrive; + protected DcMotor rightBackDrive; + + public RobotChassis(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.initMovement(); + } + + public void initMovement() { + // init motors and servos + leftFrontDrive = hardwareMap.get(DcMotor.class, "FL"); + leftBackDrive = hardwareMap.get(DcMotor.class, "BL"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "FR"); + rightBackDrive = hardwareMap.get(DcMotor.class, "BR"); + + leftFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public boolean isAllBusy() { + return leftFrontDrive.isBusy() && + leftBackDrive.isBusy() && + rightFrontDrive.isBusy() && + rightBackDrive.isBusy(); + } + + /** + * Calculates the motor powers required to achieve the requested + * robot motions: Drive (Axial motion) and Turn (Yaw motion). + * Then sends these power levels to the motors. + * + * @param axial Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param lateral Right/Left driving power (-1.0 to 1.0) +ve is forward + * @param yaw Right/Left turning power (-1.0 to 1.0) +ve is clockwise + */ + public void driveRobot(double axial, double lateral, double yaw) { + lateral = -lateral * 1.1; // Counteract imperfect strafing + yaw = -yaw; + + final double LEFT_REDUCTION = 0.96; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(axial) + Math.abs(lateral) + Math.abs(yaw), 1); + double frontLeftPower = (axial + lateral + yaw) / denominator * LEFT_REDUCTION; + double backLeftPower = (axial - lateral + yaw) / denominator * LEFT_REDUCTION; + double frontRightPower = (axial - lateral - yaw) / denominator; + double backRightPower = (axial + lateral - yaw) / denominator; + + leftFrontDrive.setPower(frontLeftPower); + leftBackDrive.setPower(backLeftPower); + rightFrontDrive.setPower(frontRightPower); + rightBackDrive.setPower(backRightPower); + } + + /** + * Set the power of every motor. + * + * @param leftFrontPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightFrontPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param leftBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + */ + public void setDrivePower(double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + } + + /** + * Stop All drive motors. + */ + public void stopMotor() { + setDrivePower(0, 0, 0, 0); + } + + /** + * Set drive wheels run mode for all drive motors. + * + * @param mode The desired run mode. + */ + public void setRunMode(DcMotor.RunMode mode) { + leftFrontDrive.setMode(mode); + rightFrontDrive.setMode(mode); + leftBackDrive.setMode(mode); + rightBackDrive.setMode(mode); + } + + /** + * Set zero power behavior for all drive motors. + * + * @param behavior The desired zero power behaviour. + */ + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) { + leftFrontDrive.setZeroPowerBehavior(behavior); + rightFrontDrive.setZeroPowerBehavior(behavior); + leftBackDrive.setZeroPowerBehavior(behavior); + rightBackDrive.setZeroPowerBehavior(behavior); + } + + /** + * Set target position for all drive motors for moving forward. + * + * @param moveCounts The desired increment/decrement. + */ + public void setTargetPosition(int[] moveCounts) { + // Set Target FIRST, then turn on RUN_TO_POSITION + leftFrontDrive.setTargetPosition(moveCounts[0]); + rightFrontDrive.setTargetPosition(moveCounts[1]); + leftBackDrive.setTargetPosition(moveCounts[2]); + rightBackDrive.setTargetPosition(moveCounts[3]); + } + + public int[] getTargetPosition() { + // Set Target FIRST, then turn on RUN_TO_POSITION + int lf = leftFrontDrive.getTargetPosition(); + int rf = rightFrontDrive.getTargetPosition(); + int lb = leftBackDrive.getTargetPosition(); + int rb = rightBackDrive.getTargetPosition(); + return new int[]{lf, lb, lb, rb}; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java new file mode 100644 index 000000000000..480af2c51b82 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -0,0 +1,127 @@ +//底盘上部代码 +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@SuppressWarnings(value = "unused") +public class RobotTop { + OpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + protected DcMotor leftLiftMotor; + protected DcMotor rightLiftMotor; + protected Servo armStretchServo; + protected Servo armTurnServo; + protected Servo armSpinXServo; + protected Servo armSpinYServo; + protected Servo armGrabServo; + protected Servo liftServo; + protected Servo topServo; + protected Servo containerServo; + public RobotTop(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.init(); + } + + public void init() { + leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLift"); + rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); + armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + armTurnServo = hardwareMap.get(Servo.class, "armTurn"); + armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); + armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + armGrabServo = hardwareMap.get(Servo.class, "armGrab"); + liftServo = hardwareMap.get(Servo.class, "liftServo"); + topServo = hardwareMap.get(Servo.class, "liftTop"); + containerServo = hardwareMap.get(Servo.class, "containerServo"); + + leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); + rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); + + leftLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + leftLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void setLeftPower(double power) { + leftLiftMotor.setPower(power); + rightLiftMotor.setPower(power); + } + + public int getLiftPosition() { + return leftLiftMotor.getCurrentPosition(); + } + + public void setArmStretchPosition(double position) { + armStretchServo.setPosition(position); + } + + public double getArmStretchPosition() { + return armStretchServo.getPosition(); + } + + public void setArmTurnPosition(double position) { + armTurnServo.setPosition(position); + } + + public double getArmTurnPosition() { + return armTurnServo.getPosition(); + } + + public void setArmSpinXPosition(double position) { + armSpinXServo.setPosition(position); + } + + public double getArmSpinXPosition() { + return armSpinXServo.getPosition(); + } + + public void setArmSpinYPosition(double position) { + armSpinYServo.setPosition(position); + } + + public double getArmSpinYPosition() { + return armSpinYServo.getPosition(); + } + + public void setArmGrabPosition(double position) { + armGrabServo.setPosition(position); + } + + public double getArmGrabPosition() { + return armGrabServo.getPosition(); + } + public void setLiftServoPosition(double position) { + liftServo.setPosition(position); + } + + public double getLiftServoPosition() { + return liftServo.getPosition(); + } + public void setTopServoPosition(double position) { + topServo.setPosition(position); + } + + public double getTopServoPosition() { + return topServo.getPosition(); + } + public void setContainerServoPosition(double position) { + containerServo.setPosition(position); + } + + public double getContainerServoPosition() { + return containerServo.getPosition(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java new file mode 100644 index 000000000000..eced74138094 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java @@ -0,0 +1,143 @@ +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.RotatedRect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +import java.util.ArrayList; +import java.util.List; + +public class RobotCameraDistance { + + private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); + private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); + private static final Scalar LOWER_BOUND_YELLOW = new Scalar(20, 100, 100); + private static final Scalar UPPER_BOUND_YELLOW = new Scalar(30, 255, 255); + private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); + private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); + + private OpenCvCamera webcam; + private String detectedColor; + private double[] polarCoordinates = new double[2]; + private Mat hsv = new Mat(); + private Mat mask = new Mat(); + private Mat hierarchy = new Mat(); + private Scalar lowerBound; + private Scalar upperBound; + private RotatedRect currentRect; + + public void initialize(HardwareMap hardwareMap, String color) { + this.detectedColor = color; + setColorBounds(color); + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new ColorDetectionPipeline()); + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + } + }); + } + + public double[] getPolarCoordinates() { + return polarCoordinates; + } + + private void setColorBounds(String color) { + switch (color.toLowerCase()) { + case "red": + lowerBound = LOWER_BOUND_RED; + upperBound = UPPER_BOUND_RED; + break; + case "yellow": + lowerBound = LOWER_BOUND_YELLOW; + upperBound = UPPER_BOUND_YELLOW; + break; + case "blue": + lowerBound = LOWER_BOUND_BLUE; + upperBound = UPPER_BOUND_BLUE; + break; + default: + throw new IllegalArgumentException("Unsupported color: " + color); + } + } + + public void updateCurrentRectExcludingCurrent() { + List contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + RotatedRect largestRotatedRect = null; + double maxArea = 0; + for (MatOfPoint contour : contours) { + MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); + RotatedRect rotatedRect = Imgproc.minAreaRect(contour2f); + double area = rotatedRect.size.area(); + if (area > maxArea && (currentRect == null || !isSameRect(rotatedRect, currentRect))) { + maxArea = area; + largestRotatedRect = rotatedRect; + } + } + + if (largestRotatedRect != null) { + currentRect = largestRotatedRect; + } + } + + private class ColorDetectionPipeline extends OpenCvPipeline { + @Override + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); + Core.inRange(hsv, lowerBound, upperBound, mask); + + updateCurrentRectExcludingCurrent(); + + if (currentRect != null) { + Point rectCenter = currentRect.center; + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + if (angle < 0) { + angle += 360; + } + polarCoordinates[0] = distance; + polarCoordinates[1] = angle; + + Point[] vertices = new Point[4]; + currentRect.points(vertices); + for (int i = 0; i < 4; i++) { + Imgproc.line(input, vertices[i], vertices[(i + 1) % 4], new Scalar(0, 255, 0), 2); + } + + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + } + return input; + } + } + + private boolean isSameRect(RotatedRect rect1, RotatedRect rect2) { + double distanceThreshold = 10.0; + double areaThreshold = 0.1; + double centerDistance = Math.sqrt(Math.pow(rect1.center.x - rect2.center.x, 2) + Math.pow(rect1.center.y - rect2.center.y, 2)); + double areaDifference = Math.abs(rect1.size.area() - rect2.size.area()) / rect1.size.area(); + return centerDistance < distanceThreshold && areaDifference < areaThreshold; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java new file mode 100644 index 000000000000..fc51c352b946 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Size; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvPipeline; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class RobotVisionAngle { + private OpenCvCamera webcam; + private double detectedAngle = 0; + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterAnglePipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + } + }); + } + + public double getDetectedAngle() { + return detectedAngle; + } + + private class CenterAnglePipeline extends OpenCvPipeline { + Mat gray = new Mat(); + Mat blurred = new Mat(); + Mat edges = new Mat(); + Mat lines = new Mat(); + Mat sharp = new Mat(); + Mat kernel = new Mat(3, 3, CvType.CV_16SC1); + + @Override + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); + + // 应用高斯模糊减少噪声 + Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + + // 增强图像锐度 + kernel.put(0, 0, 0, -1, 0); + kernel.put(1, 0, -1, 5, -1); + kernel.put(2, 0, 0, -1, 0); + Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); + + // 优化亮度和对比度 + Mat brightened = new Mat(); + sharp.convertTo(brightened, -1, 1.2, 10); + + Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); + Mat centerMat = brightened.submat(centerRect); + Imgproc.Canny(centerMat, edges, 50, 150); + Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); + + double angle = 0; + if (lines.rows() > 0) { + for (int i = 0; i < lines.rows(); i++) { + double[] line = lines.get(i, 0); + double dx = line[2] - line[0]; + double dy = line[3] - line[1]; + angle = Math.toDegrees(Math.atan2(dy, dx)); + break; + } + } + + detectedAngle = Math.round(angle / 10.0) * 10; + + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + Point rectCenter = new Point(centerRect.x + centerRect.width / 2.0, centerRect.y + centerRect.height / 2.0); + + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double rectAngle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + if (rectAngle > 180) { + rectAngle -= 360; + } + + double[] polarCoordinates = new double[2]; + polarCoordinates[0] = distance; + polarCoordinates[1] = rectAngle; + + Imgproc.rectangle(input, centerRect, new Scalar(0, 255, 0), 2); + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + rectAngle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + + return input; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java new file mode 100644 index 000000000000..e5341c15b09f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java @@ -0,0 +1,90 @@ +//这是第二版视觉识别,可以识别摄像头中心的颜色 +//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是四个类型Unknown,Red，Blue，Yellow + + +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class RobotVisionColor { + private OpenCvCamera webcam; + private String detectedColor = "Unknown"; + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory + .getInstance() + .createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterColorPipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public String getDetectedColor() { + return detectedColor; + } + + private class CenterColorPipeline extends OpenCvPipeline { + Mat hsv = new Mat(); + Mat maskRed = new Mat(); + Mat maskBlue = new Mat(); + Mat maskYellow = new Mat(); // 增加黄色检测的mask + Scalar lowerRed = new Scalar(0, 100, 100); + Scalar upperRed = new Scalar(10, 255, 255); + Scalar lowerBlue = new Scalar(110, 100, 100); + Scalar upperBlue = new Scalar(130, 255, 255); + Scalar lowerYellow = new Scalar(20, 100, 100); // 黄色的HSV范围 + Scalar upperYellow = new Scalar(30, 255, 255); + + @Override + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); + + // 获取图像中心区域 + Rect centerRect = new Rect(input.width() / 2 - 20, input.height() / 2 - 20, 40, 40); + Mat centerMat = hsv.submat(centerRect); + + // 检测红色、蓝色和黄色 + Core.inRange(centerMat, lowerRed, upperRed, maskRed); + Core.inRange(centerMat, lowerBlue, upperBlue, maskBlue); + Core.inRange(centerMat, lowerYellow, upperYellow, maskYellow); + + double redValue = Core.mean(maskRed).val[0]; + double blueValue = Core.mean(maskBlue).val[0]; + double yellowValue = Core.mean(maskYellow).val[0]; + + if (redValue > blueValue && redValue > yellowValue) { + detectedColor = "Red"; + } else if (blueValue > redValue && blueValue > yellowValue) { + detectedColor = "Blue"; + } else if (yellowValue > redValue && yellowValue > blueValue) { + detectedColor = "Yellow"; + } else { + detectedColor = "Unknown"; + } + + return input; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java new file mode 100644 index 000000000000..0f2f59952d1d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class SparkFunOTOSSensor { + private SparkFunOTOS otosDevice; + private double x; + private double y; + private double heading; + + public SparkFunOTOSSensor(HardwareMap hardwareMap) { + // 假设 OTOS 传感器连接到 I2C 端口 + otosDevice = hardwareMap.get(SparkFunOTOS.class, "otos"); + // 初始化传感器（具体初始化代码取决于 OTOS 库的实现） + this.x = 0.0; + this.y = 0.0; + this.heading = 0.0; + } + + public double getX() { + // 返回 X 坐标 + return x; // 这里应从传感器获取实际数据 + } + + public double getY() { + // 返回 Y 坐标 + return y; // 这里应从传感器获取实际数据 + } + + public double getHeading() { + // 返回方向（角度） + return heading; // 这里应从传感器获取实际数据 + } + + public void updateData(double newX, double newY, double newHeading) { + // 更新传感器数据 + this.x = newX; + this.y = newY; + this.heading = newHeading; + } + + public double getDistance() { + + return 0; + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java deleted file mode 100644 index f5f6a641ecc7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java +++ /dev/null @@ -1,126 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoController; - -import org.firstinspires.ftc.teamcode.task.Task; - -public class TimedServo { - protected Servo servo; - - public TimedServo(Servo servo) { - this.servo = servo; - } - - /** - * Sets the logical direction in which this servo operates. - * - * @param direction the direction to set for this servo - * @see #getDirection() - * @see Servo.Direction - */ - public void setDirection(Servo.Direction direction) { - servo.setDirection(direction); - } - - /** - * Returns the current logical direction in which this servo is set as operating. - * - * @return the current logical direction in which this servo is set as operating. - * @see #setDirection(Servo.Direction) - */ - public Servo.Direction getDirection() { - return servo.getDirection(); - } - - /** - * Sets the current position of the servo, expressed as a fraction of its available - * range. If PWM power is enabled for the servo, the servo will attempt to move to - * the indicated position. - * - * @param position the position to which the servo should move, a value in the range [0.0, 1.0] - * @see ServoController#pwmEnable() - * @see #getPosition() - */ - public void setPosition(double position) { - servo.setPosition(position); - } - - /** - * Returns the position to which the servo was last commanded to move. Note that this method - * does NOT read a position from the servo through any electrical means, as no such electrical - * mechanism is, generally, available. - * - * @return the position to which the servo was last commanded to move, or Double.NaN - * if no such position is known - * @see #setPosition(double) - * @see Double#NaN - * @see Double#isNaN() - */ - public double getPosition() { - return servo.getPosition(); - } - - /** - * Scales the available movement range of the servo to be a subset of its maximum range. Subsequent - * positioning calls will operate within that subset range. This is useful if your servo has - * only a limited useful range of movement due to the physical hardware that it is manipulating - * (as is often the case) but you don't want to have to manually scale and adjust the input - * to {@link #setPosition(double) setPosition()} each time. - * - *

For example, if scaleRange(0.2, 0.8) is set; then servo positions will be - * scaled to fit in that range:
- * setPosition(0.0) scales to 0.2
- * setPosition(1.0) scales to 0.8
- * setPosition(0.5) scales to 0.5
- * setPosition(0.25) scales to 0.35
- * setPosition(0.75) scales to 0.65
- *

- * - *

Note the parameters passed here are relative to the underlying full range of motion of - * the servo, not its currently scaled range, if any. Thus, scaleRange(0.0, 1.0) will reset - * the servo to its full range of movement.

- * - * @param min the lower limit of the servo movement range, a value in the interval [0.0, 1.0] - * @param max the upper limit of the servo movement range, a value in the interval [0.0, 1.0] - * @see #setPosition(double) - */ - public void scaleRange(double min, double max) { - servo.scaleRange(min, max); - } - - /** - * Sets the current position of the servo in a fixed time, expressed as a fraction of - * its available range. If PWM power is enabled for the servo, the servo will attempt - * to move to the indicated position. - * - * @param position the position to which the servo should move, a value in the range [0.0, 1.0] - * @param timeMs the time that the servo will take to move. - * @return a Task object - */ - public Task setPosition(double position, long timeMs) { - final double delta = position - servo.getPosition(); - final long iterationCount = timeMs / Task.TICK_MS; - final double perDelta = delta / iterationCount; - return new Task() { - long remainingIterations = iterationCount; - final double targetPosition = position; - - @Override - public void iterate() { - servo.setPosition(servo.getPosition() + perDelta); - remainingIterations--; - } - - @Override - public void finish() { - servo.setPosition(targetPosition); - } - - @Override - public boolean hasNext() { - return remainingIterations > 0; - } - }; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 674e53b7b517..844434ffd0e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -3,7 +3,7 @@ Welcome! This module, TeamCode, is the place where you will write/paste the code for your team's -robot controller App. This module is currently empty (a clean slate) but the +robot controller App. This module is currently empty (a clean slate板) but the process for adding OpModes is straightforward. ## Creating your own OpModes @@ -19,7 +19,7 @@ Expand the following tree elements: ### Naming of Samples To gain a better understanding of how the samples are organized, and how to interpret the -naming system, it will help to understand the conventions that were used during their creation. +naming system, it will help to understand the conventions公约 that were used during their creation. These conventions are described (in detail) in the sample_conventions.md file in this folder. @@ -29,20 +29,29 @@ The prefix of the name will be one of the following: Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure of a particular style of OpMode. These are bare bones examples. + 这是一个功能最少的 OpMode，用于说明特定 OpMode 样式的骨架/结构。这些是基本示例。 Sensor: This is a Sample OpMode that shows how to use a specific sensor. It is not intended to drive a functioning robot, it is simply showing the minimal code required to read and display the sensor values. + 这是一个示例 OpMode，展示了如何使用特定的传感器。 + 它并非用于驱动功能齐全的机器人，而只是展示读取和显示传感器值所需的最少代码。 Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. It may be used to provide a common baseline driving OpMode, or to demonstrate how a particular sensor or concept can be used to navigate. + 这是一个示例 OpMode，假设一个简单的双电机（差速）驱动基础。 + 它可用于提供通用基线驱动 OpMode，或演示如何使用特定传感器或概念进行导航。 Concept: This is a sample OpMode that illustrates performing a specific function or concept. These may be complex, but their operation should be explained clearly in the comments, or the comments should reference an external doc, guide or tutorial. Each OpMode should try to only demonstrate a single concept so they are easy to locate based on their name. These OpModes may not produce a drivable robot. + 这是演示如何执行特定功能或概念的示例 OpMode。 + 这些可能很复杂，但应在注释中清楚地解释其操作， + 或者注释应引用外部文档、指南或教程。 + 每个 OpMode 都应尝试仅演示一个概念，以便根据其名称轻松找到它们。这些 OpMode 可能无法生成可驾驶的机器人。 After the prefix, other conventions will apply: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java deleted file mode 100644 index 8a1fd39bb8da..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.firstinspires.ftc.teamcode.task; - -public interface OneTimeTask extends Task { - - void init(); - - @Override - default void iterate() {} - - @Override - default boolean hasNext() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java deleted file mode 100644 index 87ed2726201d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode.task; - -public class SleepTask implements Task { - protected int remainingSleepTicks; - - public SleepTask(int sleepTicks) { - remainingSleepTicks = sleepTicks; - } - - @Override - public void iterate() { - remainingSleepTicks--; - } - - @Override - public boolean hasNext() { - return remainingSleepTicks > 0; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java new file mode 100644 index 000000000000..e4b49a376e20 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +@Disabled +@Autonomous(group = "Test") +public class AUTOOpModeTest extends LinearOpMode +{ + + DcMotor motor; + + @Override + public void runOpMode() throws InterruptedException + { + + motor = hardwareMap.dcMotor.get("LeftRightmotor"); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + waitForStart(); + motor.setPower(1); + motor.setTargetPosition(1000); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java new file mode 100644 index 000000000000..d02139b6208d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.RobotAuto; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; + +@Autonomous +public class AutoTest extends LinearOpMode { + RobotAuto robotAuto; + RobotChassis robotChassis; + + @Override + public void runOpMode() { + robotChassis = new RobotChassis(this); + robotAuto = new RobotAuto(this); + waitForStart(); + resetRuntime(); + while (getRuntime() <= 4){ + robotChassis.driveRobot(0,-0.5,0); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java new file mode 100644 index 000000000000..3ad5f2cc462f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.test; + +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.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@Disabled +@TeleOp(group = "Test") +public class LeftTest extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor leftLiftMotor = hardwareMap.get(DcMotor.class , "leftLift"); + DcMotor rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); + + leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); + rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); + + leftLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + leftLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + waitForStart(); + + if (isStopRequested()) return; + double liftSpeed = 0; + double pos = 0; + + while (opModeIsActive()) { + pos = leftLiftMotor.getCurrentPosition(); + boolean y = gamepad1.y; + boolean a = gamepad1.a; + if(y && pos <= 1200){ + liftSpeed =0.5; + } else if (a && pos >= 100) { + liftSpeed = -0.5; + }else{liftSpeed = 0;} + leftLiftMotor.setPower(liftSpeed); + rightLiftMotor.setPower(liftSpeed); + telemetry.addData("pos",leftLiftMotor.getCurrentPosition()); + telemetry.update(); + sleep(10); + } + //[-219, 1071] + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java new file mode 100644 index 000000000000..424872db219a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.test; + +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.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@Disabled +@TeleOp(group = "Test") +public class MecanumTeleOp extends LinearOpMode { + @Override + public void runOpMode() { + // Declare our motors + // Make sure your ID's match your configuration + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("FL"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("BL"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("FR"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("BR"); + + // Reverse the right side motors. This may be wrong for your setup. + // If your robot moves backwards when commanded to go forwards, + // reverse the left side instead. + // See the note about this earlier on this page. +// frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); +// backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + double y = gamepad1.left_stick_y; // Remember, Y stick value is reversed + double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = -gamepad1.right_stick_x; + + frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + final double LEFT_REDUCTION = 0.98; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator * LEFT_REDUCTION; + double backLeftPower = (y - x + rx) / denominator * LEFT_REDUCTION; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java new file mode 100644 index 000000000000..084bfec67f0e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOSSensor; + +@Disabled +@Autonomous(group = "Test") +public class RRTest extends LinearOpMode { + + private SampleMecanumDrive drive; + private SparkFunOTOSSensor otosSensor; + + @Override + public void runOpMode() throws InterruptedException { + // 初始化驱动器和传感器 + drive = new SampleMecanumDrive(hardwareMap); + // 创建轨迹序列 +// TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(0, 0, 90)) +// .forward(100) +// .turn(Math.toRadians(90)) +// .forward(100) +// .addDisplacementMarker(() -> { +// // 在这里添加要执行的操作，如发射器射击或手臂放下 +// // bot.shooter.shoot(); +// // bot.wobbleArm.lower(); +// }) +// .turn(Math.toRadians(90)) +// .splineTo(new Vector2d(50, 15), Math.toRadians(0)) +// .turn(Math.toRadians(90)) +// .build(); + TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(35.75, -58.91, Math.toRadians(56.65))) + .splineTo(new Vector2d(61.33, -19.84), Math.toRadians(91.97)) + .splineTo(new Vector2d(54.08, 43.80), Math.toRadians(116.57)) + .splineTo(new Vector2d(-6.95, 53.87), Math.toRadians(189.13)) + .splineTo(new Vector2d(-49.44, 28.70), Math.toRadians(243.43)) + .splineTo(new Vector2d(-45.62, -36.15), Math.toRadians(-58.39)) + .build(); + + + // 等待开始指令 + waitForStart(); + + if (isStopRequested()) return; + + // 执行轨迹移动 + drive.followTrajectorySequence(trajectorySequence); + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java new file mode 100644 index 000000000000..6f0a70a0a1a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.test; + +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.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +@Disabled +@TeleOp(group = "Test") +public class ServoTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo = hardwareMap.get(Servo.class , "arm"); + //stretch(1): [0, 0.3] + //turn(3): [0.38-back, 0.71-out] + //grab(5): [0-open, 0.53-close] + //spinY(4): default: 0.07 + //spinX(2): default: 0.5 + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double servoPos = 0; + + while (opModeIsActive()){ + boolean x = gamepad1.x; + boolean b = gamepad1.b; + if(x){ + servoPos += 0.01; + }if(b){ + servoPos -= 0.01; + } + + armServo.setPosition(servoPos); + telemetry.addData("pos", servoPos); + telemetry.update(); + + previousGamepad1.copy(gamepad1); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java new file mode 100644 index 000000000000..1a3cfab2d361 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.test; + +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.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +@Disabled +@TeleOp(group = "Test") +public class ServoTest2 extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + Servo armTurnServo = hardwareMap.get(Servo.class, "armTurn"); + Servo armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); + Servo armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + Servo armGrabServo = hardwareMap.get(Servo.class, "armGrab"); + Servo liftServo = hardwareMap.get(Servo.class, "liftServo"); + Servo containerServo = hardwareMap.get(Servo.class, "containerServo"); + Servo liftTopServo = hardwareMap.get(Servo.class, "liftTop"); + Servo[] servos = {armStretchServo, armTurnServo, armSpinXServo, armSpinYServo, armGrabServo, liftServo, containerServo, liftTopServo}; + String[] names = {"stretch", "turn", "spinX", "spinY", "grab","lift","container","top"}; + //stretch(1): [0, 0.3] + //turn(3): [0.38-back, 0.71-out] + //grab(5): [0-open, 0.53-close] + //spinY(4): default: 0.07 + //spinX(2): default: 0.5 + + waitForStart(); + int index = 0; + boolean activate = false; + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double servoPos = 0; + Servo servo = servos[index]; + + while (opModeIsActive()){ + if(gamepad1.y){ + servoPos = Math.min(1, servoPos + 0.01); + }if(gamepad1.a){ + servoPos = Math.max(0, servoPos - 0.01); + } + if(gamepad1.b && !previousGamepad1.b){ + index += 1; + index = index % servos.length; + servo = servos[index]; + } + if(gamepad1.x && !previousGamepad1.x){ + activate = !activate; + } + if (activate) { + servo.setPosition(servoPos); + } + + telemetry.addData("Y","add position"); + telemetry.addData("A","minus position"); + telemetry.addData("X","activate"); + telemetry.addData("B","change servo"); + telemetry.addData("--------","-------"); + telemetry.addData("pos", servoPos); + telemetry.addData("servo", names[index]); + telemetry.addData("activated",activate); + telemetry.update(); + + previousGamepad1.copy(gamepad1); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java new file mode 100644 index 000000000000..60e5c974b123 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.test; + +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.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +//It proves out to be useless. +//Never use Thread in the Robot! + +@Disabled +public class ThreadTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo = hardwareMap.get(Servo.class , "arm"); + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double a = 0; + Thread t = new Thread(){ + @Override + public void run() { + super.run(); + try { + telemetry.addData("q", 1); + telemetry.update(); + armServo.setPosition(0.9); + sleep(3000); + armServo.setPosition(0.3); + sleep(3000); + armServo.setPosition(0.9); + sleep(3000); + armServo.setPosition(0.3); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + }; + t.start(); + while (opModeIsActive()){ + a += 1; + telemetry.addData("time", a); + telemetry.update(); + if(gamepad1.y && !previousGamepad1.y) { + Thread th = timedSetPosition(armServo, 0.2, 2000); + th.start(); + } + sleep(1000); + previousGamepad1.copy(gamepad1); + } + } + public Thread timedSetPosition(Servo servo, double position, double millisecond){ + telemetry.addData("going to run",1); + return new Thread(){ + @Override + public void run() { + final int TICK_MS = 50; + double deltaPos = position - servo.getPosition(); + double iterationCount = millisecond / TICK_MS; + double perPos = deltaPos / iterationCount; + telemetry.addData("running", 1); + try{ + super.start(); + double servoPos = servo.getPosition(); + for(; iterationCount <= 0; iterationCount--){ + servoPos += perPos; + servo.setPosition(servoPos); + sleep(TICK_MS); + telemetry.addData("for_repetition", 1); + } + }catch (InterruptedException e){ + throw new RuntimeException(e); + } + } + }; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java new file mode 100644 index 000000000000..29f4065763a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.test;//测试摄像头角度识别功能 + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; + +@Disabled +@TeleOp(group = "Test") + +public class VisionCameraAngleTest extends LinearOpMode { + private RobotVisionAngle robotVisionAngle; + + @Override + public void runOpMode() { + robotVisionAngle = new RobotVisionAngle(); + + robotVisionAngle.initialize(hardwareMap); // 初始化摄像头 + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // 显示角度检测结果 + telemetry.addData("Detected Angle", robotVisionAngle.getDetectedAngle()); + telemetry.update(); + + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java new file mode 100644 index 000000000000..22a21a4a9348 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.test;//测试摄像头颜色识别功能 +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionColor; + +@Disabled +@TeleOp(group = "Test") +public class VisionCameraColorTest extends LinearOpMode { + private RobotVisionColor robotVisionColor; + + @Override + public void runOpMode() { + robotVisionColor = new RobotVisionColor(); + + robotVisionColor.initialize(hardwareMap); // 初始化摄像头 + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // 显示颜色检测结果 + telemetry.addData("Center Color", robotVisionColor.getDetectedColor()); + telemetry.update(); + + sleep(100); + } + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java new file mode 100644 index 000000000000..551ad97b75cb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotCameraDistance; + +@Disabled +@TeleOp(name = "VisionCameraDistanceTest", group = "Test") +public class VisionCameraDistanceTest extends LinearOpMode { + private RobotCameraDistance robotCameraDistance; + + @Override + public void runOpMode() { + robotCameraDistance = new RobotCameraDistance(); + + robotCameraDistance.initialize(hardwareMap, "blue"); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + double[] polarCoordinates = robotCameraDistance.getPolarCoordinates(); + + if (gamepad1.a) { + robotCameraDistance.updateCurrentRectExcludingCurrent(); + } + + telemetry.addData("Distance from Center", polarCoordinates[0]); + telemetry.addData("Angle from Center", polarCoordinates[1]); + telemetry.update(); + + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java new file mode 100644 index 000000000000..7b4a7b2e71b5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java @@ -0,0 +1,63 @@ +package org.firstinspires.ftc.teamcode.test; + +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.DcMotor; + +@TeleOp(group = "Test") +public class WheelOffset extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("FL"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("BL"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("FR"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("BR"); + waitForStart(); + + double k1=0.6179,k2=0.7116,k3=6176,k4 = 1; + + frontLeftMotor.setPower(1*k1); + backLeftMotor.setPower(1*k2); + frontRightMotor.setPower(1*k3); + backRightMotor.setPower(1*k4); + boolean flag = true; + while (opModeIsActive()){ + if(getRuntime() >= 3 && flag){ + int FLPos = frontLeftMotor.getCurrentPosition(); + int BLPos = backLeftMotor.getCurrentPosition(); + int FRPos = frontLeftMotor.getCurrentPosition(); + int BRPos = backRightMotor.getCurrentPosition(); + frontLeftMotor.setPower(0); + backLeftMotor.setPower(0); + frontRightMotor.setPower(0); + backRightMotor.setPower(0); + telemetry.addData("FL", FLPos); + telemetry.addData("BL", BLPos); + telemetry.addData("FR", FRPos); + telemetry.addData("BR", BRPos); + double[] pos = {FLPos,BLPos,FRPos,BRPos}; + double[] rate = {1, pos[0]/pos[1], pos[0]/pos[2], pos[0]/pos[3]}; + telemetry.addData("rate origin", rate[0]); + telemetry.addData("rate origin", rate[1]); + telemetry.addData("rate origin", rate[2]); + telemetry.addData("rate origin", rate[3]); + double maxinum = Math.max(Math.max(rate[0],rate[1]),Math.max(rate[2],rate[3])); + double coefficient = 1/maxinum; + int t = 0; + for (double each:rate + ) { + rate[t] = each * coefficient; + t += 1; + } + telemetry.addData("k1", rate[0]); + telemetry.addData("k2", rate[1]); + telemetry.addData("k3", rate[2]); + telemetry.addData("k4", rate[3]); + telemetry.update(); + flag = false; + } + sleep(50); + } + } +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index a2b4ea1130fc..1ec9869b3ed6 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url = 'https://maven.brott.dev/' } } dependencies { @@ -13,5 +14,9 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'com.acmerobotics.roadrunner:core:0.5.2' + implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { + exclude group: 'org.firstinspires.ftc' + } } diff --git a/build.gradle b/build.gradle index c2fef66cf5ff..e8f79f72f864 100644 --- a/build.gradle +++ b/build.gradle @@ -6,14 +6,14 @@ buildscript { repositories { - maven { url "https://maven.aliyun.com/repository/central" } - maven { url "https://maven.aliyun.com/repository/google" } +// maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/google" } mavenCentral() google() } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.4.2' } } @@ -21,14 +21,22 @@ buildscript { // google() repository beginning with version 3.2 of the Android Gradle Plugin allprojects { repositories { - maven { url "https://maven.aliyun.com/repository/central" } - maven { url "https://maven.aliyun.com/repository/google" } +// maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/google" } mavenCentral() google() } } repositories { - maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/central" } mavenCentral() } + +repositories { + mavenCentral() + // 你可以添加其他需要的仓库，例如： + maven { url "https://jitpack.io" } + maven { url "https://repo.sparkfun.com/artifactory/sparkfun-repo" } // 假设这是SparkFun的仓库URL +} + diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fceae6e..7cc7ca52760a 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sun Dec 08 12:29:19 CST 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/settings.gradle b/settings.gradle index 9e2cfb3b4b03..a56e34042da1 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,2 +1,3 @@ include ':FtcRobotController' include ':TeamCode' +include ':MeepMeep'