diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java new file mode 100644 index 0000000..79cdadb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java @@ -0,0 +1,79 @@ +package org.timecrafters.minibots.cyberarm.phoenix; + +import com.qualcomm.robotcore.hardware.Servo; + +public class PositionalServoController { + final private Servo servo; + final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees; + + private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition; + private long lastUpdatedAt; + private boolean travelling = false; + + public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) { + this.servo = servo; + this.targetDegreesPerSecond = targetDegreesPerSecond; + this.servoDegreesPerSecond = servoDegreesPerSecond; + this.servoRangeInDegrees = servoRangeInDegrees; + + this.workingPosition = servo.getPosition(); + this.estimatedPosition = workingPosition; + this.lastEstimatedPosition = estimatedPosition; + this.lastUpdatedAt = System.currentTimeMillis(); + } + + public Servo getServo() { + return servo; + } + + public void update() { + double error = targetPosition - estimatedPosition; + double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0; + double estimatedTravel = servoDegreesPerSecond * delta; + double targetTravel = targetDegreesPerSecond * delta; + + if (targetTravel < estimatedTravel) { + estimatedTravel = targetTravel; + } + + if (travelling) { + this.lastEstimatedPosition = estimatedPosition; + + if (error < 0.0) { + this.estimatedPosition -= estimatedTravel; + } else { + this.estimatedPosition += estimatedTravel; + } + } + + if (error < 0.0 - estimatedTravel) { + workingPosition -= estimatedTravel; + travelling = true; + } else if (error > 0.0 + estimatedTravel) { + workingPosition += estimatedTravel; + travelling = true; + } else { + travelling = false; + } + + servo.setPosition(workingPosition); + this.lastUpdatedAt = System.currentTimeMillis(); + } + + public void setTargetPosition(double targetPosition) { + this.targetPosition = targetPosition; + } + + public double getEstimatedPosition() { + return estimatedPosition; + } + + public double getEstimatedAngle() { + return estimatedPosition * servoRangeInDegrees; + } + + private double lerp(double min, double max, double t) + { + return min + (max - min) * t; + } +} 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 index 2b14e55..bae4ced 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java @@ -3,18 +3,15 @@ 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.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoImplEx; import org.cyberarm.engine.V2.CyberarmEngine; import org.firstinspires.ftc.robotcore.external.ClassFactory; @@ -27,7 +24,6 @@ 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; @@ -37,6 +33,9 @@ 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 PositionalServoController leftRiserServoController, rightRiserServoController; + public final Servo cameraServo; + public final CRServo collectorLeftServo, collectorRightServo; // public final ServoImplEx gripper; public final IMU imu; public LynxModule expansionHub; @@ -52,6 +51,7 @@ public class Robot { private final ConcurrentHashMap motorVelocityError = new ConcurrentHashMap<>(); private final ConcurrentHashMap motorVelocityLastTiming = new ConcurrentHashMap<>(); private final ConcurrentHashMap motorTargetVelocity = new ConcurrentHashMap<>(); + private ArmPosition armTargetPosition; public enum ArmPosition { COLLECT, @@ -147,6 +147,33 @@ public class Robot { // MOTOR POWER arm.setPower(tuningConfig("arm_automatic_power").value()); + // SERVOS + // NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications + leftRiserServoController = new PositionalServoController( + engine.hardwareMap.servo.get("LowRiserLeft"), + 5.0, + 428.57142858, + 270.0 + ); // SERVER PORT: ? - ? HUB + + rightRiserServoController = new PositionalServoController( + engine.hardwareMap.servo.get("LowRiserRight"), + 5.0, + 428.57142858, + 270.0 + ); // SERVER PORT: ? - ? HUB + + cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB + + collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left"); // SERVER PORT: ? - ? HUB + collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB + + // SERVO DIRECTIONS + cameraServo.setDirection(Servo.Direction.REVERSE); + collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE); + collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD); + + // SENSORS // IMU IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( @@ -394,6 +421,8 @@ public class Robot { if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); } } } + + manageArmAndRiserServos(); } public void stop() { @@ -407,32 +436,67 @@ public class Robot { return; } + armTargetPosition = position; + reportStatus(Status.WARNING); + } - switch (position) { + private void manageArmAndRiserServos() { + boolean lowerServos = true; + + switch (armTargetPosition) { case COLLECT: - arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value())); + if (areRiserServosInLoweredPosition()) { + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value())); + } break; - case GROUND: - arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value())); - break; +// case GROUND: +// if (areRiserServosInLoweredPosition()) { +// arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value())); +// } +// break; + case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW case LOW: - arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value())); + if (areRiserServosInLoweredPosition()) { + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value())); + } break; case MEDIUM: - arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value())); + if (areRiserServosInLoweredPosition()) { + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value())); + } break; case HIGH: - arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value())); + double angleHigh = tuningConfig("arm_position_angle_high").value(); + arm.setTargetPosition(angleToTicks(angleHigh)); + + if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) { + lowerServos = false; + } break; default: throw new RuntimeException("Unexpected arm position!"); } + + if (lowerServos) { + leftRiserServoController.setTargetPosition(0.45); + rightRiserServoController.setTargetPosition(0.45); + } else { + leftRiserServoController.setTargetPosition(0.8); + rightRiserServoController.setTargetPosition(0.8); + } + + leftRiserServoController.update(); + rightRiserServoController.update(); + } + + private boolean areRiserServosInLoweredPosition() { + return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5; } // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 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 index 97d718a..4bf1c09 100644 --- 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 @@ -11,6 +11,7 @@ import org.timecrafters.minibots.cyberarm.phoenix.Robot; public class ArmController extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final Gamepad controller; private PIDController pidController; private double p = 0, i = 0, d = 0, f = 0; private final double ticksInDegree = 700 / 180; @@ -20,6 +21,8 @@ public class ArmController extends CyberarmState { this.groupName = groupName; this.actionName = actionName; + this.controller = engine.gamepad1; + pidController = new PIDController(p, i, d); } @@ -38,7 +41,7 @@ public class ArmController extends CyberarmState { case "derivative": d = v.value(); break; - case "feed": + case "feed": // feedback f = v.value(); break; } @@ -56,7 +59,7 @@ public class ArmController extends CyberarmState { @Override public void buttonDown(Gamepad gamepad, String button) { - if (gamepad != engine.gamepad2) { + if (gamepad != controller) { return; } @@ -64,17 +67,55 @@ public class ArmController extends CyberarmState { case "guide": robot.reloadConfig(); break; + + // Arm control case "a": robot.armPosition(Robot.ArmPosition.COLLECT); break; case "x": - robot.armPosition(Robot.ArmPosition.GROUND); + robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION break; case "b": - robot.armPosition(Robot.ArmPosition.LOW); + robot.armPosition(Robot.ArmPosition.MEDIUM); break; case "y": - robot.armPosition(Robot.ArmPosition.MEDIUM); + robot.armPosition(Robot.ArmPosition.HIGH); + break; + // Manual stepping arm + case "left_bumper": + robot.arm.setTargetPosition( + robot.arm.getCurrentPosition() - robot.angleToTicks(5.0) + ); + break; + case "right_bumper": + robot.arm.setTargetPosition( + robot.arm.getCurrentPosition() + robot.angleToTicks(5.0) + ); + break; + + // Collector control + case "dpad_up": + robot.collectorLeftServo.setPower(1); + robot.collectorRightServo.setPower(1); + break; + case "dpad_down": + robot.collectorLeftServo.setPower(-1); + robot.collectorRightServo.setPower(-1); + } + } + + @Override + public void buttonUp(Gamepad gamepad, String button) { + if (gamepad != controller) { + return; + } + + switch (button) { + // Collector control + case "dpad_up": + case "dpad_down": + robot.collectorLeftServo.setPower(0); + robot.collectorRightServo.setPower(0); break; } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java new file mode 100644 index 0000000..df57938 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java @@ -0,0 +1,118 @@ +package org.timecrafters.minibots.cyberarm.phoenix.states.teleop; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.minibots.cyberarm.phoenix.Robot; + +public class DriveController extends CyberarmState { + private final Robot robot; + private final String groupName, actionName; + + private final Gamepad controller; + private boolean robotSlowMode; + + public DriveController(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + this.controller = engine.gamepad1; + + this.robotSlowMode = true; + } + + @Override + public void exec() { + move(); + } + + @Override + public void stop() { + stopDrive(); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad != controller) { + return; + } + + // DEBUG: Toggle hardware fault + if (button.equals("guide")) { + robot.hardwareFault = !robot.hardwareFault; + + if (robot.hardwareFault) { + robot.hardwareFaultMessage = "Manually triggered."; + } else { + robot.hardwareFaultMessage = ""; + } + } + + if (button.equals("right_stick_button")) { + robotSlowMode = !robotSlowMode; + } + + if (button.equals("left_stick_button") && robot.hardwareFault) { + robot.imu.resetYaw(); + } + } + + // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html + private void move() { + if (robot.automaticAntiTipActive || robot.hardwareFault) { + return; + } + + double x = -controller.left_stick_x; + double y = -controller.left_stick_y; + + // Improve control? + if (y < 0) { + y = -Math.sqrt(-y); + } else { + y = Math.sqrt(y); + } + + if (x < 0) { + x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing; + } else { + x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing; + } + + double rx = -controller.right_stick_x; + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + + double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0; + + double heading = robot.heading(); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); + + frontLeftPower = (rotY + rotX + rx) / denominator; + backLeftPower = (rotY - rotX - rx) / denominator; + frontRightPower = (rotY - rotX + rx) / denominator; + backRightPower = (rotY + rotX - rx) / denominator; + + double maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value()); + double slowVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_slow_velocity_in_inches").value()); + double velocity = robotSlowMode ? slowVelocity : maxVelocity; + + // Power is treated as a ratio here + robot.frontLeftDrive.setVelocity(frontLeftPower * velocity); + robot.frontRightDrive.setVelocity(frontRightPower * velocity); + + robot.backLeftDrive.setVelocity(backLeftPower * velocity); + robot.backRightDrive.setVelocity(backRightPower * velocity); + } + + private void stopDrive() { + robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0); + robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0); + + robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0); + } +}