From 58f4308588774e6c5c4a9c23cadab622ad12c2b7 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 4 Dec 2023 23:42:50 -0600 Subject: [PATCH] wrote the entire TeleOp code for V1 teleOp. plan to fine tune and make sure everything works consistently for the scrimmage right away after school, just gotta finalize variables for arm positions. --- .../Common/CompetitionRobotV1.java | 43 +--- .../TeleOp/States/CompetitionTeleOpState.java | 229 ++++++++++++++---- 2 files changed, 184 insertions(+), 88 deletions(-) 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 39a2a59..588a8d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -20,7 +20,7 @@ public class CompetitionRobotV1 extends Robot { public TimeCraftersConfiguration configuration; - // HardwareMap setup + // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: public DcMotor frontLeft, frontRight, backLeft, backRight, lift; public DcMotor odometerR, odometerL, odometerA; @@ -28,7 +28,7 @@ public class CompetitionRobotV1 extends Robot { public IMU imu; public Servo shoulder, elbow, leftClaw, rightClaw; - // ---------------------------------------------------------------------------------------------------- odometry variables: + // ----------------------------------------------------------------------------------------------------------------- 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; @@ -62,13 +62,6 @@ public class CompetitionRobotV1 extends Robot { private double lastError = 0; ElapsedTime timer = new ElapsedTime(); - // ---------------------------------------------------------------------------------------------------- collector / depositor variables: - - public float depositorPos; - public float collectorPos; - public boolean lbsVar2; - public boolean rbsVar2; - //-------------------------------------------------------------------------------------------------------------- arm sequence variables: public int armPosition = 0; public long startOfSequencerTime; @@ -114,7 +107,7 @@ public class CompetitionRobotV1 extends Robot { imu = engine.hardwareMap.get(IMU.class, "imu"); - //----------------------------------------------------------------------------------------------------------------------------MOTORS + //-------------------------------------------------------------------------------------------------------------------------- MOTORS: frontRight = engine.hardwareMap.dcMotor.get("frontRight"); frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); backRight = engine.hardwareMap.dcMotor.get("backRight"); @@ -149,7 +142,7 @@ public class CompetitionRobotV1 extends Robot { initConstants(); - //-----------------------------------------------------------------------------------------------------------------------------SERVO + //--------------------------------------------------------------------------------------------------------------------------- SERVO: shoulder = hardwareMap.servo.get("shoulder"); elbow = hardwareMap.servo.get("elbow"); leftClaw = hardwareMap.servo.get("leftClaw"); @@ -159,33 +152,9 @@ public class CompetitionRobotV1 extends Robot { } - public void CollectorToggle() { - boolean lbs2 = engine.gamepad2.left_stick_button; - if (lbs2 && !lbsVar2) { - if (collectorPos == 1F) { - collectorPos = 0F; - } else { - collectorPos = 1F; - } - } - lbsVar2 = lbs2; - } - - - public void DepositorToggle() { - boolean rbs2 = engine.gamepad2.right_stick_button; - if (rbs2 && !rbsVar2) { - if (depositorPos == 0.6F) { - depositorPos = 0F; - } else { - depositorPos = 0.6F; - } - } - rbsVar2 = rbs2; - } - - public void OdometryLocalizer() { + // -------------------------------------------------------------------------------------------------------------------------- Functions: + public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer: // update positions oldRightPosition = currentRightPosition; oldLeftPosition = currentLeftPosition; 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 e473faa..6b53787 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 @@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; @@ -11,11 +12,13 @@ import dev.cyberarm.engine.V2.CyberarmState; @Config public class CompetitionTeleOpState extends CyberarmState { - private PrototypeRobot robot; - private int maxExtension = 2000; - private int minExtension = 0; + // ------------------------------------------------------------------------------------------------------------- State and engine setup: + private CompetitionRobotV1 robot; + // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; private double targetHeading; + public double collectLock = Math.toRadians(-90); + public double backDropLock = Math.toRadians(90); public double power; private double currentHeading; @@ -27,10 +30,39 @@ public class CompetitionTeleOpState extends CyberarmState { private double lastError = 0; ElapsedTime timer = new ElapsedTime(); + // ---------------------------------------------------------------------------------------------------- collector / depositor variables: + public static float leftOpen = 0; + public static float leftClose = 0; + public static float rightOpen = 0; + public static float rightClose = 0; + public float collectorPosLeft; + public float collectorPosRight; + public boolean lbsVar2; + public boolean rbsVar2; + public boolean bVar2; - private long lastCheckedTime; + //---------------------------------------------------------------------------------------------------------------- Drivetrain Variables: + private boolean lbsVar1; + private double drivePower = 1; + public double rx; - public CompetitionTeleOpState(PrototypeRobot robot) { + // ------------------------------------------------------------------------------------------------------------------- Slider Variables: + private int maxExtension = 2000; + private int minExtension = 0; + public boolean depositMode = false; + + // ---------------------------------------------------------------------------------------------------------------Arm control Variables: + public String armPos = "collect"; + public static float shoulderCollect = 0; + public static float shoulderDeposit = 0; + public static float shoulderPassive = 0; + public static float elbowCollect = 0; + public static float elbowDeposit = 0; + public static float elbowPassive = 0; + + + + public CompetitionTeleOpState(CompetitionRobotV1 robot) { this.robot = robot; } @@ -38,27 +70,75 @@ public class CompetitionTeleOpState extends CyberarmState { // } // --------------------------------------------------------------------------------------------------------- Slider control function private void SliderTeleOp() { - if (engine.gamepad2.right_trigger != 0) { - if (robot.lift.getCurrentPosition() >= maxExtension) { - robot.lift.setPower(0); - } else if (robot.lift.getCurrentPosition() >= maxExtension - 200) { - robot.lift.setPower(0.35); - } else { - robot.lift.setPower(engine.gamepad2.right_trigger); - } - } else if (engine.gamepad2.left_trigger != 0) { - if (robot.lift.getCurrentPosition() <= minExtension) { - robot.lift.setPower(0); - } else if (robot.lift.getCurrentPosition() < 350) { - robot.lift.setPower(-0.3); + if (depositMode) { + if (engine.gamepad2.right_trigger != 0) { + if (robot.lift.getCurrentPosition() >= maxExtension) { + robot.lift.setPower(0); + } else if (robot.lift.getCurrentPosition() >= maxExtension - 200) { + robot.lift.setPower(0.35); + } else { + robot.lift.setPower(engine.gamepad2.right_trigger); + } + } else if (engine.gamepad2.left_trigger != 0) { + + if (robot.lift.getCurrentPosition() <= minExtension) { + robot.lift.setPower(0); + } else if (robot.lift.getCurrentPosition() < 350) { + robot.lift.setPower(-0.3); + } else { + robot.lift.setPower(-engine.gamepad2.left_trigger); + } } else { - robot.lift.setPower(-engine.gamepad2.left_trigger); + robot.lift.setPower(0); } } else { robot.lift.setPower(0); } } + + + public void DriveTrainTeleOp () { + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + + } + boolean lbs1 = engine.gamepad1.left_stick_button; + if (lbs1 && !lbsVar1) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbsVar1 = lbs1; + + double y = -engine.gamepad1.left_stick_y; + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + // angle math to make things field oriented + double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); + + // joystick math to find the approximate power across each wheel for a movement + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + + // 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.frontLeft.setPower(frontLeftPower * drivePower); + robot.frontRight.setPower(frontRightPower * drivePower); + } + public double angleWrap(double radians) { while (radians > Math.PI) { radians -= 2 * Math.PI; @@ -79,6 +159,76 @@ public class CompetitionTeleOpState extends CyberarmState { double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); return output; } + public void ClawControlTeleOp() { + boolean b2 = engine.gamepad2.b; + if (b2 && !bVar2) { + if (collectorPosLeft == leftClose && collectorPosRight == rightClose) { + collectorPosLeft = leftOpen; + collectorPosRight = rightOpen; + } else { + collectorPosLeft = leftClose; + collectorPosRight = rightClose; + } + } + bVar2 = b2; + + 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; + } + + public void ArmPosControl(){ + + if (engine.gamepad2.a){ + armPos = "collect"; + depositMode = false; + } else if (engine.gamepad2.x){ + armPos = "passive"; + depositMode = false; + } else if (engine.gamepad2.y){ + armPos = "deposit"; + depositMode = true; + } + + if (armPos == "collect"){ + if (robot.lift.getCurrentPosition() >= 1){ + robot.lift.setPower(-0.3); + } else { + robot.lift.setPower(0); + robot.shoulder.setPosition(shoulderCollect); + robot.elbow.setPosition(elbowCollect); + } + } + if (armPos == "passive"){ + if (robot.lift.getCurrentPosition() >= 1){ + robot.lift.setPower(-0.3); + } else { + robot.lift.setPower(0); + robot.shoulder.setPosition(shoulderPassive); + robot.elbow.setPosition(elbowPassive); + } + } + if (armPos == "deposit"){ + robot.shoulder.setPosition(shoulderDeposit); + robot.elbow.setPosition(elbowDeposit); + } + } @Override public void init() { @@ -86,6 +236,7 @@ public class CompetitionTeleOpState extends CyberarmState { @Override public void exec() { + // ---------------------------------------------------------------------------------------------------------- Game Pad 1, drivetrain currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); @@ -101,15 +252,15 @@ public class CompetitionTeleOpState extends CyberarmState { } // drivetrain - robot.driveTrainTeleOp(); + DriveTrainTeleOp(); if (engine.gamepad1.b){ headingLock = true; - targetHeading = robot.backDropLock; + targetHeading = backDropLock; } if (engine.gamepad1.x){ headingLock = true; - targetHeading = robot.collectLock; + targetHeading = collectLock; } if (engine.gamepad1.a){ headingLock = true; @@ -119,33 +270,12 @@ public class CompetitionTeleOpState extends CyberarmState { headingLock = false; } -// if (engine.gamepad2.a) { -// robot.armPosition = 0; -// robot.startOfSequencerTime = System.currentTimeMillis(); -// } else if (engine.gamepad2.x) { -// robot.armPosition = 1; -// robot.startOfSequencerTime = System.currentTimeMillis(); -// } else if (engine.gamepad2.b) { -// robot.armPosition = 2; -// robot.startOfSequencerTime = System.currentTimeMillis(); -// } else if (engine.gamepad2.y) { -// robot.armPosition = 3; -// robot.startOfSequencerTime = System.currentTimeMillis(); -// } -// -// robot.depositor.setPosition(robot.depositorPos); -// robot.collector.setPosition(robot.collectorPos); -// -// -// // drivetrain -// robot.driveTrainTeleOp(); - // lift -// SliderTeleOp(); - // collector depositor -// robot.CollectorToggle(); - // depositor toggle -// robot.DepositorToggle(); + // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift: + // ------------------------------------------------------------------------------------------------------------------- Lift Control: + SliderTeleOp(); + ClawControlTeleOp(); + ArmPosControl(); } @@ -155,9 +285,6 @@ public class CompetitionTeleOpState extends CyberarmState { engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("arm Pos", robot.armPosition); engine.telemetry.addData("old arm pos", robot.oldArmPosition); - engine.telemetry.addData("depositor pos", robot.depositorPos); - engine.telemetry.addData("collector pos", robot.collectorPos); - engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); engine.telemetry.addData("pid power", power); engine.telemetry.addData("heading Lock?", headingLock); engine.telemetry.addData("Kp", Kp);