From a2ac7bf946623642a5a0716d2246d7a6a8bc055e Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Wed, 10 Jan 2024 20:04:44 -0600 Subject: [PATCH] worked on odometry which is broken, and i fixed teleOp controls for drivetrain --- .../Competition2BlueBackStage.java | 34 +++++----- .../CompetitionStates/ClawArmState.java | 1 + .../DriveToCoordinatesState.java | 16 +++-- .../Common/CompetitionRobotV1.java | 49 +++++++------ .../TeleOp/States/CompetitionTeleOpState.java | 68 +++++++++++++------ 5 files changed, 103 insertions(+), 65 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java index 9d776b8..03b110f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java @@ -22,7 +22,9 @@ public class Competition2BlueBackStage extends CyberarmEngine { public void init() { super.init(); robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.imu.resetYaw(); robot.leftClaw.setPosition(0.25); robot.rightClaw.setPosition(0.6); @@ -32,7 +34,7 @@ public class Competition2BlueBackStage extends CyberarmEngine { public void setup() { this.robot = new CompetitionRobotV1("burnsville audience blue"); this.robot.setup(); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0")); addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0")); @@ -41,9 +43,9 @@ public class Competition2BlueBackStage extends CyberarmEngine { addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1")); // drive to the left, center, or right spike mark -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0")); // place pixel addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1")); @@ -55,49 +57,49 @@ public class Competition2BlueBackStage extends CyberarmEngine { addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3")); // drive back and away from the spike mark (x,y) (1050, 1000) H = 0 -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0")); // drive to the middle truss (right version) (1130,980) H = -90 -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0")); // drive to hover mode to clear the middle truss addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1")); // drive under the middle truss across the field (right version) (1170,1080) H = -90 -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0")); // drive to deposit mode to place on back drop addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1")); // drive to back drop -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0")); // open claw to deposit gold addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1")); // drive back from backdrop -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0")); // close left claw addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1")); // drive to parking spot -// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0")); // addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0")); - addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0")); // arm to collect pos addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1")); 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 index 38df752..b712bc4 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java @@ -29,6 +29,7 @@ public class ClawArmState extends CyberarmState { @Override public void exec() { + robot.armTime = System.currentTimeMillis() - initTime; // odometry driving ALWAYS robot.DriveToCoordinates(); robot.OdometryLocalizer(); 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 e31e917..115aa74 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 @@ -1,11 +1,11 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionStates; +import android.util.Log; + import com.acmerobotics.dashboard.config.Config; -import com.arcrobotics.ftclib.controller.PIDController; 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; @Config @@ -21,8 +21,11 @@ public class DriveToCoordinatesState extends CyberarmState { public boolean posSpecific; public double maxXPower; public double maxYPower; + private String actionName; public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { + this.actionName = actionName; + this.robot = robot; this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); @@ -43,6 +46,9 @@ public class DriveToCoordinatesState extends CyberarmState { robot.yMaxPower = maxYPower; robot.xMaxPower = maxXPower; + Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget); + } @Override @@ -64,10 +70,8 @@ public class DriveToCoordinatesState extends CyberarmState { robot.DriveToCoordinates(); robot.OdometryLocalizer(); - if ((Math.abs(robot.backLeftPower) < 0.15 && - Math.abs(robot.backRightPower) < 0.15 && - Math.abs(robot.frontLeftPower) < .15 && - Math.abs(robot.frontRightPower) < 0.15)){ + if (Math.abs(robot.positionX - robot.xTarget) < 2 + && Math.abs(robot.positionY - robot.yTarget) < 2){ setHasFinished(true); } } 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 759a86c..b8a4f37 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -5,10 +5,12 @@ 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.DcMotorEx; 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.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.TouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; @@ -38,10 +40,11 @@ public class CompetitionRobotV1 extends Robot { // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: public OpenCvWebcam webcam1 = null; public WebcamName webCamName; - public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp; + public DcMotor frontLeft, frontRight, backLeft, backRight, lift, /*clawArm,*/ chinUp; + public DcMotorEx clawArm; public IMU imu; - public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo; + public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo, shootServo; public DistanceSensor customObject; public TouchSensor touchLeftArm, touchRightArm; @@ -97,14 +100,16 @@ public class CompetitionRobotV1 extends Robot { PIDController pidController; public double power; public String armPos; + public long armTime; + public int target; - public double p = 0.007, i = 0, d = 0.0001, f = 0; - public double shoulderCollect = 0.38; - public double shoulderDeposit = 0.36; - public double shoulderPassive = 0.8; - public double elbowCollect = 0.02; - public double elbowDeposit = 0; + public double p = 0.007, i = 0, d = 0.0001, f = 0; + public double shoulderCollect = 1; + public double shoulderDeposit = 1; + public double shoulderPassive = 1; + public double elbowCollect = 0.02; + public double elbowDeposit = 0; private HardwareMap hardwareMap; @@ -140,7 +145,7 @@ public class CompetitionRobotV1 extends Robot { backLeft = engine.hardwareMap.dcMotor.get("backLeft"); chinUp = engine.hardwareMap.dcMotor.get("chinUp"); lift = engine.hardwareMap.dcMotor.get("Lift"); - clawArm = engine.hardwareMap.dcMotor.get("clawArm"); + clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm"); frontRight.setDirection(DcMotorSimple.Direction.FORWARD); backRight.setDirection(DcMotorSimple.Direction.REVERSE); @@ -160,9 +165,6 @@ public class CompetitionRobotV1 extends Robot { 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); @@ -198,7 +200,7 @@ public class CompetitionRobotV1 extends Robot { leftClaw = hardwareMap.servo.get("leftClaw"); rightClaw = hardwareMap.servo.get("rightClaw"); chinUpServo = hardwareMap.servo.get("chinUpServo"); - + shootServo = hardwareMap.servo.get("shoot"); elbow.setDirection(Servo.Direction.REVERSE); @@ -365,24 +367,27 @@ public class CompetitionRobotV1 extends Robot { if (lift.getCurrentPosition() >= 20) { lift.setPower(-0.6); } else { - shoulder.setPosition(0.38); + shoulder.setPosition(shoulderCollect); target = 120; } } if (armPos.equals("search")) { - shoulder.setPosition(0.15); - target = 570; + shoulder.setPosition(0.48); + if (armTime > 400){ + target = 570; + } } - pidController.setPID(p, i, d); - int armPos = clawArm.getCurrentPosition(); - double pid = pidController.calculate(armPos, target); +// pidController.setPID(p, i, d); +// int armPos = clawArm.getCurrentPosition(); +// double pid = pidController.calculate(armPos, target); +// +// power = pid; - power = pid; - - clawArm.setPower(power); + clawArm.setTargetPosition(target); + clawArm.setPower(0.4); } } 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 4ee04c4..1f17c2b 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 @@ -20,9 +20,13 @@ public class CompetitionTeleOpState extends CyberarmState { private CompetitionRobotV1 robot; // ------------------------------------------------------------------------------------------------------------robot claw arm variables: private PIDController pidController; - public double p = 0.007, i = 0, d = 0.0001, f = 0; + public double p = 0.005, i = 0, d = 0.0001, f = 0; public int target = 0; + // ---------------------------------------------------------------------------------------------------------------------shoot variables: + public static double releasePos = 0.95; + public static double holdPos = 0.55 ; + // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; @@ -111,19 +115,16 @@ public class CompetitionTeleOpState extends CyberarmState { boolean lbs1 = engine.gamepad1.left_stick_button; if (lbs1 && !lbsVar1) { if (drivePower == 1) { - drivePower = 0.5; + drivePower = 0.35; } else { drivePower = 1; } } - lbsVar1 = lbs1; + lbsVar1 = lbs1 - if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){ - boost = engine.gamepad1.right_trigger + 1; - } - double x = -((engine.gamepad1.left_stick_x * 0.5) * boost); - double y = ((engine.gamepad1.left_stick_y * 0.5) * boost); + double x = -(engine.gamepad1.left_stick_x); + double y = (engine.gamepad1.left_stick_y); double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); // angle math to make things field oriented @@ -139,10 +140,10 @@ 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); - robot.backRight.setPower(-backRightPower); - robot.frontLeft.setPower(frontLeftPower); - robot.frontRight.setPower(frontRightPower); + 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) { @@ -211,6 +212,9 @@ public class CompetitionTeleOpState extends CyberarmState { } else if (engine.gamepad2.b) { armPos = "hover"; depositMode = true; + } else if (engine.gamepad2.x) { + armPos = "passive"; + depositMode = true; } else if (engine.gamepad2.dpad_left) { armPos = "lift up"; depositMode = true; @@ -230,7 +234,7 @@ public class CompetitionTeleOpState extends CyberarmState { robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); robot.chinUpServo.setPosition(chinUpServoDown); - target = 30; + target = 0; } } @@ -251,8 +255,18 @@ public class CompetitionTeleOpState extends CyberarmState { robot.elbow.setPosition(robot.elbowCollect); target = 120; } - } + + if (Objects.equals(armPos, "passive")) { + if (robot.lift.getCurrentPosition() >= 20) { + robot.chinUpServo.setPosition(chinUpServoDown); + robot.lift.setPower(-0.6); + } else { + robot.shoulder.setPosition(robot.shoulderPassive); + target = 570; + } + } + if (Objects.equals(armPos, "lift up")) { robot.shoulder.setPosition(robot.shoulderDeposit); robot.elbow.setPosition(robot.elbowDeposit); @@ -275,9 +289,10 @@ public class CompetitionTeleOpState extends CyberarmState { if (armPos.equals("reset")) { robot.shoulder.setPosition(robot.shoulderPassive); + target = -10000; if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) { robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); armPos = "collect"; } } @@ -290,11 +305,22 @@ public class CompetitionTeleOpState extends CyberarmState { pidController = new PIDController(p, i, d); robot.imu.resetYaw(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setTargetPosition(0); + robot.clawArm.setPower(0); + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } @Override public void exec() { + if (engine.gamepad2.start && engine.gamepad2.x){ + robot.shootServo.setPosition(releasePos); + } else { + robot.shootServo.setPosition(holdPos); + } + if (engine.gamepad2.dpad_up) { robot.chinUp.setPower(-1); @@ -334,16 +360,16 @@ public class CompetitionTeleOpState extends CyberarmState { DriveTrainTeleOp(); // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift: - pidController.setPID(p, i, d); - int armCurrentPos = robot.clawArm.getCurrentPosition(); - double pid = pidController.calculate(armCurrentPos, target); +// pidController.setPID(p, i, d); +// int armCurrentPos = robot.clawArm.getCurrentPosition(); +// double pid = pidController.calculate(armCurrentPos, target); if (armPos.equals("reset")){ - armPower = -0.2; + robot.clawArm.setPower(armPower); } else { - armPower = pid; + armPower = 0.4; // pid } - + robot.clawArm.setTargetPosition(target); robot.clawArm.setPower(armPower); // ------------------------------------------------------------------------------------------------------------------- Lift Control: