From fa54f5f2096f26af214418397137fab33800adbd Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 4 Mar 2023 12:42:43 -0600 Subject: [PATCH] Added FTCLib, implemented Arm PID(f) controller --- .../minibots/cyberarm/phoenix/Robot.java | 625 ++++++++++++++++++ .../cyberarm/phoenix/engines/TeleOp.java | 47 ++ .../phoenix/states/teleop/ArmController.java | 81 +++ build.common.gradle | 2 +- build.dependencies.gradle | 1 + 5 files changed, 755 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java new file mode 100644 index 0000000..2b14e55 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java @@ -0,0 +1,625 @@ +package org.timecrafters.minibots.cyberarm.phoenix; + +import android.annotation.SuppressLint; +import android.util.Log; + +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.Blinker; +import com.qualcomm.robotcore.hardware.CRServoImplEx; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoImplEx; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; +import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; + +import java.util.ArrayList; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.CopyOnWriteArrayList; +import java.util.concurrent.TimeUnit; + +public class Robot { + private static final String TAG = "Phoenix | Robot"; + public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm; +// public final ServoImplEx gripper; + public final IMU imu; + public LynxModule expansionHub; + + public final double imuAngleOffset, initialFacing; + public boolean armManuallyControlled = false; + public boolean automaticAntiTipActive = false; + public boolean hardwareFault = false; + public String hardwareFaultMessage = ""; + + private Status status = Status.OKAY, lastStatus = Status.OKAY; + private final CopyOnWriteArrayList reportedStatuses = new CopyOnWriteArrayList<>(); + private final ConcurrentHashMap motorVelocityError = new ConcurrentHashMap<>(); + private final ConcurrentHashMap motorVelocityLastTiming = new ConcurrentHashMap<>(); + private final ConcurrentHashMap motorTargetVelocity = new ConcurrentHashMap<>(); + + public enum ArmPosition { + COLLECT, + GROUND, + LOW, + MEDIUM, + HIGH + + } + + public enum Status { + OKAY, + MONITORING, + WARNING, + DANGER + } + + private final CyberarmEngine engine; + private TimeCraftersConfiguration configuration; + private final double radius, diameter; + + private final double wheelRadius, wheelGearRatio, armGearRatio; + private final int wheelTicksPerRevolution, armTicksPerRevolution; + + private final VuforiaLocalizer vuforia; + private final TFObjectDetector tfod; + + private boolean LEDStatusToggle = false; + private double lastLEDStatusAnimationTime = 0; + + public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) { + this.engine = engine; + this.configuration = configuration; + + radius = tuningConfig("field_localizer_robot_radius").value(); + diameter = radius * 2; + + imuAngleOffset = hardwareConfig("imu_angle_offset").value(); + + wheelRadius = tuningConfig("wheel_radius").value(); + wheelGearRatio = tuningConfig("wheel_gear_ratio").value(); + wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); + + armGearRatio = tuningConfig("arm_gear_ratio").value(); + armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); + + // FIXME: Rename motors in configuration + // Define hardware + backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left"); // MOTOR PORT: 2 - CONTROL HUB + frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB + + frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left"); // MOTOR PORT: 3 - CONTROL HUB + backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right"); // MOTOR PORT: 0 - CONTROL HUB + + // FIXME: Rename lift_drive to arm in hardware config + arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor"); // MOTOR PORT: 2 - EXPANSION HUB + +// gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ? + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + + // Configure hardware + // MOTORS + // DIRECTION + frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + + backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + + arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + + // RUNMODE + frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + arm.setTargetPosition(0); + arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // ZERO POWER BEHAVIOR + frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // MOTOR POWER + arm.setPower(tuningConfig("arm_automatic_power").value()); + + // IMU + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); + + imu.initialize(parameters); + + // Preserve our "initial" facing, since we transform it from zero. + initialFacing = facing(); + + // BulkRead from Hubs + for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + + if (!hub.isParent() && expansionHub == null) { + expansionHub = hub; + } + } + + // Set LED pattern + if (expansionHub != null) { + expansionHub.setPattern(ledPatternOkay()); + } + + // Webcam + vuforia = initVuforia(); + tfod = initTfod(); + + // Drive Encoder Error Setup + engine.blackboardSet("left_drive_error", 0); + engine.blackboardSet("right_drive_error", 0); + } + + private VuforiaLocalizer initVuforia() { + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value(); + parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + + return ClassFactory.getInstance().createVuforia(parameters); + } + + private TFObjectDetector initTfod() { + int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfodParameters.minResultConfidence = 0.75f; + tfodParameters.isModelTensorFlow2 = true; + tfodParameters.inputSize = 300; + TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + + tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel"); + + return tfod; + } + + public void reloadConfig() { + String name = configuration.getConfig().getName(); + configuration = new TimeCraftersConfiguration(name); + } + + public void standardTelemetry() { + engine.telemetry.addLine(); + + // STATUS + engine.telemetry.addLine("DATA"); + engine.telemetry.addData(" Robot Status", status); + engine.telemetry.addData(" Hardware Fault", hardwareFault); + engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage); + engine.telemetry.addLine(); + + // Motor Powers + engine.telemetry.addLine("Motor Powers"); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getPower()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getPower()); + + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getPower()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getPower()); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", arm.getPower()); + + engine.telemetry.addLine(); + + // Motor Positions + engine.telemetry.addLine("Motor Positions"); + engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition())); + engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition())); + + engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition())); + engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition())); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition())); + + engine.telemetry.addLine(); + + // Motor Target Positions + engine.telemetry.addLine("Motor Target Positions"); + engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition())); + engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition())); + + engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition())); + engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition())); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition())); + + // Motor Velocity + engine.telemetry.addLine("Motor Velocity"); + engine.telemetry.addData(" Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity())); + engine.telemetry.addData(" Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity())); + + engine.telemetry.addData(" Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity())); + engine.telemetry.addData(" Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity())); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity())); + + engine.telemetry.addLine(); + + // Motor Currents + engine.telemetry.addLine("Motor Currents (AMPS)"); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS)); + + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS)); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS)); + + engine.telemetry.addLine(); + + // Motor Directions + engine.telemetry.addLine("Motor Directions"); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getDirection()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getDirection()); + + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getDirection()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getDirection()); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", arm.getDirection()); + + engine.telemetry.addLine(); + + // Sensors / IMU + engine.telemetry.addLine("IMU"); + engine.telemetry.addData(" Facing (Degrees)", facing()); + engine.telemetry.addData(" Heading (Radians)", heading()); + engine.telemetry.addData(" Turn Rate", turnRate()); + engine.telemetry.addData(" Angle Offset (Degrees)", imuAngleOffset); + + engine.telemetry.addLine(); + } + + private ArrayList ledPatternStandby() { + final ArrayList steps = new ArrayList<>(); + + steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS)); + + return steps; + } + + private ArrayList ledPatternOkay() { + final ArrayList steps = new ArrayList<>(); + + steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS)); + + return steps; + } + + private ArrayList ledPatternMonitoring() { + final ArrayList steps = new ArrayList<>(); + + steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS)); + + return steps; + } + + private ArrayList ledPatternWarning() { + final ArrayList steps = new ArrayList<>(); + + steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS)); + + return steps; + } + + private ArrayList ledPatternDanger() { + final ArrayList steps = new ArrayList<>(); + + steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS)); + steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS)); + + return steps; + } + + public void reportStatus(Status status) { + reportedStatuses.add(status); + } + + public void update() { + for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) { + hub.clearBulkCache(); + } + + double voltage = getVoltage(); + if (voltage < 9.75) { + reportStatus(Status.DANGER); + hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)"; + + hardwareFault = true; + } + + status = Status.OKAY; + for (Status s : reportedStatuses) { + if (s.ordinal() > status.ordinal()) { + status = s; + } + } + + reportedStatuses.clear(); + + if (status != lastStatus) { + lastStatus = status; + + if (expansionHub != null) { + if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); } + if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); } + if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); } + if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); } + } + } + } + + public void stop() { + if (expansionHub != null) { + expansionHub.setPattern(ledPatternStandby()); + } + } + + public void armPosition(ArmPosition position) { + if (hardwareFault) { + return; + } + + reportStatus(Status.WARNING); + + switch (position) { + case COLLECT: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value())); + break; + + case GROUND: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value())); + break; + + case LOW: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value())); + break; + + case MEDIUM: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value())); + break; + + case HIGH: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value())); + break; + + default: + throw new RuntimeException("Unexpected arm position!"); + } + } + + // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 + public double angleDiff(double from, double to) { + double value = (to - from + 180); + + double fmod = (value - 0.0) % (360.0 - 0.0); + + return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; + } + + public double lerp(double min, double max, double t) + { + return min + (max - min) * t; + } + + public Status getStatus() { return status; } + + public double getRadius() { return radius; } + + public double getDiameter() { return diameter; } + + public double getVoltage() { + return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); + } + + public TFObjectDetector getTfod() { return tfod; } + + public VuforiaLocalizer getVuforia() { return vuforia; } + + public TimeCraftersConfiguration getConfiguration() { return configuration; } + + // For: Drive Wheels + public int unitToTicks(DistanceUnit unit, double distance) { + double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution)); + + double inches = unit.toInches(unit.fromUnit(unit, distance)); + + double ticks = fI * inches; + + return (int)ticks; + } + + // For: Drive Wheels + public double ticksToUnit(DistanceUnit unit, int ticks) { + // Convert to inches, then to unit. + double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution); + + return unit.fromUnit(DistanceUnit.INCH, inches); + } + + // For: Arm + public int angleToTicks(double angle) { + double d = (armGearRatio * armTicksPerRevolution) / 360.0; + + // Casting to float so that the int version of Math.round is used. + return Math.round((float)d * (float)angle); + } + + // For: Arm + public double ticksToAngle(int ticks) { + double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution); + + return oneDegree * ticks; + } + + public Variable hardwareConfig(String variableName) { + Action hardwareConfiguration = configuration.action("Robot", "Hardware"); + + for (Variable v : hardwareConfiguration.getVariables()) { + if (variableName.trim().equals(v.name)) { + return v; + } + } + + throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware"); + } + + public Variable tuningConfig(String variableName) { + Action action = configuration.action("Robot", "Tuning"); + + for (Variable v : action.getVariables()) { + if (variableName.trim().equals(v.name)) { + return v; + } + } + + throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning"); + } + + @SuppressLint("NewApi") + public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) { + Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName); + double proportional = 0, integral = 0, derivative = 0; + + for (Variable v : action.getVariables()) { + switch (v.name.trim()) { + case "proportional": + proportional = v.value(); + break; + case "integral": + integral = v.value(); + break; + case "derivative": + derivative = v.value(); + break; + } + } + + double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0)); + + double distance = motor.getTargetPosition() - motor.getCurrentPosition(); + + if (Math.abs(distance) < Math.abs(targetVelocity)) { + if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) { + targetVelocity = -distance; + } else { + targetVelocity = distance; + } + } + + double error = targetVelocity - motor.getVelocity(); + double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0); + + double kIntegral = error * interval; + double kDerivative = deltaError / interval; + + double kp = proportional * error; + double ki = integral * kIntegral; + double kd = derivative * kDerivative; + + motorVelocityError.put(motorName, error); + motorVelocityLastTiming.put(motorName, engine.getRuntime()); + + double newTargetVelocity = kp + ki + kd + targetVelocity; + + motor.setVelocity(newTargetVelocity); + + Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " + + distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional + + ", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd + + ", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks."); + } + + @SuppressLint("NewApi") + public void controlArmMotor(double targetVelocity) { +// double time = System.currentTimeMillis(); +// double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity); +// double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time); +// double deltaTime = (time - lastTiming) * 0.001; +// +// double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition(); +// double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity; +// +// double error = adjustedTargetVelocity - arm.getVelocity(); +// double kp = 0.9; +// +// newTargetVelocity += error * kp * deltaTime; +// +// motorTargetVelocity.put("Arm", newTargetVelocity); +// motorVelocityLastTiming.put("Arm", time); + +// arm.setVelocity(newTargetVelocity); + + arm.setPower(tuningConfig("arm_automatic_power").value()); + } + + public double initialFacing() { + return initialFacing; + } + + public double facing() { + double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; + } + + public double heading() { + return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); +// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + public double turnRate() { + return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED + } + + public boolean isBetween(double value, double min, double max) { + return value >= min && value <= max; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java new file mode 100644 index 0000000..b1ebad5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java @@ -0,0 +1,47 @@ +package org.timecrafters.minibots.cyberarm.phoenix.engines; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.minibots.cyberarm.phoenix.Robot; + +@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix") +public class TeleOp extends CyberarmEngine { + private Robot robot; + private TimeCraftersConfiguration configuration; + final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates"; + + @Override + public void setup() { + configuration = new TimeCraftersConfiguration(configurationName); + + robot = new Robot( + this, + configuration + ); + + robot.imu.resetYaw(); + + setupFromConfig( + robot.getConfiguration(), + "org.timecrafters.minibots.cyberarm.phoenix.states.teleop", + robot, + Robot.class, + actionsGroupName); + } + + @Override + public void loop() { + robot.update(); + + super.loop(); + + robot.standardTelemetry(); + } + + @Override + public void stop() { + robot.stop(); + + super.stop(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java new file mode 100644 index 0000000..97d718a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java @@ -0,0 +1,81 @@ +package org.timecrafters.minibots.cyberarm.phoenix.states.teleop; + +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; +import org.timecrafters.minibots.cyberarm.phoenix.Robot; + +public class ArmController extends CyberarmState { + private final Robot robot; + private final String groupName, actionName; + private PIDController pidController; + private double p = 0, i = 0, d = 0, f = 0; + private final double ticksInDegree = 700 / 180; + + public ArmController(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + pidController = new PIDController(p, i, d); + } + + @Override + public void exec() { + Action action = robot.getConfiguration().action("Robot", "Tuning_PIDF_Arm"); + + for (Variable v : action.getVariables()) { + switch (v.name.trim()) { + case "proportional": + p = v.value(); + break; + case "integral": + i = v.value(); + break; + case "derivative": + d = v.value(); + break; + case "feed": + f = v.value(); + break; + } + } + + pidController.setPID(p, i, d); + int armPos = robot.arm.getCurrentPosition(); + double pid = pidController.calculate(armPos, robot.arm.getTargetPosition()); + double ff = Math.cos(Math.toRadians(robot.arm.getTargetPosition() / ticksInDegree)) * f; + + double power = pid + ff; + + robot.arm.setPower(power); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad != engine.gamepad2) { + return; + } + + switch (button) { + case "guide": + robot.reloadConfig(); + break; + case "a": + robot.armPosition(Robot.ArmPosition.COLLECT); + break; + case "x": + robot.armPosition(Robot.ArmPosition.GROUND); + break; + case "b": + robot.armPosition(Robot.ArmPosition.LOW); + break; + case "y": + robot.armPosition(Robot.ArmPosition.MEDIUM); + break; + } + } +} diff --git a/build.common.gradle b/build.common.gradle index 2848cf3..a32ef4c 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -51,7 +51,7 @@ android { defaultConfig { signingConfig signingConfigs.debug applicationId 'com.qualcomm.ftcrobotcontroller' - minSdkVersion 23 + minSdkVersion 24 //noinspection ExpiredTargetSdkVersion targetSdkVersion 28 diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 5fe53f4..84c49a3 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -22,5 +22,6 @@ dependencies { implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.acmerobotics.roadrunner:core:0.5.6' implementation 'com.acmerobotics.dashboard:dashboard:0.4.8' + implementation 'org.ftclib.ftclib:core:2.1.1' }