From 5043110e5ceff933c373cc8702c4ea4f11877240 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 9 Dec 2023 15:05:54 -0600 Subject: [PATCH] autonomous states and engine --- .../CompetitionRedClose.java | 30 ++++++ .../CompetitionStates/ClawArmState.java | 89 ++++++++++++++++++ .../CompetitionStates/DistanceCheckState.java | 74 +++++++++++++++ .../DriveToCoordinatesState.java | 28 ++++-- .../Engines/OdometryTestEngine.java | 11 +-- .../Common/CompetitionRobotV1.java | 91 ++++++++++++++----- .../Engines/CompetitionRobotV1Engine.java | 2 - .../TeleOp/States/CompetitionTeleOpState.java | 84 +++++++++++------ 8 files changed, 338 insertions(+), 71 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java new file mode 100644 index 0000000..94c40de --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java @@ -0,0 +1,30 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Competition Red Close") + +public class CompetitionRedClose extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Competition Red Close"); + this.robot.setup(); + + addState(new DriveToCoordinatesState(robot,"Competition Red Close", "00-1")); + addState(new DistanceCheckState(robot,"Competition Red Close", "00-2")); + + addState(new ClawArmState(robot,"Competition Red Close", "01-0")); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java new file mode 100644 index 0000000..b3a713e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java @@ -0,0 +1,89 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config +public class ClawArmState extends CyberarmState { + + CompetitionRobotV1 robot; + public String armPos; + + public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) { + this.robot = robot; + this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value(); + } + + @Override + public void exec() { + // odometry driving ALWAYS + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); + + // driving arm to pos + if (armPos.equals("collect")){ + if (robot.lift.getCurrentPosition() >= 1){ + robot.lift.setPower(-0.6); + } else { + robot.lift.setPower(0); + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + robot.target = 10; + + } + } + if (armPos.equals("passive")){ + if (robot.lift.getCurrentPosition() >= 1){ + robot.lift.setPower(-0.6); + } else { + robot.lift.setPower(0); + robot.shoulder.setPosition(robot.shoulderPassive); + robot.elbow.setPosition(robot.elbowPassive); + robot.target = 850; + } + } + if (armPos.equals("deposit")){ + robot.shoulder.setPosition(robot.shoulderDeposit); + robot.elbow.setPosition(robot.elbowDeposit); + robot.target = 370; + + } + if (armPos.equals("hover")) { + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + robot.target = 150; + + } + robot.armPos = armPos; + robot.clawArmControl(); + + if (robot.clawArm.getCurrentPosition() >= robot.target - 20 && robot.clawArm.getCurrentPosition() <= robot.target + 20) { +// setHasFinished(true); + } + + + + } + + + @Override + public void telemetry() { + engine.telemetry.addData("x pos", robot.positionX); + engine.telemetry.addData("y pos", robot.positionY); + engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); + engine.telemetry.addData("aux encoder", robot.currentAuxPosition); + engine.telemetry.addData("left encoder", robot.currentLeftPosition); + engine.telemetry.addData("right encoder", robot.currentRightPosition); + engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("front right power", robot.frontRightPower); + engine.telemetry.addData("front left power", robot.frontLeftPower); + engine.telemetry.addData("back right power", robot.backRightPower); + engine.telemetry.addData("back left power", robot.backLeftPower); + engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition()); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java new file mode 100644 index 0000000..5c1315f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java @@ -0,0 +1,74 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config +public class DistanceCheckState extends CyberarmState { + + CompetitionRobotV1 robot; + public String armPos; + public long lastCheckedTIme = System.currentTimeMillis(); + + public DistanceCheckState(CompetitionRobotV1 robot, String groupName, String actionName) { + this.robot = robot; + } + + @Override + public void exec() { + // odometry driving ALWAYS + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); + + if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > -47 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < -43){ + robot.objectPos = "Right"; + } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000){ + robot.hTarget = 0; + } + if (robot.objectPos != "right") + if (Math.abs(robot.backLeftPower) < 0.15 && + Math.abs(robot.backRightPower) < 0.15 && + Math.abs(robot.frontLeftPower) < 0.15 && + Math.abs(robot.frontRightPower) < 0.15 && System.currentTimeMillis() - lastCheckedTIme > 2000){ + if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 2000){ + robot.objectPos = "middle"; + } else { + robot.hTarget = 45; + robot.objectPos = "left"; + } + } + + if (robot.objectPos.equals("left") || robot.objectPos.equals("right") || robot.objectPos.equals("middle")){ + setHasFinished(true); + } + + + + + + } + + + @Override + public void telemetry() { + engine.telemetry.addData("x pos", robot.positionX); + engine.telemetry.addData("y pos", robot.positionY); + engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); + engine.telemetry.addData("aux encoder", robot.currentAuxPosition); + engine.telemetry.addData("left encoder", robot.currentLeftPosition); + engine.telemetry.addData("right encoder", robot.currentRightPosition); + engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("front right power", robot.frontRightPower); + engine.telemetry.addData("front left power", robot.frontLeftPower); + engine.telemetry.addData("back right power", robot.backRightPower); + engine.telemetry.addData("back left power", robot.backLeftPower); + engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition()); + engine.telemetry.addData("distance ", robot.customObject.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("object location ", robot.objectPos); + } +} 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 ffeb038..7ef6c60 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 @@ -16,12 +16,14 @@ public class DriveToCoordinatesState extends CyberarmState { public static double yTarget = 0; public static double hTarget= 0; public boolean posAchieved = false; + public boolean armDrive; - public DriveToCoordinatesState(CompetitionRobotV1 robot/*, String groupName, String actionName*/) { + public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; -// this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); -// this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); -// this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); + this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); + this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); + this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); + this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value(); } @Override @@ -32,13 +34,19 @@ public class DriveToCoordinatesState extends CyberarmState { robot.DriveToCoordinates(); robot.OdometryLocalizer(); + + if (armDrive){ + robot.clawArmControl(); + } + if (posAchieved){ -// setHasFinished(true); + setHasFinished(true); } else { - if (((robot.positionX > xTarget - 1) && (robot.positionX < xTarget + 1)) && - ((robot.positionH > hTarget - 1) && (robot.positionH < hTarget + 1)) && - ((robot.positionY > yTarget - 1) && (robot.positionY < yTarget + 1))){ -// posAchieved = true; + if (Math.abs(robot.backLeftPower) < 0.15 && + Math.abs(robot.backRightPower) < 0.15 && + Math.abs(robot.frontLeftPower) < 0.15 && + Math.abs(robot.frontRightPower) < 0.15){ + posAchieved = true; } } } @@ -48,7 +56,7 @@ public class DriveToCoordinatesState extends CyberarmState { public void telemetry() { engine.telemetry.addData("x pos", robot.positionX); engine.telemetry.addData("y pos", robot.positionY); - engine.telemetry.addData("h pos odo", robot.positionH); + engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); engine.telemetry.addData("aux encoder", robot.currentAuxPosition); engine.telemetry.addData("left encoder", robot.currentLeftPosition); engine.telemetry.addData("right encoder", robot.currentRightPosition); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java index 17d8462..4ceae82 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import org.timecrafters.CenterStage.Common.PrototypeRobot; @@ -15,18 +16,14 @@ public class OdometryTestEngine extends CyberarmEngine { CompetitionRobotV1 robot; + @Override public void setup() { this.robot = new CompetitionRobotV1("Autonomous odometry test"); this.robot.setup(); - addState(new DriveToCoordinatesState(robot/*,"odoTest", "00-1"*/)); + addState(new DriveToCoordinatesState(robot,"odoTest", "00-1")); + addState(new ClawArmState(robot,"odoTest", "01-0")); } - @Override - public void init() { - robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - } } 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 7405508..7d827d5 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -1,9 +1,12 @@ package org.timecrafters.CenterStage.Common; import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; @@ -22,25 +25,27 @@ public class CompetitionRobotV1 extends Robot { private CyberarmEngine engine; public TimeCraftersConfiguration configuration; + public String objectPos; // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: - public DcMotor frontLeft, frontRight, backLeft, backRight, lift; + public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp; public DcMotor odometerR, odometerL, odometerA; public IMU imu; public Servo shoulder, elbow, leftClaw, rightClaw; + public DistanceSensor customObject; // ----------------------------------------------------------------------------------------------------------------- odometry variables: - public static double Hp = 0, Hi = 0, Hd = 0; - public static double Xp = 0, Xi = 0, Xd = 0; - public static double Yp = 0, Yi = 0, Yd = 0; + public static double Hp = 0.8, Hi = 0, Hd = 0; + public static double Xp = -0.03, Xi = 0, Xd = 0; + public static double Yp = 0.03, Yi = 0, Yd = 0.0013; private double drivePower = 1; public double rx; public double xMultiplier = 1; public double yMultiplier = 1; - public double positionX = 0; - public double positionY = 0; + public double positionX = 1000; + public double positionY = 1000; public double positionH = 0; public double xTarget; public double yTarget; @@ -53,7 +58,7 @@ public class CompetitionRobotV1 extends Robot { public int oldLeftPosition = 0; public int oldAuxPosition = 0; public final static double L = 22.5; // distance between left and right encoder in cm - final static double B = 11.5; // distance between the midpoint of the left and right encoder from the auxillary 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) @@ -75,12 +80,17 @@ public class CompetitionRobotV1 extends Robot { public double backRightPower; //-------------------------------------------------------------------------------------------------------------- arm sequence variables: + PIDController pidController; + public String armPos; + public int target; + public static double p = 0.0015, i = 0, d = 0, f = 0; public static double shoulderCollect = 0.25; - public static double shoulderDeposit = 0.55; - public static double shoulderPassive = 1; - public static double elbowCollect = 0.7; - public static double elbowDeposit = 0.8; - public static double elbowPassive = 0.28; + public static double shoulderDeposit = 0.32; + public static double shoulderPassive = 0.8; + public static double elbowCollect = 0; + public static double elbowDeposit = 0; + public static double elbowPassive = 0; + private HardwareMap hardwareMap; @@ -90,6 +100,8 @@ public class CompetitionRobotV1 extends Robot { this.engine = engine; setup(); this.string = string; + pidController = new PIDController(p, i, d); + } public void initConstants() { @@ -109,24 +121,38 @@ public class CompetitionRobotV1 extends Robot { frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); + chinUp = engine.hardwareMap.dcMotor.get("chinUp"); lift = engine.hardwareMap.dcMotor.get("Lift"); + clawArm = engine.hardwareMap.dcMotor.get("clawArm"); frontRight.setDirection(DcMotorSimple.Direction.FORWARD); backRight.setDirection(DcMotorSimple.Direction.REVERSE); backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + clawArm.setDirection(DcMotorSimple.Direction.REVERSE); lift.setDirection(DcMotorSimple.Direction.FORWARD); + chinUp.setDirection(DcMotorSimple.Direction.FORWARD); backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + chinUp.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //IMU IMU.Parameters parameters = new IMU.Parameters( @@ -142,15 +168,20 @@ public class CompetitionRobotV1 extends Robot { initConstants(); + pidController = new PIDController(p, i, d); + + customObject = engine.hardwareMap.get(Rev2mDistanceSensor.class, "customObject"); //--------------------------------------------------------------------------------------------------------------------------- SERVO: shoulder = hardwareMap.servo.get("shoulder"); elbow = hardwareMap.servo.get("elbow"); leftClaw = hardwareMap.servo.get("leftClaw"); - rightClaw = hardwareMap.servo.get("leftClaw"); + rightClaw = hardwareMap.servo.get("rightClaw"); - shoulder.setPosition(shoulderPassive); - elbow.setPosition(elbowPassive); + elbow.setDirection(Servo.Direction.REVERSE); + +// shoulder.setPosition(shoulderPassive); +// elbow.setPosition(elbowPassive); @@ -160,11 +191,12 @@ public class CompetitionRobotV1 extends Robot { public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer: // update positions + oldRightPosition = currentRightPosition; oldLeftPosition = currentLeftPosition; oldAuxPosition = currentAuxPosition; - currentRightPosition = -frontLeft.getCurrentPosition(); + currentRightPosition = frontLeft.getCurrentPosition(); currentLeftPosition = -backRight.getCurrentPosition(); currentAuxPosition = backLeft.getCurrentPosition(); @@ -205,7 +237,7 @@ public class CompetitionRobotV1 extends Robot { } public double XPIDControl ( double reference, double current){ - double error = (current - reference); + double error = (reference - current); xIntegralSum += error * xTimer.seconds(); double derivative = (error - xLastError) / xTimer.seconds(); @@ -216,7 +248,7 @@ public class CompetitionRobotV1 extends Robot { } public double YPIDControl ( double reference, double current){ - double error = (current - reference); + double error = (reference - current); yIntegralSum += error * yTimer.seconds(); double derivative = (error - yLastError) / yTimer.seconds(); @@ -232,8 +264,9 @@ public class CompetitionRobotV1 extends Robot { // this uses PID to adjust needed Power for robot to move to target double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); - double pidX = XPIDControl(xTarget, positionX); - double pidY = YPIDControl(yTarget, positionY); + + double pidX = YPIDControl(yTarget, positionY); + double pidY = XPIDControl(xTarget, positionX); double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); @@ -246,12 +279,24 @@ public class CompetitionRobotV1 extends Robot { frontLeftPower = (rotY + rotX + rx) / denominator; backLeftPower = (rotY - rotX + rx) / denominator; frontRightPower = (rotY - rotX - rx) / denominator; - backRightPower = (rotY - rotX + rx) / denominator; + backRightPower = (rotY + rotX - rx) / denominator; // apply my powers frontLeft.setPower(frontLeftPower); backLeft.setPower(backLeftPower); - backRight.setPower(backRightPower); + backRight.setPower(-backRightPower); frontRight.setPower(frontRightPower); } + + public void clawArmControl(){ + + pidController.setPID(p, i, d); + int armPos = clawArm.getCurrentPosition(); + double pid = pidController.calculate(armPos, target); + + double power = pid; + + clawArm.setPower(power); + + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java index 00774fc..867b395 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/CompetitionRobotV1Engine.java @@ -3,9 +3,7 @@ package org.timecrafters.CenterStage.TeleOp.Engines; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; -import org.timecrafters.CenterStage.Common.PrototypeRobot; import org.timecrafters.CenterStage.TeleOp.States.CompetitionTeleOpState; -import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; import dev.cyberarm.engine.V2.CyberarmEngine; 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 bb76a7c..246e8d1 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 @@ -1,6 +1,8 @@ package org.timecrafters.CenterStage.TeleOp.States; import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -14,6 +16,11 @@ import dev.cyberarm.engine.V2.CyberarmState; public class CompetitionTeleOpState extends CyberarmState { // ------------------------------------------------------------------------------------------------------------- State and engine setup: private CompetitionRobotV1 robot; + // ------------------------------------------------------------------------------------------------------------robot claw arm variables: + private PIDController pidController; + public static double p = 0.0015, i = 0, d = 0, f = 0; + public static int target = 0; + // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; private double targetHeading; @@ -31,8 +38,8 @@ public class CompetitionTeleOpState extends CyberarmState { ElapsedTime timer = new ElapsedTime(); // ---------------------------------------------------------------------------------------------------- collector / depositor variables: - public static double leftOpen = 0; - public static double leftClose = 0; + public static double leftOpen = 0.25; + public static double leftClose = 0.6; public static double rightOpen = 0.6; public static double rightClose = 0.25; public double collectorPosLeft = leftClose; @@ -58,7 +65,7 @@ public class CompetitionTeleOpState extends CyberarmState { public CompetitionTeleOpState(CompetitionRobotV1 robot) { this.robot = robot; - + pidController = new PIDController(p, i, d); } // } @@ -129,7 +136,7 @@ public class CompetitionTeleOpState extends CyberarmState { // setting each power determined previously from the math above // as well as multiplying it by a drive power that can be changed. robot.backLeft.setPower(backLeftPower * drivePower); - robot.backRight.setPower(backRightPower * drivePower); + robot.backRight.setPower(-backRightPower * drivePower); robot.frontLeft.setPower(frontLeftPower * drivePower); robot.frontRight.setPower(frontRightPower * drivePower); } @@ -155,7 +162,7 @@ public class CompetitionTeleOpState extends CyberarmState { return output; } public void ClawControlTeleOp() { - boolean b2 = engine.gamepad2.b; + boolean b2 = engine.gamepad1.y; if (b2 && !bVar2) { if (collectorPosLeft == leftClose && collectorPosRight == rightClose) { collectorPosLeft = leftOpen; @@ -167,7 +174,7 @@ public class CompetitionTeleOpState extends CyberarmState { } bVar2 = b2; - boolean lbs2 = engine.gamepad2.left_stick_button; + boolean lbs2 = engine.gamepad1.left_bumper; if (lbs2 && !lbsVar2) { if (collectorPosLeft == leftClose) { collectorPosLeft = leftOpen; @@ -177,7 +184,7 @@ public class CompetitionTeleOpState extends CyberarmState { } lbsVar2 = lbs2; - boolean rbs2 = engine.gamepad2.right_stick_button; + boolean rbs2 = engine.gamepad1.right_bumper; if (rbs2 && !rbsVar2) { if (collectorPosRight == rightClose) { collectorPosRight = rightOpen; @@ -200,6 +207,10 @@ public class CompetitionTeleOpState extends CyberarmState { armPos = "deposit"; depositMode = true; } + else if (engine.gamepad2.b){ + armPos = "hover"; + depositMode = true; + } if (armPos == "collect"){ if (robot.lift.getCurrentPosition() >= 1){ @@ -208,6 +219,8 @@ public class CompetitionTeleOpState extends CyberarmState { robot.lift.setPower(0); robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); + target = 10; + } } if (armPos == "passive"){ @@ -217,20 +230,42 @@ public class CompetitionTeleOpState extends CyberarmState { robot.lift.setPower(0); robot.shoulder.setPosition(robot.shoulderPassive); robot.elbow.setPosition(robot.elbowPassive); + target = 850; } } if (armPos == "deposit"){ robot.shoulder.setPosition(robot.shoulderDeposit); robot.elbow.setPosition(robot.elbowDeposit); - } + target = 370; + + } + if (armPos == "hover"){ + + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + target = 120; + + + } } @Override public void init() { + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + pidController = new PIDController(p, i, d); + } @Override public void exec() { + if (engine.gamepad2.dpad_up) { + robot.chinUp.setPower(1); + } else if (engine.gamepad2.dpad_down){ + robot.chinUp.setPower(-1); + } else { + robot.chinUp.setPower(0); + } // ---------------------------------------------------------------------------------------------------------- Game Pad 1, drivetrain if (engine.gamepad1.right_stick_button) { robot.imu.resetYaw(); @@ -265,35 +300,25 @@ public class CompetitionTeleOpState extends CyberarmState { } // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift: + pidController.setPID(p, i, d); + int armPos = robot.clawArm.getCurrentPosition(); + double pid = pidController.calculate(armPos, target); +// double ff = Math.cos(Math.toRadians(target / ticksInDegree)) * f; + + double power = pid; +// double power = pid + ff; + + robot.clawArm.setPower(power); // ------------------------------------------------------------------------------------------------------------------- Lift Control: SliderTeleOp(); robot.leftClaw.setPosition(collectorPosLeft); robot.rightClaw.setPosition(collectorPosRight); - - boolean lbs2 = engine.gamepad2.left_stick_button; - if (lbs2 && !lbsVar2) { - if (collectorPosLeft == leftClose) { - collectorPosLeft = leftOpen; - } else { - collectorPosLeft = leftClose; - } - } - lbsVar2 = lbs2; - - boolean rbs2 = engine.gamepad2.right_stick_button; - if (rbs2 && !rbsVar2) { - if (collectorPosRight == rightClose) { - collectorPosRight = rightOpen; - } else { - collectorPosRight = rightClose; - } - } - rbsVar2 = rbs2; - ArmPosControl(); + ClawControlTeleOp(); + } @Override @@ -308,6 +333,7 @@ public class CompetitionTeleOpState extends CyberarmState { engine.telemetry.addData("Ki", Ki); engine.telemetry.addData("Kd", Kd); engine.telemetry.addData("arm pos", armPos); + engine.telemetry.addData("arm pos ticks", robot.clawArm.getCurrentPosition()); } }