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 388c8ca..36a3aab 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 @@ -1,9 +1,12 @@ package dev.cyberarm.minibots.red_crab; +import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; @@ -15,6 +18,8 @@ import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.tfod.TfodProcessor; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; import java.sql.Time; @@ -30,47 +35,51 @@ public class RedCrabMinibot { public static final int ClawArm_COLLECT = 3; /// 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 CLAW_ARM_kP = 0.1; - public static final double CLAW_ARM_POSITION_TOLERANCE = 1.5; + public static double DRIVETRAIN_MAX_SPEED = 0.5; + + public static double CLAW_ARM_MAX_SPEED = 0.5; + public static double CLAW_ARM_kP = 0.1; + public static int CLAW_ARM_POSITION_TOLERANCE = 1; + public static double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 + public static double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 + public static double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; + public static double CLAW_ARM_COLLECT_ANGLE = 200.0; + public static final double WINCH_MAX_SPEED = 0.5; - public static final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 - public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 - public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; - public static final double CLAW_ARM_COLLECT_ANGLE = 200.0; - public static final double CLAW_WRIST_STOW_POSITION = 0.7; - public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64; - public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; - public static final double CLAW_WRIST_COLLECT_POSITION = 0.64; + public static double CLAW_WRIST_STOW_POSITION = 0.7; + public static double CLAW_WRIST_DEPOSIT_POSITION = 0.64; + public static double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; + public static double CLAW_WRIST_COLLECT_POSITION = 0.64; - public static final double CLAW_LEFT_CLOSED_POSITION = 0.2; - public static final double CLAW_LEFT_OPEN_POSITION = 0.5; - public static final double CLAW_RIGHT_CLOSED_POSITION = 0.77; - public static final double CLAW_RIGHT_OPEN_POSITION = 0.5; + public static double CLAW_LEFT_CLOSED_POSITION = 0.2; + public static double CLAW_LEFT_OPEN_POSITION = 0.5; + public static double CLAW_RIGHT_CLOSED_POSITION = 0.77; + public static 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.7; - public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; + public static double DRONE_LATCH_INITIAL_POSITION = 0.5; + public static double DRONE_LATCH_LAUNCH_POSITION = 0.7; + public static int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; - 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 + public static double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8 + public static double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0 /// MOTOR CONSTANTS /// - public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; - public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop + public static int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; + public static double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop /// HARDWARE /// public final IMU imu; - public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm; + public final MotorEx frontLeft, frontRight, backLeft, backRight, winch; + public final DcMotorEx clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; public final MotorGroup left, right; final CyberarmEngine engine; - public final TimeCraftersConfiguration config; + public TimeCraftersConfiguration config; + private final PIDController clawArmPIDController; public enum Path { LEFT, @@ -85,6 +94,7 @@ public class RedCrabMinibot { engine = CyberarmEngine.instance; config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + loadConstants(); if (autonomous) { tfod = TfodProcessor.easyCreateWithDefaults(); @@ -164,23 +174,23 @@ public class RedCrabMinibot { /// CLAW and Co. /// /// ------------------------------------------------------------------------------------ /// - clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ?? + clawArmPIDController = new PIDController(0, 0, 0); + clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("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(); + clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); /// --- --- DIRECTION - clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + clawArm.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); + clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- --- Run Mode - clawArm.setRunMode(Motor.RunMode.PositionControl); - clawArm.setPositionCoefficient(CLAW_ARM_kP); - clawArm.setPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); + clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); /// --- Claws @@ -205,6 +215,49 @@ public class RedCrabMinibot { // hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF: } + public void reloadConfig() { + config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + + loadConstants(); + } + + private void loadConstants() { + /// Drivetrain + RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").value(); + + /// CLAW ARM + RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value(); + RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value(); + RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value(); + RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value(); + RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); + RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); + + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value(); + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value(); + + /// CLAW WRIST + RedCrabMinibot.CLAW_WRIST_STOW_POSITION = config.variable("Robot", "ClawWrist_Tuning", "stow_position").value(); + RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "deposit_position").value(); + RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "float_collect_position").value(); + RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "collect_position").value(); + + /// CLAWS + RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_closed_position").value(); + RedCrabMinibot.CLAW_LEFT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_open_position").value(); + RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_closed_position").value(); + RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_open_position").value(); + + /// HOOK ARM + RedCrabMinibot.HOOK_ARM_STOW_POSITION = config.variable("Robot", "HookArm_Tuning", "stow_position").value(); + RedCrabMinibot.HOOK_ARM_UP_POSITION = config.variable("Robot", "HookArm_Tuning", "up_position").value(); + + /// DRONE LATCH + RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value(); + RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value(); + RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value(); + } + public void standardTelemetry() { engine.telemetry.addLine(); @@ -245,10 +298,11 @@ public class RedCrabMinibot { engine.telemetry.addData( "Claw Arm", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - clawArm.motorEx.getPower(), - clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - clawArm.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f", + clawArm.getPower(), + clawArm.getCurrent(CurrentUnit.MILLIAMPS), + clawArm.getCurrentPosition(), + clawArm.getVelocity()); engine.telemetry.addLine(); engine.telemetry.addLine("Servos"); @@ -265,4 +319,37 @@ public class RedCrabMinibot { engine.telemetry.addLine(); } + + public void controlClawArm() { + Action action = config.action("Robot", "ClawArm_Tuning"); + + double p = 0.0, i = 0.0, d = 0.0, f = 0.0; + double ticksInDegree = Utilities.motorAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1); + + for (Variable v : action.getVariables()) { + switch (v.name.trim()) { + case "kP": + p = v.value(); + break; + case "kI": + i = v.value(); + break; + case "kD": + d = v.value(); + break; + case "kF": // feedback + f = v.value(); + break; + } + } + + clawArmPIDController.setPID(p, i, d); + int armPos = clawArm.getCurrentPosition(); + double pid = clawArmPIDController.calculate(armPos, clawArm.getTargetPosition()); + double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; + + double power = pid + ff; + + clawArm.setPower(power); + } } 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 index 45d138e..6e59e91 100644 --- 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 @@ -4,12 +4,17 @@ 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.ClawArmTask; 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(false))); + RedCrabMinibot robot = new RedCrabMinibot(false); + + addTask(new ClawArmTask(robot)); + + addState(new Pilot(robot)); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java index ad5a9ad..1246343 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java @@ -1,5 +1,6 @@ package dev.cyberarm.minibots.red_crab.states; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; @@ -25,15 +26,16 @@ public class ClawArmMove extends CyberarmState { @Override public void start() { - robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); + robot.clawArm.setTargetPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); - - engine.blackboardSet("clawArmPower", power); } @Override public void exec() { - if (robot.clawArm.atTargetPosition() || runTime() >= timeoutMS) { + int tolerance = robot.clawArm.getTargetPositionTolerance(); + int position = robot.clawArm.getCurrentPosition(); + + if (Utilities.isBetween(position, position - tolerance, position + tolerance) || runTime() >= timeoutMS) { this.finished(); } } @@ -41,7 +43,7 @@ public class ClawArmMove extends CyberarmState { @Override public void telemetry() { engine.telemetry.addLine(); - engine.telemetry.addData("Motor Power", robot.clawArm.get()); + engine.telemetry.addData("Motor Power", robot.clawArm.getPower()); engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition()); engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition())); engine.telemetry.addData("Motor Target Position", Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java index d7b42c8..1e14eec 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java @@ -11,8 +11,6 @@ public class ClawArmTask extends CyberarmState { @Override public void exec() { - double power = engine.blackboardGetDouble("clawArmPower"); - - robot.clawArm.set(power); + robot.controlClawArm(); } } 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 4db680d..6ee06ee 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 @@ -69,6 +69,9 @@ public class Pilot extends CyberarmState { case "dpad_right": clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT; break; + case "start": + robot.reloadConfig(); + break; } } } @@ -164,9 +167,9 @@ public class Pilot extends CyberarmState { RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) { - robot.clawArm.set(0); - } else { - robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED); +// robot.clawArm.setPower(0); +// } else { +// robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED); } }