From f6d85000b95e641d91e3ca0c44ef1611b1c7a7b2 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 4 Jan 2024 17:57:21 -0600 Subject: [PATCH 1/2] Pizza bot is now fieldcentric and arm has been commented out. --- .../Common/SodiPizzaMinibotObject.java | 7 +- .../TeleOp/States/SodiPizzaTeleOPState.java | 349 ++++++++---------- 2 files changed, 162 insertions(+), 194 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java index 9deae07..25c736a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -19,7 +19,8 @@ public class SodiPizzaMinibotObject extends Robot { public HardwareMap hardwareMap; public DcMotor leftFront, rightFront, leftBack, rightBack; - public Servo shoulder, gripper, launcher; +// public Servo shoulder, gripper; + public Servo launcher; public IMU imu; public Rev2mDistanceSensor distSensor; private String string; @@ -70,8 +71,8 @@ public class SodiPizzaMinibotObject extends Robot { rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //Servo Defining - shoulder = engine.hardwareMap.servo.get("arm"); - gripper = engine.hardwareMap.servo.get("gripper"); +// shoulder = engine.hardwareMap.servo.get("arm"); +// gripper = engine.hardwareMap.servo.get("gripper"); launcher = engine.hardwareMap.servo.get("launcher"); //Distance Sensor diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java index bbd8167..1f65b1b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java @@ -26,7 +26,9 @@ public class SodiPizzaTeleOPState extends CyberarmState { public final double minInput = 0.1 /* <- Minimum input from stick to send command */; public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative; public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3; - private int armPos, objectPos; + private double lfPower, rfPower, lbPower, rbPower; + private float yTransitPercent, xTransitPercent, rotPercent, percentDenom; + private int objectPos; private boolean droneLaunched; private char buttonPressed; private GamepadChecker gp1checker, gp2checker; @@ -48,12 +50,6 @@ public class SodiPizzaTeleOPState extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Arm should be at Position ", armPos); - engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition()); - - engine.telemetry.addData("Button Pressed = ", buttonPressed); - - engine.telemetry.addLine(); engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition()); engine.telemetry.addData("Drone Launched?", droneLaunched); } @@ -77,15 +73,18 @@ public class SodiPizzaTeleOPState extends CyberarmState { gp2checker = new GamepadChecker(engine, engine.gamepad2); lastMoveTime = System.currentTimeMillis(); - armPos = 0; + lastDistRead = System.currentTimeMillis(); } @Override public void exec() { + + + if (Math.abs(engine.gamepad1.left_stick_y) < minInput && Math.abs(engine.gamepad1.left_stick_x) < minInput && - Math.abs(engine.gamepad1.right_stick_x) < minInput) /* <- input from ONLY left stick y means to move forward or backward */{ + Math.abs(engine.gamepad1.right_stick_x) < minInput){ drivePower = 0; robot.leftFront.setPower(drivePower); @@ -94,69 +93,37 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.rightBack.setPower(drivePower); } - if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5) { - if (Math.abs(engine.gamepad1.right_stick_x) > minInput && - Math.abs(engine.gamepad1.left_stick_y) > minInput) { - robot.rightFront.setPower(robot.leftFront.getPower() * 0.8); - robot.rightBack.setPower(robot.leftBack.getPower() * 0.8); - } - - } else if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 && - Math.abs(engine.gamepad1.left_stick_y) > minInput) { - - robot.leftFront.setPower(robot.rightFront.getPower() * 0.8); - robot.leftBack.setPower(robot.rightBack.getPower() * 0.8); + if (Math.abs(yTransitPercent) > 0.01) { + percentDenom = 100; } else { - - robot.leftFront.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(drivePower); + percentDenom = 0; } - if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ { + if (Math.abs(xTransitPercent) > 0.01) { - robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.imu.resetYaw(); + percentDenom = percentDenom + 100; } - if (Math.abs(engine.gamepad1.left_stick_y) > minInput && - Math.abs(engine.gamepad1.left_stick_x) < minInput && - armPos == 0/* && - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 && - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5*/) { + if (Math.abs(rotPercent) > 0.01) { - drivePower = engine.gamepad1.left_stick_y; - robot.leftFront.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(drivePower); + percentDenom = percentDenom + 100; } + yTransitPercent = engine.gamepad1.left_stick_y * 100; + xTransitPercent = engine.gamepad1.left_stick_x * 100; + rotPercent = engine.gamepad1.right_stick_x * -100; - if (Math.abs(engine.gamepad1.left_stick_x) > minInput && - armPos == 0) { + lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom); + robot.leftFront.setPower(lfPower); - drivePower = engine.gamepad1.left_stick_x; - robot.leftFront.setPower(-drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightBack.setPower(-drivePower); - } + rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom); + robot.rightFront.setPower(rfPower); - if (Math.abs(engine.gamepad1.right_stick_x) > minInput && - armPos == 0) { + lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom); + robot.leftBack.setPower(lbPower); - drivePower = engine.gamepad1.right_stick_x; - robot.leftFront.setPower(-drivePower); - robot.rightFront.setPower(drivePower); - robot.leftBack.setPower(-drivePower); - robot.rightBack.setPower(drivePower); - lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - } + rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom); + robot.rightBack.setPower(rbPower); if (engine.gamepad2.left_stick_button) { if (System.currentTimeMillis() - lastMoveTime >= 200) { @@ -182,140 +149,140 @@ public class SodiPizzaTeleOPState extends CyberarmState { } } - // This moves the arm to Collect position, which is at servo position 0.00. - if (engine.gamepad2.a && !engine.gamepad2.start) { - armPos = 1; - } - - if (armPos == 1) { - - buttonPressed = 'a'; - - if (Math.abs(drivePower) > 0.25) { - drivePower = 0.15; - robot.leftFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.rightBack.setPower(drivePower); - } else { - //if servo's position is greater than Collect position with a run-to tolerance of 0.05, - //decrement position at a rate of 0.05 per 150 milliseconds. - if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); - lastMoveTime = System.currentTimeMillis(); - } else if (System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(ARM_COLLECT); - armPos = 0; - } - - } - - } - //End of code for armPos = 1 - - // This moves the arm to Precollect position, which is at servo position 0.05. - if (engine.gamepad2.x) { - armPos = 2; - } - - if (armPos == 2) { - - buttonPressed = 'x'; - - if (Math.abs(drivePower) > 0.25) { - drivePower = 0.15; - robot.leftFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.rightBack.setPower(drivePower); - } else { - //if servo's position is greater than Precollect position with a run-to tolerance of 0.05, - //decrement position at a rate of 0.05 per 150 milliseconds. - if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); - lastMoveTime = System.currentTimeMillis(); - }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to. - else if (System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(ARM_PRECOLLECT); - armPos = 0; - } - - } - } - //End of code for armPos = 2 - - // This moves the arm to Deliver position, which is at servo position 0.28. - if (engine.gamepad2.b && !engine.gamepad2.start) { - armPos = 3; - } - - if (armPos == 3) { - - buttonPressed = 'b'; - - if (Math.abs(drivePower) > 0.25) { - drivePower = 0.15; - robot.leftFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.rightBack.setPower(drivePower); - } else { - //if servo's position is less than Deliver position with a run-to tolerance of 0.05, - //increment position at a rate of 0.05 per 150 milliseconds. - if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05); - lastMoveTime = System.currentTimeMillis(); - }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05, - //decrement position at a rate of 0.05 per 150 milliseconds. - else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); - lastMoveTime = System.currentTimeMillis(); - } else if (System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(ARM_DELIVER); - armPos = 0; - } - - } - } - //End of code for armPos = 3 - - // This moves the arm to Stow position, which is at servo position 0.72. - if (engine.gamepad2.y) { - armPos = 4; - } - - if (armPos == 4) { - - buttonPressed = 'y'; - - if (Math.abs(drivePower) > 0.25) { - drivePower = 0.15; - robot.leftFront.setPower(drivePower); - robot.leftBack.setPower(drivePower); - robot.rightFront.setPower(drivePower); - robot.rightBack.setPower(drivePower); - } else { - //if servo's position is less than Collect position with a run-to tolerance of 0.05, - //increment position at a rate of 0.05 per 150 milliseconds. - if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05); - lastMoveTime = System.currentTimeMillis(); - } - //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing. - else if (System.currentTimeMillis() - lastMoveTime >= 150) { - robot.shoulder.setPosition(ARM_STOW); - armPos = 0; - } - - } - } +// // This moves the arm to Collect position, which is at servo position 0.00. +// if (engine.gamepad2.a && !engine.gamepad2.start) { +// armPos = 1; +// } +// +// if (armPos == 1) { +// +// buttonPressed = 'a'; +// +// if (Math.abs(drivePower) > 0.25) { +// drivePower = 0.15; +// robot.leftFront.setPower(drivePower); +// robot.leftBack.setPower(drivePower); +// robot.rightFront.setPower(drivePower); +// robot.rightBack.setPower(drivePower); +// } else { +// //if servo's position is greater than Collect position with a run-to tolerance of 0.05, +// //decrement position at a rate of 0.05 per 150 milliseconds. +// if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); +// lastMoveTime = System.currentTimeMillis(); +// } else if (System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(ARM_COLLECT); +// armPos = 0; +// } +// +// } +// +// } +// //End of code for armPos = 1 +// +// // This moves the arm to Precollect position, which is at servo position 0.05. +// if (engine.gamepad2.x) { +// armPos = 2; +// } +// +// if (armPos == 2) { +// +// buttonPressed = 'x'; +// +// if (Math.abs(drivePower) > 0.25) { +// drivePower = 0.15; +// robot.leftFront.setPower(drivePower); +// robot.leftBack.setPower(drivePower); +// robot.rightFront.setPower(drivePower); +// robot.rightBack.setPower(drivePower); +// } else { +// //if servo's position is greater than Precollect position with a run-to tolerance of 0.05, +// //decrement position at a rate of 0.05 per 150 milliseconds. +// if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); +// lastMoveTime = System.currentTimeMillis(); +// }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to. +// else if (System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(ARM_PRECOLLECT); +// armPos = 0; +// } +// +// } +// } +// //End of code for armPos = 2 +// +// // This moves the arm to Deliver position, which is at servo position 0.28. +// if (engine.gamepad2.b && !engine.gamepad2.start) { +// armPos = 3; +// } +// +// if (armPos == 3) { +// +// buttonPressed = 'b'; +// +// if (Math.abs(drivePower) > 0.25) { +// drivePower = 0.15; +// robot.leftFront.setPower(drivePower); +// robot.leftBack.setPower(drivePower); +// robot.rightFront.setPower(drivePower); +// robot.rightBack.setPower(drivePower); +// } else { +// //if servo's position is less than Deliver position with a run-to tolerance of 0.05, +// //increment position at a rate of 0.05 per 150 milliseconds. +// if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05); +// lastMoveTime = System.currentTimeMillis(); +// }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05, +// //decrement position at a rate of 0.05 per 150 milliseconds. +// else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); +// lastMoveTime = System.currentTimeMillis(); +// } else if (System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(ARM_DELIVER); +// armPos = 0; +// } +// +// } +// } +// //End of code for armPos = 3 +// +// // This moves the arm to Stow position, which is at servo position 0.72. +// if (engine.gamepad2.y) { +// armPos = 4; +// } +// +// if (armPos == 4) { +// +// buttonPressed = 'y'; +// +// if (Math.abs(drivePower) > 0.25) { +// drivePower = 0.15; +// robot.leftFront.setPower(drivePower); +// robot.leftBack.setPower(drivePower); +// robot.rightFront.setPower(drivePower); +// robot.rightBack.setPower(drivePower); +// } else { +// //if servo's position is less than Collect position with a run-to tolerance of 0.05, +// //increment position at a rate of 0.05 per 150 milliseconds. +// if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05); +// lastMoveTime = System.currentTimeMillis(); +// } +// //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing. +// else if (System.currentTimeMillis() - lastMoveTime >= 150) { +// robot.shoulder.setPosition(ARM_STOW); +// armPos = 0; +// } +// +// } +// } //End of code for armPos = 4 - if (engine.gamepad2.dpad_left) { - robot.gripper.setPosition(GRIPPER_OPEN); - } else if (engine.gamepad2.dpad_right) { - robot.gripper.setPosition(GRIPPER_CLOSED); - } +// if (engine.gamepad2.dpad_left) { +// robot.gripper.setPosition(GRIPPER_OPEN); +// } else if (engine.gamepad2.dpad_right) { +// robot.gripper.setPosition(GRIPPER_CLOSED); +// } gp1checker.update(); gp2checker.update(); From 9a6487a36885d147365af8056ba86a8b62de4b57 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Thu, 4 Jan 2024 20:56:23 -0600 Subject: [PATCH 2/2] Updated Vexy/Yellow minibot (disabled code in autonomous drive states) --- .../minibots/yellow/YellowMinibot.java | 182 ++++++++++-------- .../minibots/yellow/states/Pilot.java | 107 +++++++--- .../minibots/yellow/states/Rotate.java | 62 +++--- .../minibots/yellow/states/StrafeMove.java | 34 ++-- .../minibots/yellow/states/TankMove.java | 76 ++++---- 5 files changed, 268 insertions(+), 193 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java index a7ff840..498ede1 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java @@ -3,8 +3,14 @@ package dev.cyberarm.minibots.yellow; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; @@ -14,46 +20,88 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class YellowMinibot { private final CyberarmEngine engine; - public final TimeCraftersConfiguration config; - public MotorEx leftFront, rightFront, leftBack, rightBack; - public MotorGroup left, right; - public IMU imu; - public CRServo droneLauncher; - public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing; + public TimeCraftersConfiguration config; + public final DcMotorEx backLeft, frontLeft, frontRight, backRight; + public final IMU imu; + public final Servo leftClaw, rightClaw, droneLatch; + public final CRServo arm; + public final TouchSensor armEndStopLeft, armEndStopRight; + + public double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0; + public int DRIVETRAIN_MOTOR_TICKS = 4; + public double DRIVETRAIN_MOTOR_GEAR_RATIO = 72.0; + public double DRIVETRAIN_MAX_VELOCITY_MM = 305.0; + public double LEFT_CLAW_CLOSED_POSITION = 0.0; + public double LEFT_CLAW_OPEN_POSITION = 0.0; + public double RIGHT_CLAW_CLOSED_POSITION = 0.0; + public double RIGHT_CLAW_OPEN_POSITION = 0.0; + public double DRONE_LATCH_INITIAL_POSITION = 0.0; + public double DRONE_LATCH_LAUNCH_POSITION = 0.0; + public YellowMinibot(CyberarmEngine engine) { this.engine = engine; - this.config = new TimeCraftersConfiguration("Vexy_2023"); + reloadConfig(); - leftFront = new MotorEx(engine.hardwareMap, "leftFront"); - rightFront = new MotorEx(engine.hardwareMap, "rightFront"); + backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft"); + frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft"); + frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight"); + backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight"); - leftBack = new MotorEx(engine.hardwareMap, "leftBack"); - rightBack = new MotorEx(engine.hardwareMap, "rightBack"); + backLeft.setDirection(DcMotorSimple.Direction.FORWARD); + frontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); - wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius; - distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio + backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - leftFront.setDistancePerPulse(distancePerTick); - rightFront.setDistancePerPulse(distancePerTick); - leftBack.setDistancePerPulse(distancePerTick); - rightBack.setDistancePerPulse(distancePerTick); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - left = new MotorGroup(leftFront, leftBack); - right = new MotorGroup(rightFront, rightBack); + leftClaw = engine.hardwareMap.servo.get("leftClaw"); /// Port 0 + rightClaw = engine.hardwareMap.servo.get("rightClaw"); /// Port 1 + arm = engine.hardwareMap.crservo.get("arm"); /// Port 2 + arm.setDirection(DcMotorSimple.Direction.REVERSE); + droneLatch = engine.hardwareMap.servo.get("droneLatch"); /// Port 3 - imu = engine.hardwareMap.get(IMU.class, "imu"); + armEndStopLeft = engine.hardwareMap.touchSensor.get("armEndStopLeft"); + armEndStopRight = engine.hardwareMap.touchSensor.get("armEndStopRight"); + + imu = engine.hardwareMap.get(IMU.class, "imu"); // Embedded | IC2 port 0 IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.RIGHT)); imu.initialize(parameters); + } - imuAngleOffset = 0; - initialFacing = facing(); + public void reloadConfig() { + this.config = new TimeCraftersConfiguration("cyberarm_Vexy_Yellow"); - droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5 + loadConstants(); + } + + public void loadConstants() { + DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value(); + DRIVETRAIN_MOTOR_TICKS = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value(); + DRIVETRAIN_MOTOR_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "motor_gear_ratio").value(); + DRIVETRAIN_MAX_VELOCITY_MM = config.variable("Robot", "Drivetrain_Tuning", "max_velocity_mm").value(); + + LEFT_CLAW_CLOSED_POSITION = config.variable("Robot", "LeftClaw_Tuning", "open_position").value(); + LEFT_CLAW_OPEN_POSITION = config.variable("Robot", "LeftClaw_Tuning", "closed_position").value(); + + + RIGHT_CLAW_OPEN_POSITION = config.variable("Robot", "RightClaw_Tuning", "open_position").value(); + RIGHT_CLAW_CLOSED_POSITION = config.variable("Robot", "RightClaw_Tuning", "closed_position").value(); + + DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLatch_Tuning", "initial_position").value(); + DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLatch_Tuning", "launch_position").value(); } public void standardTelemetry() { @@ -61,35 +109,45 @@ public class YellowMinibot { engine.telemetry.addLine("Motors"); engine.telemetry.addData( - "Left Front", + "Front Left", "Power: %.2f, Current: %.2f mAmp, Position: %d", - leftFront.motorEx.getPower(), - leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - leftFront.motorEx.getCurrentPosition()); + frontLeft.getPower(), + frontLeft.getCurrent(CurrentUnit.MILLIAMPS), + frontLeft.getCurrentPosition()); engine.telemetry.addData( - "Right Front", + "Front Right", "Power: %.2f, Current: %.2f mAmp, Position: %d", - rightFront.motorEx.getPower(), - rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - rightFront.motorEx.getCurrentPosition()); + frontRight.getPower(), + frontRight.getCurrent(CurrentUnit.MILLIAMPS), + frontRight.getCurrentPosition()); engine.telemetry.addData( - "Left Back", + "Back Left", "Power: %.2f, Current: %.2f mAmp, Position: %d", - leftBack.motorEx.getPower(), - leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - leftBack.motorEx.getCurrentPosition()); + backLeft.getPower(), + backLeft.getCurrent(CurrentUnit.MILLIAMPS), + backLeft.getCurrentPosition()); engine.telemetry.addData( - "Right Back", + "Back Right", "Power: %.2f, Current: %.2f mAmp, Position: %d", - rightBack.motorEx.getPower(), - rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - rightBack.motorEx.getCurrentPosition()); + backRight.getPower(), + backRight.getCurrent(CurrentUnit.MILLIAMPS), + backRight.getCurrentPosition()); engine.telemetry.addLine(); + engine.telemetry.addLine("Servos"); - engine.telemetry.addData("droneLauncher", droneLauncher.getPower()); + engine.telemetry.addData("left claw", leftClaw.getPosition()); + engine.telemetry.addData("right claw", rightClaw.getPosition()); + engine.telemetry.addData("arm", arm.getPower()); + engine.telemetry.addData("droneLatch", droneLatch.getPosition()); engine.telemetry.addLine(); + + engine.telemetry.addData("arm endstop left", armEndStopLeft.isPressed()); + engine.telemetry.addData("arm endstop right", armEndStopRight.isPressed()); + + engine.telemetry.addLine(); + } public void teleopTelemetry() { @@ -97,46 +155,4 @@ public class YellowMinibot { engine.telemetry.addLine(); } - - public double distanceMM(int ticks) { - return distancePerTick * ticks; - } - - - public double initialFacing() { - return initialFacing; - } - - public double facing() { - double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - - return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; - } - - public double heading() { - return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); -// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - } - - public double turnRate() { - return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED - } - - public boolean isBetween(double value, double min, double max) { - return value >= min && value <= max; - } - - // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 - public double angleDiff(double from, double to) { - double value = (to - from + 180); - - double fmod = (value - 0.0) % (360.0 - 0.0); - - return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; - } - - public double lerp(double min, double max, double t) - { - return min + (max - min) * t; - } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java index 6117c15..1591be6 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java @@ -1,40 +1,43 @@ package dev.cyberarm.minibots.yellow.states; +import com.qualcomm.robotcore.hardware.Gamepad; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; import dev.cyberarm.minibots.yellow.YellowMinibot; public class Pilot extends CyberarmState { final YellowMinibot robot; + private boolean leftClawOpen, rightClawOpen; public Pilot(YellowMinibot robot) { this.robot = robot; + + this.leftClawOpen = false; + this.rightClawOpen = false; } @Override public void exec() { - /// --- IMU Reset --- /// - if (engine.gamepad1.guide) { - robot.imu.resetYaw(); - } + drivetrain(); + armController(); + clawControllers(); + droneLatchController(); + } - /// --- DRONE --- /// - if (engine.gamepad1.y) { - robot.droneLauncher.setPower(1.0); - } else if (engine.gamepad1.a) { - robot.droneLauncher.setPower(0.0); - } - - /// --- DRIVE --- /// + private void drivetrain() + { // robot.left.set(engine.gamepad1.left_stick_y); // robot.right.set(engine.gamepad1.right_stick_y); // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric - double y = engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed; - double x = -engine.gamepad1.left_stick_x; - double rx = -engine.gamepad1.right_stick_x; + double y = -engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed; + double x = engine.gamepad1.left_stick_x; + double rx = engine.gamepad1.right_stick_x; double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); @@ -53,16 +56,72 @@ public class Pilot extends CyberarmState { double frontRightPower = (rotY - rotX - rx) / denominator; double backRightPower = (rotY + rotX - rx) / denominator; -// frontLeftPower *= 24.0; -// backLeftPower *= 24.0; -// frontRightPower *= 24.0; -// backRightPower *= 24.0; + double velocity = Utilities.unitToTicks( + robot.DRIVETRAIN_MOTOR_TICKS, + robot.DRIVETRAIN_MOTOR_GEAR_RATIO, + robot.DRIVETRAIN_WHEEL_DIAMETER_MM, + DistanceUnit.MM, + robot.DRIVETRAIN_MAX_VELOCITY_MM); - double maxPower = 1.0; + robot.backLeft.setVelocity(frontLeftPower * velocity); + robot.frontLeft.setVelocity(backLeftPower * velocity); + robot.frontRight.setVelocity(frontRightPower * velocity); + robot.backRight.setVelocity(backRightPower * velocity); + } - robot.leftFront.motorEx.setPower(frontLeftPower * maxPower); - robot.leftBack.motorEx.setPower(backLeftPower * maxPower); - robot.rightFront.motorEx.setPower(frontRightPower * maxPower); - robot.rightBack.motorEx.setPower(backRightPower * maxPower); + private void armController() + { + double armPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger; + if (armPower > 0) { + if (robot.armEndStopLeft.isPressed() || robot.armEndStopRight.isPressed()) + armPower = 0; + } + + robot.arm.setPower(armPower); + } + + private void clawControllers() + { + robot.leftClaw.setPosition(leftClawOpen ? robot.LEFT_CLAW_OPEN_POSITION : robot.LEFT_CLAW_CLOSED_POSITION); + robot.rightClaw.setPosition(rightClawOpen ? robot.RIGHT_CLAW_OPEN_POSITION : robot.RIGHT_CLAW_CLOSED_POSITION); + } + + private void droneLatchController() + {} + + @Override + public void telemetry() { + robot.standardTelemetry(); + robot.teleopTelemetry(); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad == engine.gamepad1) + { + switch (button) { + case "guide": + /// --- IMU Reset --- /// + robot.imu.resetYaw(); + break; + case "start": + robot.reloadConfig(); + break; + case "left_bumper": + leftClawOpen = !leftClawOpen; + break; + case "right_bumper": + rightClawOpen = !rightClawOpen; + break; + case "y": + /// DRONE LATCH /// + robot.droneLatch.setPosition(robot.DRONE_LATCH_LAUNCH_POSITION); + break; + case "a": + /// DRONE LATCH /// + robot.droneLatch.setPosition(robot.DRONE_LATCH_INITIAL_POSITION); + break; + } + } } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java index 3594496..f0a7e8e 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java @@ -47,40 +47,40 @@ public class Rotate extends CyberarmState { @Override public void exec() { - if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) { - robot.left.set(0); - robot.right.set(0); - - setHasFinished(true); - - return; - } - - if (runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - - setHasFinished(true); - } - - double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading); - double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0)); - - if (angleDiff > 0) { - robot.left.set(-velocity); - robot.right.set(velocity); - } else { - robot.left.set(velocity); - robot.right.set(-velocity); - } +// if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) { +// robot.left.set(0); +// robot.right.set(0); +// +// setHasFinished(true); +// +// return; +// } +// +// if (runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// +// setHasFinished(true); +// } +// +// double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading); +// double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0)); +// +// if (angleDiff > 0) { +// robot.left.set(-velocity); +// robot.right.set(velocity); +// } else { +// robot.left.set(velocity); +// robot.right.set(-velocity); +// } } @Override public void telemetry() { - engine.telemetry.addLine(); - engine.telemetry.addData("Robot Heading", robot.facing()); - engine.telemetry.addData("Robot Target Heading", heading); - engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading)); - engine.telemetry.addData("Robot Turn Rate", robot.turnRate()); +// engine.telemetry.addLine(); +// engine.telemetry.addData("Robot Heading", robot.facing()); +// engine.telemetry.addData("Robot Target Heading", heading); +// engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading)); +// engine.telemetry.addData("Robot Turn Rate", robot.turnRate()); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java index 95bdfe7..3d5d661 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java @@ -38,26 +38,26 @@ public class StrafeMove extends CyberarmState { @Override public void start() { - robot.leftFront.setTargetDistance(distanceMM); - - robot.left.setPositionTolerance(tolerance); - robot.right.setPositionTolerance(tolerance); - - double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity); - - robot.leftFront.set(motorVelocity); - robot.rightFront.set(-motorVelocity); - - robot.leftBack.set(-motorVelocity); - robot.rightBack.set(motorVelocity); +// robot.leftFront.setTargetDistance(distanceMM); +// +// robot.left.setPositionTolerance(tolerance); +// robot.right.setPositionTolerance(tolerance); +// +// double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity); +// +// robot.leftFront.set(motorVelocity); +// robot.rightFront.set(-motorVelocity); +// +// robot.leftBack.set(-motorVelocity); +// robot.rightBack.set(motorVelocity); } @Override public void exec() { - if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - setHasFinished(true); - } +// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// setHasFinished(true); +// } } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java index e797bfe..12ffe22 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java @@ -43,51 +43,51 @@ public class TankMove extends CyberarmState { @Override public void start() { - robot.left.resetEncoder(); - robot.right.resetEncoder(); - - robot.left.setTargetDistance(leftDistanceMM); - robot.right.setTargetDistance(rightDistanceMM); - - robot.left.setPositionTolerance(tolerance); - robot.right.setPositionTolerance(tolerance); - - robot.left.set(leftVelocity); - robot.right.set(rightVelocity); +// robot.left.resetEncoder(); +// robot.right.resetEncoder(); +// +// robot.left.setTargetDistance(leftDistanceMM); +// robot.right.setTargetDistance(rightDistanceMM); +// +// robot.left.setPositionTolerance(tolerance); +// robot.right.setPositionTolerance(tolerance); +// +// robot.left.set(leftVelocity); +// robot.right.set(rightVelocity); } @Override public void exec() { - if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) { - robot.left.set(0); - } - if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) { - robot.right.set(0); - } - - if ( - (robot.left.atTargetPosition() && robot.right.atTargetPosition()) || - (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) || - runTime() >= timeoutMS) { - robot.left.set(0); - robot.right.set(0); - setHasFinished(true); - } +// if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) { +// robot.left.set(0); +// } +// if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) { +// robot.right.set(0); +// } +// +// if ( +// (robot.left.atTargetPosition() && robot.right.atTargetPosition()) || +// (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) || +// runTime() >= timeoutMS) { +// robot.left.set(0); +// robot.right.set(0); +// setHasFinished(true); +// } } @Override public void telemetry() { - engine.telemetry.addLine(); - - engine.telemetry.addData("Left distance", robot.leftFront.getDistance()); - engine.telemetry.addData("Left position", robot.left.getPositions().get(0)); - engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0)); - engine.telemetry.addData("Left velocity", robot.left.getVelocity()); - engine.telemetry.addLine(); - - engine.telemetry.addData("Right distance", robot.rightFront.getDistance()); - engine.telemetry.addData("Right position", robot.right.getPositions().get(0)); - engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0)); - engine.telemetry.addData("Right velocity", robot.right.getVelocity()); +// engine.telemetry.addLine(); +// +// engine.telemetry.addData("Left distance", robot.leftFront.getDistance()); +// engine.telemetry.addData("Left position", robot.left.getPositions().get(0)); +// engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0)); +// engine.telemetry.addData("Left velocity", robot.left.getVelocity()); +// engine.telemetry.addLine(); +// +// engine.telemetry.addData("Right distance", robot.rightFront.getDistance()); +// engine.telemetry.addData("Right position", robot.right.getPositions().get(0)); +// engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0)); +// engine.telemetry.addData("Right velocity", robot.right.getVelocity()); } }