From f3f3d5408ef551c3f5658c553dd90dbac93eab34 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 4 Dec 2023 13:52:46 -0600 Subject: [PATCH 1/4] Untested RedCrab minibot controller --- .../dev/cyberarm/engine/V2/Utilities.java | 86 ++++++++ .../minibots/red_crab/RedCrabMinibot.java | 199 ++++++++++++++++++ .../red_crab/engines/RedCrabTeleOpEngine.java | 15 ++ .../minibots/red_crab/states/Pilot.java | 180 ++++++++++++++++ 4 files changed, 480 insertions(+) create mode 100644 TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java new file mode 100644 index 0000000..825df1a --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java @@ -0,0 +1,86 @@ +package dev.cyberarm.engine.V2; + +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class Utilities { + /*** + * The current heading of the robot as a full 360 degree value, instead of that half radian mess. + * @param imu IMU of rev hub or similar + * @param imuAngleOffset optional angle offset added to IMU value + * @return full range heading of the robot, in DEGREES. + */ + static public double facing(IMU imu, double imuAngleOffset) { + double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; + } + + static public double facing(IMU imu) { + return facing(imu, 0); + } + + static public double heading(double facing) { + return AngleUnit.normalizeRadians(-facing * Math.PI / 180.0); + } + + static public double turnRate(IMU imu) { + return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; + } + + /*** + * + * @param value value to check with + * @param min minimum value + * @param max maximum value + * @return true if value is between min and max. inclusive. + */ + static public boolean isBetween(double value, double min, double max) { + return value >= min && value <= max; + } + + static public boolean isBetween(int value, int min, int max) { + return value >= min && value <= max; + } + + // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 + + /*** + * The angular difference between two angles + * **NOTE** flip flopped from and to values may result in continuous inversion of the angle difference (180 to -180 for example) + * @param from angle in DEGREES + * @param to angle in DEGREES + * @return Angular difference two angles in DEGREES + */ + static 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; + } + + /*** + * Linear interpolation + * @param min minimum value + * @param max maximum value + * @param t factor + * @return + */ + static public double lerp(double min, double max, double t) + { + return min + (max - min) * t; + } + + /*** + * Calculates motor angle in ticks + * @param motorTicksPerRevolution + * @param gearRatio + * @param angleInDegrees + * @return Angle in motor ticks + */ + static public int motorAngle(int motorTicksPerRevolution, double gearRatio, double angleInDegrees) { + return (int) (angleInDegrees / (360.0 / (motorTicksPerRevolution * gearRatio))); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java new file mode 100644 index 0000000..2aa2ee5 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -0,0 +1,199 @@ +package dev.cyberarm.minibots.red_crab; + +import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class RedCrabMinibot { + /// CLAW ARM /// + public static final int ClawArm_STOW = 0; + public static final int ClawArm_DEPOSIT = 1; + public static final int ClawArm_COLLECT = 2; + + /// TUNING CONSTANTS /// + public static final double DRIVETRAIN_MAX_SPEED = 0.5; + public static final double WINCH_MAX_SPEED = 0.5; + + public static final double CLAW_ARM_STOW_ANGLE = 0.0; + public static final double CLAW_ARM_DEPOSIT_ANGLE = 120.0; + public static final double CLAW_ARM_COLLECT_ANGLE = 150.0; + + public static final double CLAW_WRIST_STOW_POSITION = 0.5; + public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.5; + public static final double CLAW_WRIST_COLLECT_POSITION = 0.5; + + public static final double CLAW_LEFT_CLOSED_POSITION = 0.5; + public static final double CLAW_LEFT_OPEN_POSITION = 0.5; + public static final double CLAW_RIGHT_CLOSED_POSITION = 0.5; + public static final double CLAW_RIGHT_OPEN_POSITION = 0.5; + + public static final double DRONE_LATCH_INITIAL_POSITION = 0.5; + public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5; + public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; + + public static final double HOOK_ARM_STOW_POSITION = 0.0; + public static final double HOOK_ARM_UP_POSITION = 0.5; + + /// MOTOR CONSTANTS /// + public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; + public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72; + + /// HARDWARE /// + public final IMU imu; + public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm; + public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; + + final CyberarmEngine engine; + + public RedCrabMinibot() { + engine = CyberarmEngine.instance; + + /// IMU /// + /// ------------------------------------------------------------------------------------ /// + imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0 + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + imu.initialize(parameters); + + /// DRIVE TRAIN /// + /// ------------------------------------------------------------------------------------ /// + frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ?? + frontRight = new MotorEx(engine.hardwareMap, "frontRight"); // | Ctrl|Ex Hub, Port: ?? + backLeft = new MotorEx(engine.hardwareMap, "backLeft"); // | Ctrl|Ex Hub, Port: ?? + backRight = new MotorEx(engine.hardwareMap, "backRight"); // | Ctrl|Ex Hub, Port: ?? + + /// --- (SOFT) RESET MOTOR ENCODERS + frontLeft.resetEncoder(); + frontRight.resetEncoder(); + backLeft.resetEncoder(); + backRight.resetEncoder(); + + /// --- MOTOR DIRECTIONS + frontLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + frontRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + backLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + + /// --- MOTOR BRAKING MODE + /// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control + frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE)); + + /// WINCH /// + /// ------------------------------------------------------------------------------------ /// + winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? + + /// --- (SOFT) MOTOR ENCODER RESET + winch.resetEncoder(); + + /// --- MOTOR DIRECTION + /// --- NOTE: Unknown if FORWARD or REVERSE is correct + winch.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + + /// CLAW and Co. /// + /// ------------------------------------------------------------------------------------ /// + clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ?? + clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Ctrl|Ex Hub, Port: ?? + leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Ctrl|Ex Hub, Port: ?? + rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Ctrl|Ex Hub, Port: ?? + + /// --- Claw Arm Motor + /// --- --- (SOFT) RESET MOTOR ENCODER + clawArm.resetEncoder(); + /// --- --- DIRECTION + clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + /// --- --- BRAKING + /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 + clawArm.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + + /// --- Claws + /// --- --- Wrist + clawWrist.setDirection(Servo.Direction.FORWARD); +// clawWrist.setPosition(CLAW_WRIST_INITIAL_POSITION); + /// --- --- Left + leftClaw.setDirection(Servo.Direction.FORWARD); +// leftClaw.setPosition((CLAW_LEFT_CLOSED_POSITION)); + /// --- --- Right + leftClaw.setDirection(Servo.Direction.FORWARD); +// leftClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION)); + + /// DRONE LATCH /// + droneLatch = engine.hardwareMap.servo.get("droneLatch"); + droneLatch.setDirection(Servo.Direction.FORWARD); +// droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION); + + /// HOOK ARM /// + hookArm = engine.hardwareMap.servo.get("hookArm"); + hookArm.setDirection(Servo.Direction.FORWARD); +// hookArm.setPosition(HOOK_ARM_STOW_POSITION); + } + + public void standardTelemetry() { + engine.telemetry.addLine(); + + engine.telemetry.addLine("Motors"); + engine.telemetry.addData( + "Front Left", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + frontLeft.motorEx.getPower(), + frontLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + frontLeft.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Front Right", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + frontRight.motorEx.getPower(), + frontRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + frontRight.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Back Left", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + backLeft.motorEx.getPower(), + backLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + backLeft.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Back Right", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + backRight.motorEx.getPower(), + backRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + backRight.motorEx.getCurrentPosition()); + + engine.telemetry.addLine(); + engine.telemetry.addData( + "Winch", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + winch.motorEx.getPower(), + winch.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + winch.motorEx.getCurrentPosition()); + engine.telemetry.addLine(); + + engine.telemetry.addData( + "Claw Arm", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + clawArm.motorEx.getPower(), + clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + clawArm.motorEx.getCurrentPosition()); + + engine.telemetry.addLine(); + engine.telemetry.addLine("Servos"); + engine.telemetry.addData("Claw Wrist", clawWrist.getPosition()); + engine.telemetry.addData("Left Claw", leftClaw.getPosition()); + engine.telemetry.addData("Right Claw", rightClaw.getPosition()); + engine.telemetry.addLine(); + engine.telemetry.addData("Drone Latch", droneLatch.getPosition()); + engine.telemetry.addLine(); + engine.telemetry.addData("Hook Arm", hookArm.getPosition()); + + engine.telemetry.addLine(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java new file mode 100644 index 0000000..fadb4d8 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -0,0 +1,15 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.Pilot; + +@TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT") +public class RedCrabTeleOpEngine extends CyberarmEngine { + @Override + public void setup() { + addState(new Pilot(new RedCrabMinibot())); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java new file mode 100644 index 0000000..36ba10f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -0,0 +1,180 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class Pilot extends CyberarmState { + private final RedCrabMinibot robot; + + private boolean leftClawOpen = false; + private boolean rightClawOpen = false; + private int clawArmPosition = RedCrabMinibot.ClawArm_STOW; + private boolean hookArmUp = false; + private boolean droneLaunchAuthorized = false; + private boolean droneLaunchRequested = false; + private double droneLastLaunchRequestStartMS = 0; + + public Pilot(RedCrabMinibot robot) { + this.robot = robot; + } + + @Override + public void exec() { + drivetrain(); + +// clawArmAndWristController(); +// clawController(); +// droneLatchController(); +// hookArmController(); +// winchController(); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad == engine.gamepad1) { + switch (button) { + case "y": + droneLaunchRequested = true; + droneLastLaunchRequestStartMS = runTime(); + break; + case "x": + hookArmUp = true; + break; + case "b": + hookArmUp = false; + break; + case "guide": + robot.imu.resetYaw(); + break; + case "left_bumper": + leftClawOpen = !leftClawOpen; + break; + case "right_bumper": + rightClawOpen = !rightClawOpen; + break; + case "dpad_up": + clawArmPosition = RedCrabMinibot.ClawArm_COLLECT; + break; + case "dpad_down": + clawArmPosition = RedCrabMinibot.ClawArm_DEPOSIT; + break; + case "dpad_left": + case "dpad_right": + clawArmPosition = RedCrabMinibot.ClawArm_STOW; + break; + } + } + } + + @Override + public void buttonUp(Gamepad gamepad, String button) { + if (gamepad == engine.gamepad1) { + switch (button) { + case "y": + droneLaunchRequested = false; + droneLastLaunchRequestStartMS = runTime(); + break; + } + } + } + + @Override + public void telemetry() { + robot.standardTelemetry(); + } + + private void drivetrain() { + // 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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + // Rotate the movement direction counter to the bot's rotation + double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); + double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); + + rotX = rotX * 1.1; // Counteract imperfect strafing + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + double maxPower = RedCrabMinibot.DRIVETRAIN_MAX_SPEED; + + robot.frontLeft.motorEx.setPower(frontLeftPower * maxPower); + robot.backLeft.motorEx.setPower(backLeftPower * maxPower); + robot.frontRight.motorEx.setPower(frontRightPower * maxPower); + robot.backRight.motorEx.setPower(backRightPower * maxPower); + } + + private void clawArmAndWristController() { + switch (clawArmPosition) { + case RedCrabMinibot.ClawArm_STOW: + robot.clawArm.setTargetPosition(Utilities.motorAngle( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_STOW_ANGLE)); + + robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION); + break; + case RedCrabMinibot.ClawArm_DEPOSIT: + robot.clawArm.setTargetPosition(Utilities.motorAngle( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE)); + + robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION); + break; + case RedCrabMinibot.ClawArm_COLLECT: + robot.clawArm.setTargetPosition(Utilities.motorAngle( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE)); + + robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION); + break; + + } + } + + private void clawController() { + robot.leftClaw.setPosition((leftClawOpen ? RedCrabMinibot.CLAW_LEFT_OPEN_POSITION : RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION)); + robot.rightClaw.setPosition((rightClawOpen ? RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION : RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION)); + } + + private void droneLatchController() { + if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS) + droneLaunchAuthorized = true; + + if (droneLaunchAuthorized) { + robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION); + } + } + + private void hookArmController() { + robot.hookArm.setPosition((hookArmUp ? RedCrabMinibot.HOOK_ARM_UP_POSITION : RedCrabMinibot.HOOK_ARM_STOW_POSITION)); + } + + private void winchController() { + if (engine.gamepad1.right_trigger > 0) { + robot.winch.motorEx.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + } else if (engine.gamepad1.left_trigger > 0) { + robot.winch.motorEx.setPower(engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + } else { + robot.winch.motorEx.setPower(0); + } + } +} From 7449fd7f09ca897b32aa42578f49ec4726293e92 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 4 Dec 2023 14:13:05 -0600 Subject: [PATCH 2/4] Possibly fix claw arm non functional --- .../java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java | 5 ++++- .../java/dev/cyberarm/minibots/red_crab/states/Pilot.java | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 2aa2ee5..484d057 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -19,8 +19,8 @@ public class RedCrabMinibot { /// TUNING CONSTANTS /// public static final double DRIVETRAIN_MAX_SPEED = 0.5; + public static final double CLAW_ARM_MAX_SPEED = 0.5; public static final double WINCH_MAX_SPEED = 0.5; - public static final double CLAW_ARM_STOW_ANGLE = 0.0; public static final double CLAW_ARM_DEPOSIT_ANGLE = 120.0; public static final double CLAW_ARM_COLLECT_ANGLE = 150.0; @@ -116,6 +116,9 @@ public class RedCrabMinibot { /// --- --- BRAKING /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 clawArm.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + /// --- --- Run Mode + clawArm.setRunMode(Motor.RunMode.PositionControl); + clawArm.setTargetPosition(0); /// --- Claws /// --- --- Wrist diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 36ba10f..0d086cb 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -148,6 +148,8 @@ public class Pilot extends CyberarmState { break; } + + robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED); } private void clawController() { From aee541b5408268dec444d600d88d796264f2188c Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 4 Dec 2023 19:57:49 -0600 Subject: [PATCH 3/4] robot code for odometry is wrote, still needs to be tuned, and redid the entire robot structure for organization and my sanity --- .../DriveToCoordinatesState.java | 55 ++ .../Engines/OdometryTestEngine.java | 23 + .../Engines/ProtoBotEngineSodi.java | 2 +- .../Autonomous/Engines/ServoTestEngine.java | 6 +- .../Engines/SodiPizzaAutoRedRightEngine.java | 8 +- .../{States => SodiStates}/ArmStateSodi.java | 5 +- .../AutoStateSample.java | 2 +- .../AutoStateScrimmage.java | 2 +- .../DepositorArmPosState.java | 3 +- .../MiniYAutonomousState.java | 2 +- .../ProtoBotStateSodi.java | 7 +- .../SodiPizzaAutoArmState.java | 2 +- .../SodiPizzaAutoFirstDriveState.java | 2 +- .../SodiPizzaAutoSecDriveState.java | 2 +- .../SodiPizzaAutoTurnState.java | 4 +- .../SodiPizzaWheelTest.java | 2 +- .../States/DriveToCoordinatesState.java | 82 --- .../Common/CompetitionRobotV1.java | 214 +++++++ .../CenterStage/Common/PrototypeRobot.java | 529 +++++++----------- .../TeleOp/States/CompetitionTeleOpState.java | 168 ++++++ .../States/PrototypeRobotDrivetrainState.java | 95 +++- .../TeleOp/States/XDrivetrainRobotState.java | 5 +- .../TeleOp/States/headingLockTeleOp.java | 4 +- 23 files changed, 739 insertions(+), 485 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/ArmStateSodi.java (90%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/AutoStateSample.java (91%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/AutoStateScrimmage.java (98%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/DepositorArmPosState.java (99%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/MiniYAutonomousState.java (72%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/ProtoBotStateSodi.java (93%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/SodiPizzaAutoArmState.java (88%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/SodiPizzaAutoFirstDriveState.java (97%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/SodiPizzaAutoSecDriveState.java (90%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/SodiPizzaAutoTurnState.java (95%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/{States => SodiStates}/SodiPizzaWheelTest.java (98%) delete mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java 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 new file mode 100644 index 0000000..da091ba --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -0,0 +1,55 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +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; + +public class DriveToCoordinatesState extends CyberarmState { + + CompetitionRobotV1 robot; + public double xTarget; + public double yTarget; + public double hTarget; + public boolean posAchieved = false; + + 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(); + } + + @Override + public void exec() { + robot.hTarget = hTarget; + robot.yTarget = yTarget; + robot.xTarget = xTarget; + + if (posAchieved){ +// setHasFinished(true); + } else { + robot.OdometryLocalizer(); + 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; + } + } + } + + + @Override + 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("aux encoder", robot.odometerA.getCurrentPosition()); + engine.telemetry.addData("left encoder", robot.odometerL.getCurrentPosition()); + engine.telemetry.addData("right encoder", robot.odometerR.getCurrentPosition()); + engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + } +} 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 new file mode 100644 index 0000000..53e28be --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java @@ -0,0 +1,23 @@ +package org.timecrafters.CenterStage.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "odo test and tune") + +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"*/)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java index c147779..9964f7f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; //import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.timecrafters.CenterStage.Common.ProtoBotSodi; -import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; +import org.timecrafters.CenterStage.Autonomous.SodiStates.ProtoBotStateSodi; import dev.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java index 9fd1f9e..49ef095 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java @@ -1,13 +1,9 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import org.timecrafters.CenterStage.Autonomous.States.AutoStateScrimmage; -import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState; +import org.timecrafters.CenterStage.Autonomous.SodiStates.AutoStateScrimmage; import org.timecrafters.CenterStage.Common.PrototypeRobot; -import org.timecrafters.CenterStage.Common.ServoTestRobot; -import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; import dev.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java index 2c05058..b7debf1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java @@ -2,14 +2,10 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoSecDriveState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; +import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoTurnState; import dev.cyberarm.engine.V2.CyberarmEngine; -import dev.cyberarm.engine.V2.CyberarmState; @Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "") public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java similarity index 90% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java index bea0a49..21ddbf1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java @@ -1,11 +1,8 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; /**BEGAN WITH 43 LINES**/ -import com.arcrobotics.ftclib.hardware.motors.Motor; - import org.timecrafters.CenterStage.Common.ProtoBotSodi; -import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java similarity index 91% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java index 1be2687..501ea3d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.PrototypeRobot; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java index d0ebc50..2f8b426 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import com.qualcomm.robotcore.hardware.DcMotor; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java similarity index 99% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java index ca8688c..6ebfecf 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java @@ -1,6 +1,5 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; -import org.timecrafters.CenterStage.Common.PrototypeRobot; import org.timecrafters.CenterStage.Common.ServoTestRobot; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java similarity index 72% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java index a7e909b..4263946 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java similarity index 93% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java index 5384597..d156607 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java @@ -1,9 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; - -import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Servo; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import dev.cyberarm.engine.V2.CyberarmState; import org.timecrafters.CenterStage.Common.ProtoBotSodi; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java similarity index 88% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java index f9651a3..5f2c6a2 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java similarity index 97% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java index 87556d7..7ebce28 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java similarity index 90% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java index 453d17c..e5b7d8a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java similarity index 95% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java index 0823dc9..f3fe252 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java @@ -1,10 +1,8 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; -import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java index 87bcf10..bd816cb 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java deleted file mode 100644 index 5eb7c00..0000000 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.timecrafters.CenterStage.Autonomous.States; - -import com.arcrobotics.ftclib.controller.PIDController; - -import org.timecrafters.CenterStage.Common.PrototypeRobot; - -import dev.cyberarm.engine.V2.CyberarmState; - -public class DriveToCoordinatesState extends CyberarmState { - - private final boolean stateDisabled; - PrototypeRobot robot; - private PIDController driveXPidController; - private PIDController driveYPidController; - private PIDController driveHPidController; - private double pidX; - private double pidY; - private double pidH; - - public double xTarget; - public double yTarget; - public double hTarget; - - public double currentVelocityX; - public double currentVelocityY; - public double currentVelocityH; - - public double targetVelocityFL; - public double targetVelocityBL; - public double targetVelocityFR; - public double targetVelocityBR; - - - public static double Xp = 0, Xi = 0, Xd = 0; // p = 0.02 i = 0.09 d = 0 - public static double Yp = 0, Yi = 0, Yd = 0;// p = 0.03 i = 0.07 d = 0 - public static double Hp = 0, Hi = 0, Hd = 0;// p = 0.03 i = 0.07 d = 0 - - public DriveToCoordinatesState(PrototypeRobot robot, String groupName, String actionName) { - this.robot = robot; - driveXPidController = new PIDController(Xp, Xi, Xd); - driveYPidController = new PIDController(-Yp, -Yi, -Yd); - driveHPidController = new PIDController(-Hp, -Hi, -Hd); - this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; - 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(); - } - - @Override - public void start() { - //add variables that need to be reinitillized - } - - @Override - public void exec() { - // PID functions to come up with the velocity to get to the final point. - driveXPidController.setPID(Xp, Xi, Xd); - pidX = driveXPidController.calculate(robot.positionX, xTarget); - driveYPidController.setPID(Yp, Yi, Yd); - pidY = (driveYPidController.calculate(robot.positionY, yTarget)); - driveHPidController.setPID(Hp, Hi, Hd); - pidH = (driveYPidController.calculate(robot.positionH, hTarget)); - - // PID robot Velocity Ratios - currentVelocityX = robot.MaxVelocityForward * pidX; - currentVelocityY = robot.MaxStrafeVelocity * pidY; - currentVelocityH = robot.MaxRotationalVelocity * pidH; - - // Kinematic Formulas with plugged in velocities to solve for wheel velocities. - targetVelocityFL = currentVelocityX - currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityBL = currentVelocityX + currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityBR = currentVelocityX - currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityFR = currentVelocityX + currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - -// robot.frontLeft.setVelocity((targetVelocityFL / robot.R)); -// robot.backLeft.setVelocity((targetVelocityBL / robot.R)); -// robot.backRight.setVelocity((targetVelocityBR / robot.R)); -// robot.frontRight.setVelocity((targetVelocityFR / robot.R)); - -// 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 new file mode 100644 index 0000000..39a2a59 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -0,0 +1,214 @@ +package org.timecrafters.CenterStage.Common; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + + +public class CompetitionRobotV1 extends Robot { + // --------------------------------------------------------------------------------------------------- engine and state setup variables: + private String string; + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + // HardwareMap setup + + public DcMotor frontLeft, frontRight, backLeft, backRight, lift; + public DcMotor odometerR, odometerL, odometerA; + + public IMU imu; + public Servo shoulder, elbow, leftClaw, rightClaw; + + // ---------------------------------------------------------------------------------------------------- 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; + private double drivePower = 1; + + public double xMultiplier = 1; + public double yMultiplier = 1; + public double positionX = 0; + public double positionY = 0; + public double positionH = 0; + public double xTarget; + public double yTarget; + public double hTarget; + + public int currentRightPosition = 0; + public int currentLeftPosition = 0; + public int currentAuxPosition = 0; + public int oldRightPosition = 0; + public int oldLeftPosition = 0; + public int oldAuxPosition = 0; + public double rx; + public final static double L = 23.425; // distance between left and right encoder in cm + final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm + public final static double R = 4.5; // wheel radius in cm + final static double N = 8192; // encoder ticks per revolution (REV encoder) + + // heading math variables for pid with imu + public double integralSum = 0; + public double targetHeading; + public final double cm_per_tick = (2 * Math.PI * R) / N; + 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; + public int oldArmPosition = 0; + public float DEPOSITOR_SHOULDER_IN; + public float DEPOSITOR_SHOULDER_OUT; + public float DEPOSITOR_ELBOW_IN; + public float DEPOSITOR_ELBOW_OUT; + public float COLLECTOR_SHOULDER_IN; + public float COLLECTOR_SHOULDER_PASSIVE; + public float COLLECTOR_SHOULDER_OUT; + public float COLLECTOR_ELBOW_IN; + public float COLLECTOR_ELBOW_PASSIVE; + public float COLLECTOR_ELBOW_OUT; + private HardwareMap hardwareMap; + + + + public CompetitionRobotV1(String string) { + this.engine = engine; + setup(); + this.string = string; + } + + public void initConstants() { + DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); + DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); + DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); + DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value(); + COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value(); + COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value(); + COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value(); + COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); + COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); + COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); + } + + @Override + public void setup() { + System.out.println("Bacon: " + this.string); + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + //----------------------------------------------------------------------------------------------------------------------------MOTORS + frontRight = engine.hardwareMap.dcMotor.get("frontRight"); + frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); + backRight = engine.hardwareMap.dcMotor.get("backRight"); + backLeft = engine.hardwareMap.dcMotor.get("backLeft"); + lift = engine.hardwareMap.dcMotor.get("Lift"); + odometerL = engine.hardwareMap.dcMotor.get("leftodo"); + odometerR = engine.hardwareMap.dcMotor.get("rightodo"); + odometerA = engine.hardwareMap.dcMotor.get("auxodo"); + + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + lift.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setDirection(DcMotorSimple.Direction.FORWARD); + odometerL.setDirection(DcMotorSimple.Direction.REVERSE); + odometerA.setDirection(DcMotorSimple.Direction.FORWARD); + + odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_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); + + configuration = new TimeCraftersConfiguration("Blue Crab"); + + initConstants(); + + + //-----------------------------------------------------------------------------------------------------------------------------SERVO + shoulder = hardwareMap.servo.get("shoulder"); + elbow = hardwareMap.servo.get("elbow"); + leftClaw = hardwareMap.servo.get("leftClaw"); + rightClaw = hardwareMap.servo.get("leftClaw"); + + + + } + + 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() { + + // update positions + oldRightPosition = currentRightPosition; + oldLeftPosition = currentLeftPosition; + oldAuxPosition = currentAuxPosition; + + currentRightPosition = -odometerR.getCurrentPosition(); + currentLeftPosition = -odometerL.getCurrentPosition(); + currentAuxPosition = odometerA.getCurrentPosition(); + + int dnl1 = currentLeftPosition - oldLeftPosition; + int dnr2 = currentRightPosition - oldRightPosition; + int dna3 = currentAuxPosition - oldAuxPosition; + + // the robot has turned and moved a tiny bit between two measurements + double dtheta = cm_per_tick * (dnr2 - dnl1) / L; + double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; + double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); + + // the small movement of the bot gets added to the field coordinates + double theta = positionH + (dtheta / 2.0); + positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; + positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; + positionH += dtheta; + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 9ccf95e..717e60e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.Common; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.drivebase.HDrive; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -10,32 +11,21 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.Library.Robot; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmEngine; - +@Config public class PrototypeRobot extends Robot { - -// public double integralSum = 0; -// double Kp = 0; -// double Ki = 0; -// double Kd = 0; -// public double lastError = 0; - public double PIDrx; - public double targetHeading; public boolean headingLock = false; + public double collectLock = Math.toRadians(-90); public double backDropLock = Math.toRadians(90); - ElapsedTime timer = new ElapsedTime(); public int armPosition = 0; - public boolean stateFinished; public long startOfSequencerTime; public int oldArmPosition = 0; - public long waitTime; - public double servoWaitTime; - public double servoSecPerDeg = 0.14/60; public float DEPOSITOR_SHOULDER_IN; public float DEPOSITOR_SHOULDER_OUT; public float DEPOSITOR_ELBOW_IN; @@ -46,56 +36,56 @@ public class PrototypeRobot extends Robot { public float COLLECTOR_ELBOW_IN; public float COLLECTOR_ELBOW_PASSIVE; public float COLLECTOR_ELBOW_OUT; - public float lastSetPosShoulder; - public float lastSetPosElbow; - public float currentSetPosShoulder; - public float currentSetPosElbow; + + // hardware map private HardwareMap hardwareMap; public DcMotor frontLeft, frontRight, backLeft, backRight, lift; public DcMotor odometerR, odometerL, odometerA; public IMU imu; public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor, collector; - private HDrive xDrive; private String string; + + // robot geometry constants for odometry ----------------------------------------------------------------------------------------------- + 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; private double drivePower = 1; public double xMultiplier = 1; public double yMultiplier = 1; - public double positionX; - public double positionY; - public double positionH; + public double positionX = 0; + public double positionY = 0; + public double positionH = 0; + public double xTarget; + public double yTarget; + public double hTarget; - // robot geometry constants for odometry ----------------------------------------------------------------------------------------------- public int currentRightPosition = 0; public int currentLeftPosition = 0; public int currentAuxPosition = 0; public int oldRightPosition = 0; public int oldLeftPosition = 0; public int oldAuxPosition = 0; - public double globalPositionX; - public double globalPositionY; - public double globalPositionH; - public double localPositionX; - public double localPositionY; - public double localPositionH; - + public double rx; public final static double L = 23.425; // distance between left and right encoder in cm final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm public final static double R = 4.5; // wheel radius in cm final static double N = 8192; // encoder ticks per revolution (REV encoder) - public final double MaxVelocityForward = 40; - public final double MaxStrafeVelocity = 34; - public final double MaxRotationalVelocity = 20; + // heading math variables for pid with imu + public double integralSum = 0; + public double targetHeading; + + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + + // collector / depositor variables private boolean lbsVar1; private boolean lbsVar2; private boolean rbsVar2; public float depositorPos; public float collectorPos; - public double rx; - - - public final double cm_per_tick = (2 * Math.PI * R) / N; private CyberarmEngine engine; @@ -108,7 +98,7 @@ public class PrototypeRobot extends Robot { this.string = string; } - public void initConstants(){ + public void initConstants() { DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); @@ -119,7 +109,7 @@ public class PrototypeRobot extends Robot { COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); - } + } @Override public void setup() { @@ -135,7 +125,9 @@ public class PrototypeRobot extends Robot { backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); lift = engine.hardwareMap.dcMotor.get("Lift"); - odometerR = engine.hardwareMap.dcMotor.get("odometerR"); + odometerL = engine.hardwareMap.dcMotor.get("leftodo"); + odometerR = engine.hardwareMap.dcMotor.get("rightodo"); + odometerA = engine.hardwareMap.dcMotor.get("auxodo"); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -146,8 +138,14 @@ public class PrototypeRobot extends Robot { backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); lift.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setDirection(DcMotorSimple.Direction.FORWARD); + odometerL.setDirection(DcMotorSimple.Direction.REVERSE); + odometerA.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -166,8 +164,6 @@ public class PrototypeRobot extends Robot { //SERVO depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); depositorElbow = hardwareMap.servo.get("depositor_elbow"); - collectorShoulder = hardwareMap.servo.get("collector_shoulder"); - collectorElbow = hardwareMap.servo.get("collector_elbow"); depositor = hardwareMap.servo.get("depositor"); collector = hardwareMap.servo.get("collector"); @@ -176,305 +172,166 @@ public class PrototypeRobot extends Robot { collectorElbow.setPosition(COLLECTOR_ELBOW_IN); collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); collector.setPosition(0F); - - - } - public void driveTrainTeleOp() { - 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 - - if (headingLock){ - ; - } else if (headingLock == false){ - rx = engine.gamepad1.right_stick_x; - } - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - - // angle math to make things field oriented - double heading = -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. - backLeft.setPower(backLeftPower * drivePower); - backRight.setPower(backRightPower * drivePower); - frontLeft.setPower(frontLeftPower * drivePower); - frontRight.setPower(frontRightPower * drivePower); - } - - public void ShoulderServoWaitTime(){ - - servoWaitTime = servoSecPerDeg * (Math.abs(lastSetPosShoulder - currentSetPosShoulder)); - - } - - public void ElbowServoWaitTime(){ - - servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow))); - - } - public void OdometryLocalizer(){ - - if (Math.toDegrees(positionH) > 360){ - positionH -= 360; - } - - globalPositionX = localPositionX; - globalPositionY = localPositionY; - globalPositionH = localPositionH; - - - // update positions - oldRightPosition = currentRightPosition; - oldLeftPosition = currentLeftPosition; - oldAuxPosition = currentAuxPosition; - - currentRightPosition = -odometerR.getCurrentPosition(); - currentLeftPosition = -odometerL.getCurrentPosition(); - currentAuxPosition = odometerA.getCurrentPosition(); - - int dnl1 = currentLeftPosition - oldLeftPosition; - int dnr2 = currentRightPosition - oldRightPosition; - int dna3 = currentAuxPosition - oldAuxPosition; - - // the robot has turned and moved a tiny bit between two measurements - double dtheta = cm_per_tick * (dnr2 - dnl1) / L; - double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; - double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); - - // the small movement of the bot gets added to the field coordinates - double theta = positionH + (dtheta / 2.0); - positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; - positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; - positionH += dtheta; - - } - -// public double PIDControl(double reference, double current){ -// double error = reference - current; -// integralSum += error * timer.seconds(); -// double derivative = (error - lastError) / timer.seconds(); -// -// timer.reset(); -// -// double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); -// return output; -// } - - 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 ArmSequences(){ - switch (armPosition) { - case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos - switch (oldArmPosition) { - case 0: - // transfer - case 1: - // driving - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 0; - } - } - case 2: - // collect - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 0; - } - } - case 3: - // deposit - depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met - depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { - oldArmPosition = 0; - } - } - } - } + public void driveTrainTeleOp () { + boolean lbs1 = engine.gamepad1.left_stick_button; + if (lbs1 && !lbsVar1) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; } + } + lbsVar1 = lbs1; -// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 1; -// } -// } -// break; -// case 1: -// // drive pos -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2100) { -// oldArmPosition = 1; -// } -// } -// break; -// case 3: -// // deposit -// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met -// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { -// oldArmPosition = 1; -// } -// } -// } -// } -// break; -// } -// break; -// -// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 2; -// } -// } -// break; -// case 1: -// // driving -// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 2; -// } -// } -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 2; -// } -// } -// break; -// case 3: -// // deposit -// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met -// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { -// oldArmPosition = 2; -// } -// } -// } -// } -// break; -// } -// break; + double y = -engine.gamepad1.left_stick_y; + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing -// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 3; -// } -// } -// break; -// case 1: -// // driving -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 3; -// } -// } -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 3; -// } -// } -// break; -// case 3: -// // deposit -// break; -// } -// break; + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + // angle math to make things field oriented + double heading = -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. + backLeft.setPower(backLeftPower * drivePower); + backRight.setPower(backRightPower * drivePower); + frontLeft.setPower(frontLeftPower * drivePower); + frontRight.setPower(frontRightPower * drivePower); + } + + public void OdometryLocalizer () { + + // update positions + oldRightPosition = currentRightPosition; + oldLeftPosition = currentLeftPosition; + oldAuxPosition = currentAuxPosition; + + currentRightPosition = -odometerR.getCurrentPosition(); + currentLeftPosition = -odometerL.getCurrentPosition(); + currentAuxPosition = odometerA.getCurrentPosition(); + + int dnl1 = currentLeftPosition - oldLeftPosition; + int dnr2 = currentRightPosition - oldRightPosition; + int dna3 = currentAuxPosition - oldAuxPosition; + + // the robot has turned and moved a tiny bit between two measurements + double dtheta = cm_per_tick * (dnr2 - dnl1) / L; + double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; + double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); + + // the small movement of the bot gets added to the field coordinates + double theta = positionH + (dtheta / 2.0); + positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; + positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; + positionH += dtheta; } + + 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 double angleWrap ( double radians){ + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI) { + radians += 2 * Math.PI; + } + return radians; + } + + public double HeadingPIDControl ( double reference, double current){ + double error = angleWrap(current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Hp) + (derivative * Hd) + (integralSum * Hi); + return output; + } + + public double XPIDControl ( double reference, double current){ + double error = (current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Xp) + (derivative * Xd) + (integralSum * Xi); + return output; + } + + public double YPIDControl ( double reference, double current){ + double error = (current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Yp) + (derivative * Yd) + (integralSum * Yi); + return output; + } + + public void DriveToCoordinates () { + + // determine the velocities needed for each direction + // 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 denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); + + // field oriented math, (rotating the global field to the relative field) + double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading); + double rotX = pidY * Math.sin(heading) + pidX * Math.cos(heading); + + + // finding approximate power for each wheel. + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + + // apply my powers + frontLeft.setPower(frontLeftPower); + backLeft.setPower(backLeftPower); + backRight.setPower(backRightPower); + frontRight.setPower(frontRightPower); + } } -} \ No newline at end of file + + 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 new file mode 100644 index 0000000..e473faa --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -0,0 +1,168 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +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.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config + +public class CompetitionTeleOpState extends CyberarmState { + private PrototypeRobot robot; + private int maxExtension = 2000; + private int minExtension = 0; + public double integralSum = 0; + private double targetHeading; + + public double power; + private double currentHeading; + private boolean headingLock = false; + + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + + private long lastCheckedTime; + + public CompetitionTeleOpState(PrototypeRobot robot) { + this.robot = robot; + + } + + // } + // --------------------------------------------------------------------------------------------------------- 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); + } else { + robot.lift.setPower(-engine.gamepad2.left_trigger); + } + } else { + robot.lift.setPower(0); + } + } + public double angleWrap(double radians) { + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI){ + radians += 2 * Math.PI; + } + return radians; + } + + public double HeadingPIDControl(double reference, double current){ + double error = angleWrap(current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + return output; + } + + @Override + public void init() { + } + + @Override + public void exec() { + + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + if (engine.gamepad1.right_stick_button) { + robot.imu.resetYaw(); + } + + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + + } + + // drivetrain + robot.driveTrainTeleOp(); + + if (engine.gamepad1.b){ + headingLock = true; + targetHeading = robot.backDropLock; + } + if (engine.gamepad1.x){ + headingLock = true; + targetHeading = robot.collectLock; + } + if (engine.gamepad1.a){ + headingLock = true; + targetHeading = currentHeading; + } + if (engine.gamepad1.right_stick_x != 0){ + 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(); + + + } + + @Override + public void telemetry () { + engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition()); + 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); + engine.telemetry.addData("Ki", Ki); + engine.telemetry.addData("Kd", Kd); + } + } + diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index 326b411..a8fc2d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.TeleOp.States; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.robotcore.util.ElapsedTime; @@ -7,18 +8,26 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; +@Config public class PrototypeRobotDrivetrainState extends CyberarmState { private PrototypeRobot robot; private int maxExtension = 2000; private int minExtension = 0; public double integralSum = 0; - double Kp = 0; - double Ki = 0; - double Kd = 0; + private double targetHeading; + private double lastError = 0; ElapsedTime timer = new ElapsedTime(); + public double power; + private double currentHeading; + private boolean headingLock = false; + + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + private long lastCheckedTime; @@ -61,12 +70,8 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { return radians; } - private void HeadingLock(){ - double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - - } public double HeadingPIDControl(double reference, double current){ - double error = angleWrap(reference - current); + double error = angleWrap(current - reference); integralSum += error * timer.seconds(); double derivative = (error - lastError) / timer.seconds(); @@ -83,36 +88,64 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { @Override public void exec() { + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + if (engine.gamepad1.right_stick_button) { robot.imu.resetYaw(); } - 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(); + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + } - robot.depositor.setPosition(robot.depositorPos); - robot.collector.setPosition(robot.collectorPos); + // drivetrain + robot.driveTrainTeleOp(); + if (engine.gamepad1.b){ + headingLock = true; + targetHeading = robot.backDropLock; + } + if (engine.gamepad1.x){ + headingLock = true; + targetHeading = robot.collectLock; + } + if (engine.gamepad1.a){ + headingLock = true; + targetHeading = currentHeading; + } + if (engine.gamepad1.right_stick_x != 0){ + headingLock = false; + } - // drivetrain - robot.driveTrainTeleOp(); +// 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(); +// SliderTeleOp(); // collector depositor - robot.CollectorToggle(); +// robot.CollectorToggle(); // depositor toggle - robot.DepositorToggle(); +// robot.DepositorToggle(); } @@ -126,6 +159,14 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { 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("heading Lock?", headingLock); + engine.telemetry.addData("Kp", Kp); + engine.telemetry.addData("Ki", Ki); + engine.telemetry.addData("Kd", Kd); + engine.telemetry.addData("front left motor", robot.frontLeft.getCurrentPosition()); + engine.telemetry.addData("front right motor", robot.frontRight.getCurrentPosition()); + engine.telemetry.addData("back left motor", robot.backLeft.getCurrentPosition()); + engine.telemetry.addData("back right motor", robot.backRight.getCurrentPosition()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java index ffa943a..ea162d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java @@ -33,13 +33,10 @@ public class XDrivetrainRobotState extends CyberarmState { private double ArmlastError = 0; private boolean headingLock = false; - private long lastCheckedTime; - public XDrivetrainRobotState(XDrivetrainBot robot) { this.robot = robot; } - // } // --------------------------------------------------------------------------------------------------------- Slider control function public double angleWrap(double radians) { @@ -149,7 +146,7 @@ public class XDrivetrainRobotState extends CyberarmState { engine.telemetry.addData("heading lock?", headingLock); engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading)); engine.telemetry.addData("robot arm current pos ", robot.armMotor.getCurrentPosition()); - engine.telemetry.addData("arm pid power ", power); + engine.telemetry.addData("arm pid power ", HeadingPIDControl(targetHeading, currentHeading)); engine.telemetry.addData("p", ArmP); engine.telemetry.addData("i", ArmI); engine.telemetry.addData("d", ArmD); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java index 9cae281..610de49 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java @@ -54,7 +54,7 @@ public class headingLockTeleOp extends CyberarmState { timer.reset(); double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); - robot.PIDrx = output; +// robot.PIDrx = output; } // drivetrain @@ -82,7 +82,7 @@ public class headingLockTeleOp extends CyberarmState { public void telemetry () { engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("rx", robot.rx); - engine.telemetry.addData("PIDrx", robot.PIDrx); +// engine.telemetry.addData("PIDrx", robot.PIDrx); } } From 208d7c1513072aa6f4d79fa419a90c5af6b06774 Mon Sep 17 00:00:00 2001 From: scott Date: Mon, 4 Dec 2023 21:03:26 -0600 Subject: [PATCH 4/4] debugging --- .../cyberarm/minibots/red_crab/RedCrabMinibot.java | 14 +++++++------- .../cyberarm/minibots/red_crab/states/Pilot.java | 6 +++--- gradle/wrapper/gradle-wrapper.properties | 3 ++- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 484d057..06b7fa9 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -18,7 +18,7 @@ public class RedCrabMinibot { public static final int ClawArm_COLLECT = 2; /// TUNING CONSTANTS /// - public static final double DRIVETRAIN_MAX_SPEED = 0.5; + public static final double DRIVETRAIN_MAX_SPEED = 1.0; public static final double CLAW_ARM_MAX_SPEED = 0.5; public static final double WINCH_MAX_SPEED = 0.5; public static final double CLAW_ARM_STOW_ANGLE = 0.0; @@ -38,8 +38,8 @@ public class RedCrabMinibot { public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5; public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; - public static final double HOOK_ARM_STOW_POSITION = 0.0; - public static final double HOOK_ARM_UP_POSITION = 0.5; + public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8 + public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0 /// MOTOR CONSTANTS /// public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; @@ -78,10 +78,10 @@ public class RedCrabMinibot { backRight.resetEncoder(); /// --- MOTOR DIRECTIONS - frontLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); - frontRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); - backLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); - backRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); + backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); /// --- MOTOR BRAKING MODE /// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 0d086cb..7eaa9f5 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -27,10 +27,10 @@ public class Pilot extends CyberarmState { public void exec() { drivetrain(); -// clawArmAndWristController(); + clawArmAndWristController(); // clawController(); // droneLatchController(); -// hookArmController(); +// hookArmController(); // disabled for swrist debug // winchController(); } @@ -149,7 +149,7 @@ public class Pilot extends CyberarmState { } - robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED); + robot.clawArm.set(RedCrabMinibot. CLAW_ARM_MAX_SPEED); } private void clawController() { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fc..8da469e 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Mon Dec 04 18:52:45 CST 2023 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists