From ebbbc9c263150ced6523307d609c0ed1777c50b5 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Tue, 6 Feb 2024 23:47:27 -0600 Subject: [PATCH] wrote the implementation for tuning velocity, I plan to tune in the teleOp and then copy over to autonomous --- .../DriveToCoordinatesState.java | 11 +++++++++++ .../CenterStage/Common/CompetitionRobotV1.java | 6 ++++-- .../TeleOp/States/CompetitionTeleOpState.java | 17 +++++++++++++++-- 3 files changed, 30 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java index 300d267..f84336a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -15,6 +15,9 @@ public class DriveToCoordinatesState extends CyberarmState { public double xTarget; public double yTarget; public double hTarget; + public double maxVelocityX; + public double maxVelocityY; + public double maxVelocityH; public boolean posAchieved = false; public boolean armDrive; public int objectPos; @@ -57,6 +60,12 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void exec() { + if (robot.xVelocity > maxVelocityX){ + maxVelocityX = robot.xVelocity; + } + if (robot.yVelocity > maxVelocityY){ + maxVelocityY = robot.yVelocity; + } if (posSpecific) { if (objectPos != robot.objectPos) { // enter the ending loop @@ -86,6 +95,8 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void telemetry() { + engine.telemetry.addData("x velocity max", maxVelocityX); + engine.telemetry.addData("y velocity max", maxVelocityY); engine.telemetry.addData("x pos", robot.positionX); engine.telemetry.addData("y pos", robot.positionY); engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index cc15127..f468362 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -52,7 +52,7 @@ public class CompetitionRobotV1 extends Robot { // ----------------------------------------------------------------------------------------------------------------- odometry variables: public static double Hp = 0.8, Hi = 0, Hd = 0; public static double Xp = -0.03, Xi = 0, Xd = 0; - public static double xvp = -0.03, xvi = 0, xvd = 0; + public static double xvp = -0, xvi = 0, xvd = 0; public static double Yp = 0.03, Yi = 0, Yd = 0; public static double yvp = 0.03, yvi = 0, yvd = 0; @@ -75,7 +75,7 @@ public class CompetitionRobotV1 extends Robot { public int oldRightPosition = 0; public int oldLeftPosition = 0; public int oldAuxPosition = 0; - public final static double L = 22.5; // distance between left and right encoder in cm + public final static double L = 20.9; // distance between left and right encoder in cm final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm public final static double R = 3; // wheel radius in cm final static double N = 8192; // encoder ticks per revolution (REV encoder) @@ -376,6 +376,8 @@ public class CompetitionRobotV1 extends Robot { public void DriveToCoordinates () { // determine the powers needed for each direction // this uses PID to adjust needed Power for robot to move to target + targetVelocityX = XPIDControl(xTarget, positionX); + targetVelocityY = YPIDControl(yTarget, positionY); XVeloPIDControl(targetVelocityX, xVelocity); YVeloPIDControl(targetVelocityY, yVelocity); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java index 173c79d..32998df 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -28,6 +28,9 @@ public class CompetitionTeleOpState extends CyberarmState { public static double holdPos = 0.55 ; + public double maxVelocityX; + public double maxVelocityY; + // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; private double targetHeading; @@ -110,7 +113,6 @@ public class CompetitionTeleOpState extends CyberarmState { } } - public void DriveTrainTeleOp () { boolean lbs1 = engine.gamepad1.left_stick_button; @@ -340,7 +342,13 @@ public class CompetitionTeleOpState extends CyberarmState { } @Override - public void exec() { + public void exec() { //------------------------------------------------------------------------------------------------------ EXEC Start + if (robot.xVelocity > maxVelocityX){ + maxVelocityX = robot.xVelocity; + } + if (robot.yVelocity > maxVelocityY){ + maxVelocityY = robot.yVelocity; + } robot.OdometryLocalizer(); if (engine.gamepad2.start && engine.gamepad2.x){ @@ -409,10 +417,15 @@ public class CompetitionTeleOpState extends CyberarmState { ClawControlTeleOp(); + + robot.velocityChecker(); + } @Override public void telemetry () { + engine.telemetry.addData("x velocity max", maxVelocityX); + engine.telemetry.addData("y velocity max", maxVelocityY); engine.telemetry.addData("Dnl1", robot.Dnl1); engine.telemetry.addData("Dnr2", robot.Dnr2); engine.telemetry.addData("x pos", robot.positionX);