From 208d7c1513072aa6f4d79fa419a90c5af6b06774 Mon Sep 17 00:00:00 2001 From: scott Date: Mon, 4 Dec 2023 21:03:26 -0600 Subject: [PATCH 1/3] debugging --- .../cyberarm/minibots/red_crab/RedCrabMinibot.java | 14 +++++++------- .../cyberarm/minibots/red_crab/states/Pilot.java | 6 +++--- gradle/wrapper/gradle-wrapper.properties | 3 ++- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 484d057..06b7fa9 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -18,7 +18,7 @@ public class RedCrabMinibot { public static final int ClawArm_COLLECT = 2; /// TUNING CONSTANTS /// - public static final double DRIVETRAIN_MAX_SPEED = 0.5; + public static final double DRIVETRAIN_MAX_SPEED = 1.0; public static final double CLAW_ARM_MAX_SPEED = 0.5; public static final double WINCH_MAX_SPEED = 0.5; public static final double CLAW_ARM_STOW_ANGLE = 0.0; @@ -38,8 +38,8 @@ public class RedCrabMinibot { public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5; public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; - public static final double HOOK_ARM_STOW_POSITION = 0.0; - public static final double HOOK_ARM_UP_POSITION = 0.5; + public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8 + public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0 /// MOTOR CONSTANTS /// public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; @@ -78,10 +78,10 @@ public class RedCrabMinibot { backRight.resetEncoder(); /// --- MOTOR DIRECTIONS - frontLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); - frontRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); - backLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); - backRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); /// --- MOTOR BRAKING MODE /// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 0d086cb..7eaa9f5 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -27,10 +27,10 @@ public class Pilot extends CyberarmState { public void exec() { drivetrain(); -// clawArmAndWristController(); + clawArmAndWristController(); // clawController(); // droneLatchController(); -// hookArmController(); +// hookArmController(); // disabled for swrist debug // winchController(); } @@ -149,7 +149,7 @@ public class Pilot extends CyberarmState { } - robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED); + robot.clawArm.set(RedCrabMinibot. CLAW_ARM_MAX_SPEED); } private void clawController() { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fc..8da469e 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Mon Dec 04 18:52:45 CST 2023 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-7.3.3-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From db49950f30e8c898a0a8719fa90bea746daa24c3 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Mon, 4 Dec 2023 21:31:51 -0600 Subject: [PATCH 2/3] Second drive state, tried to make ramping speed work better --- .../Engines/SodiPizzaAutoRedRightEngine.java | 3 +- .../States/SodiPizzaAutoFirstDriveState.java | 53 ++++++++++---- .../States/SodiPizzaAutoSecDriveState.java | 72 +++++++++++++++++++ .../States/SodiPizzaAutoTurnState.java | 22 ++++-- 4 files changed, 128 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java index 2c05058..37b1b97 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java @@ -17,8 +17,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { blackboardSet("readyToTurn", 0); + addState(new SodiPizzaAutoFirstDriveState()); addState(new SodiPizzaAutoTurnState()); - addParallelStateToLastState(new SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoSecDriveState()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java index 87556d7..c73c6b3 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java @@ -11,9 +11,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ final private SodiPizzaMinibotObject robot; final private String groupName, actionName; private long lastMoveTime; - private int targetPos = 2500; - private double drivePower; - public int readyToTurn; + private double drivePower, drivePowerRaw; + public int readyToTurn, neededTicks, currentTicks, targetTicks = 1500; public SodiPizzaAutoFirstDriveState() { groupName = " "; @@ -21,6 +20,25 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ robot = new SodiPizzaMinibotObject(); robot.setup(); } + + private double getDrivePower() { + if (Math.abs(neededTicks) > 1) { + drivePower = (drivePowerRaw * neededTicks) / 10; + } + return drivePower; + } + + public void CalculateNeededTicks() { + if (targetTicks >= 0 && currentTicks >= 0) { + neededTicks = Math.abs(targetTicks - currentTicks); + } else if (targetTicks < 0 && currentTicks < 0) { + neededTicks = Math.abs(targetTicks - currentTicks); + } else if (targetTicks > 0 && currentTicks < 0) { + neededTicks = (targetTicks + Math.abs(currentTicks)); + } else if (targetTicks < 0 && currentTicks > 0) { + neededTicks = (currentTicks + Math.abs(targetTicks)); + } + } @SuppressLint("SuspiciousIndentation") @@ -34,10 +52,9 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ @Override public void telemetry() { - engine.telemetry.addData("Target Ticks?", robot.leftFront.getTargetPosition()); - engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition()); - engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() - - robot.leftFront.getCurrentPosition()); + engine.telemetry.addData("Target Ticks?", targetTicks); + engine.telemetry.addData("Current Ticks?", currentTicks); + engine.telemetry.addData("Ticks Needed?", neededTicks); engine.telemetry.addLine(); engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn); engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM)); @@ -49,15 +66,20 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ readyToTurn = engine.blackboardGet("readyToTurn"); - // Move forward from 0 to targetPos + currentTicks = robot.leftFront.getCurrentPosition(); + + CalculateNeededTicks(); + + // Move forward from 0 to targetTicks if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) { - robot.leftFront.setTargetPosition(targetPos); - robot.leftBack.setTargetPosition(targetPos); - robot.rightFront.setTargetPosition(targetPos); - robot.rightBack.setTargetPosition(targetPos); + robot.leftFront.setTargetPosition(targetTicks); + robot.leftBack.setTargetPosition(targetTicks); + robot.rightFront.setTargetPosition(targetTicks); + robot.rightBack.setTargetPosition(targetTicks); - drivePower = 0.5; + drivePowerRaw = 0.5; + getDrivePower(); robot.leftFront.setPower(drivePower); robot.leftBack.setPower(drivePower); @@ -66,9 +88,10 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ } //Stop and finish set after return to 0 - else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) { + else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) { - drivePower = 0; + drivePowerRaw = 0; + getDrivePower(); robot.leftFront.setPower(drivePower); robot.leftBack.setPower(drivePower); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java index 453d17c..3938419 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -8,6 +10,9 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState { final private SodiPizzaMinibotObject robot; final private String groupName, actionName; + private long lastMoveTime; + private double drivePower, drivePowerRaw; + public int readyToTurn, neededTicks, currentTicks, targetTicks = 500; public SodiPizzaAutoSecDriveState() { groupName = " "; @@ -15,14 +20,81 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState { robot = new SodiPizzaMinibotObject(); robot.setup(); } + + private double getDrivePower() { + if (Math.abs(neededTicks) > 1) { + drivePower = (drivePowerRaw * neededTicks) / 10; + } + return drivePower; + } + + public void CalculateNeededTicks() { + if (targetTicks >= 0 && currentTicks >= 0) { + neededTicks = Math.abs(targetTicks - currentTicks); + } else if (targetTicks < 0 && currentTicks < 0) { + neededTicks = Math.abs(targetTicks - currentTicks); + } else if (targetTicks > 0 && currentTicks < 0) { + neededTicks = (targetTicks + Math.abs(currentTicks)); + } else if (targetTicks < 0 && currentTicks > 0) { + neededTicks = (currentTicks + Math.abs(targetTicks)); + } + } @Override public void start() { + + robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } @Override public void exec() { + readyToTurn = engine.blackboardGet("readyToTurn"); + + currentTicks = robot.leftFront.getCurrentPosition(); + + CalculateNeededTicks(); + + // Move forward from 0 to targetPos + if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2) { + + robot.leftFront.setTargetPosition(targetTicks); + robot.leftBack.setTargetPosition(targetTicks); + robot.rightFront.setTargetPosition(targetTicks); + robot.rightBack.setTargetPosition(targetTicks); + + drivePowerRaw = 0.5; + getDrivePower(); + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + } + //Stop and finish set after return to 0 + else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) { + + drivePowerRaw = 0; + getDrivePower(); + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + engine.blackboardSet("readyToTurn", 3); + + setHasFinished(true); + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java index 0823dc9..0651879 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import android.annotation.SuppressLint; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; @@ -9,7 +11,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmState; public class SodiPizzaAutoTurnState extends CyberarmState { - final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject(); + final private SodiPizzaMinibotObject robot; final private String groupName, actionName; private long lastMoveTime; private double turnSpeedRaw, turnSpeed; @@ -29,11 +31,12 @@ public class SodiPizzaAutoTurnState extends CyberarmState { public SodiPizzaAutoTurnState() { groupName = " "; actionName = " "; + robot = new SodiPizzaMinibotObject(); robot.setup(); } private double getTurnSpeed() { - if (Math.abs(neededRot) > 5) { + if (Math.abs(neededRot) > 1) { turnSpeed = (turnSpeedRaw * neededRot) / 10; } return turnSpeed; @@ -79,6 +82,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState { } + @SuppressLint("SuspiciousIndentation") @Override public void exec() { @@ -92,14 +96,20 @@ public class SodiPizzaAutoTurnState extends CyberarmState { if (readyToTurn == 0) { targetRot = 0; - if (currentRot < targetRot - 1) { + if (currentRot <= targetRot - 1) { turnSpeedRaw = 0.5; getTurnSpeed(); robot.rightFront.setPower(robot.leftFront.getPower() + turnSpeed); robot.rightBack.setPower(robot.leftBack.getPower() + turnSpeed); - } + + } else if (currentRot >= targetRot + 1) + + turnSpeedRaw = 0.5; + getTurnSpeed(); + + robot.leftFront.setPower(robot.rightFront.getPower() + turnSpeed); } @@ -130,7 +140,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(-turnSpeed); robot.rightBack.setPower(-turnSpeed); - } else if (readyToTurn == 1 && Math.abs(neededRot) < 3) { + } else if (readyToTurn == 1 && Math.abs(neededRot) < 2) { turnSpeedRaw = 0; getTurnSpeed(); @@ -140,7 +150,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState { robot.rightFront.setPower(turnSpeed); robot.rightBack.setPower(turnSpeed); - engine.blackboardSet("readyToTurn", 0); + engine.blackboardSet("readyToTurn", 2); } } From 77f162a81f37a9cea1ade00bec7630cb167bab03 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Mon, 4 Dec 2023 21:33:13 -0600 Subject: [PATCH 3/3] Second drive state, tried to make ramping speed work better --- .../Autonomous/Engines/SodiPizzaAutoRedRightEngine.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java index 5718379..6db291f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoFirstDriveState; import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoTurnState; +import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoSecDriveState; import dev.cyberarm.engine.V2.CyberarmEngine;