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); + } + } +}