Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java

@@ -53,6 +53,7 @@ public class PositionalServoController {
             workingPosition += estimatedTravel;
             travelling = true;
         } else {
+            workingPosition = targetPosition;
             travelling = false;
         }
 
@@ -68,10 +69,16 @@ public class PositionalServoController {
         return estimatedPosition;
     }
 
+    public double getWorkingPosition() { return workingPosition; }
+
+    public double getTargetPosition() { return targetPosition; }
+
     public double getEstimatedAngle() {
         return estimatedPosition * servoRangeInDegrees;
     }
 
+    public double getError() { return targetPosition - estimatedPosition; }
+
     private double lerp(double min, double max, double t)
     {
         return min + (max - min) * t;

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java

@@ -134,6 +134,7 @@ public class Robot {
 
         arm.setTargetPosition(0);
         arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        armTargetPosition = ArmPosition.COLLECT;
 
         //      ZERO POWER BEHAVIOR
         frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -148,16 +149,21 @@ public class Robot {
         arm.setPower(tuningConfig("arm_automatic_power").value());
 
         //   SERVOS
+        Servo leftRiser = engine.hardwareMap.servo.get("LowRiserLeft");
+        leftRiser.setDirection(Servo.Direction.FORWARD);
+        Servo rightRiser = engine.hardwareMap.servo.get("LowRiserRight");
+        rightRiser.setDirection(Servo.Direction.REVERSE);
+
         //      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"),
+                leftRiser,
                 5.0,
                 428.57142858,
                 270.0
         ); // SERVER PORT: ? - ? HUB
 
         rightRiserServoController = new PositionalServoController(
-                engine.hardwareMap.servo.get("LowRiserRight"),
+                rightRiser,
                 5.0,
                 428.57142858,
                 270.0
@@ -176,9 +182,11 @@ public class Robot {
         //  SENSORS
         //      IMU
         IMU.Parameters parameters = new IMU.Parameters(
-                new RevHubOrientationOnRobot(
-                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
-                    RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
+            new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+            )
+        );
 
         imu.initialize(parameters);
 
@@ -286,6 +294,8 @@ public class Robot {
 
         engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
 
+        engine.telemetry.addLine();
+
         // 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()));
@@ -330,26 +340,26 @@ public class Robot {
 
         // Servo Positions
         engine.telemetry.addLine("Servo Positions");
-        engine.telemetry.addData("Left Riser (Est)", leftRiserServoController.getEstimatedPosition());
-        engine.telemetry.addData("Right Riser (Est)", rightRiserServoController.getEstimatedPosition());
-        engine.telemetry.addData("Camera (Blind)", cameraServo.getPosition());
+        engine.telemetry.addData("      Left Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", leftRiserServoController.getEstimatedPosition(), leftRiserServoController.getError(), leftRiserServoController.getTargetPosition(), leftRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Right Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", rightRiserServoController.getEstimatedPosition(), rightRiserServoController.getError(), rightRiserServoController.getTargetPosition(), rightRiserServoController.getServo().getPosition());
+        engine.telemetry.addData("      Camera (Blind)", cameraServo.getPosition());
 
         engine.telemetry.addLine();
 
         // Continuous Servo Powers
         engine.telemetry.addLine("Servo Powers");
-        engine.telemetry.addData("Collector Left", collectorLeftServo.getPower());
-        engine.telemetry.addData("Collector Right", collectorRightServo.getPower());
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getPower());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getPower());
 
         engine.telemetry.addLine();
 
         // Servo Directions
         engine.telemetry.addLine("Servo Directions");
-        engine.telemetry.addData("Left Riser", leftRiserServoController.getServo().getDirection());
-        engine.telemetry.addData("Right Riser", rightRiserServoController.getServo().getDirection());
-        engine.telemetry.addData("Camera", cameraServo.getDirection());
-        engine.telemetry.addData("Collector Left", collectorLeftServo.getDirection());
-        engine.telemetry.addData("Collector Right", collectorRightServo.getDirection());
+        engine.telemetry.addData("      Left Riser", leftRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Right Riser", rightRiserServoController.getServo().getDirection());
+        engine.telemetry.addData("      Camera", cameraServo.getDirection());
+        engine.telemetry.addData("      Collector Left", collectorLeftServo.getDirection());
+        engine.telemetry.addData("      Collector Right", collectorRightServo.getDirection());
 
         engine.telemetry.addLine();
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java

@@ -95,12 +95,12 @@ public class ArmController extends CyberarmState {
 
             // 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);
+                break;
+            case "dpad_down":
+                robot.collectorLeftServo.setPower(1);
+                robot.collectorRightServo.setPower(1);
         }
     }
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java

@@ -65,7 +65,7 @@ public class DriveController extends CyberarmState {
             return;
         }
 
-        double x = -controller.left_stick_x;
+        double x = controller.left_stick_x;
         double y = -controller.left_stick_y;
 
         // Improve control?

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java

@@ -1,7 +1,10 @@
 package org.timecrafters.minibots.states;
 
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.engines.Mini2023Engine;
@@ -11,8 +14,10 @@ public class Mini2023Bot {
     private final Mini2023Engine engine;
     public TimeCraftersConfiguration configuration;
 
-        public DcMotor Opportunity, Spirit, Victoria, Endeavour; //Don't ask. Just don't.
+        public DcMotor Opportunity, Spirit; //Don't ask. Just don't.
         public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names.
+        public IMU imu;
+        public ModernRoboticsI2cRangeSensor hazcam;
 
     public Mini2023Bot(Mini2023Engine engine) {
         this.engine = engine;
@@ -21,15 +26,21 @@ public class Mini2023Bot {
 
     private void setupRobot() {
 
-        Opportunity = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
-        Spirit = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
-//        Victoria = engine.hardwareMap.get(DcMotor.class, "Rear Dead Wheel");
-//        Endeavour = engine.hardwareMap.get(DcMotor.class, "Front Dead Wheel");
+        Spirit = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
+        Opportunity = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
 
         servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
         servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
         servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
 
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+        ));
+
+        this.imu = engine.hardwareMap.get(IMU.class, "imu");
+        imu.initialize(parameters);
+
 
 
 //        configuration = new TimeCraftersConfiguration("2023 Mini");

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java

@@ -9,51 +9,66 @@ public class Mini2023State extends CyberarmState {
 
     public double lThrust, rThrust, orbitSpeed;
 
+    public double deltaServo;
+
     public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
 
+    public double getDeltaServo() {
+
+        return deltaServo;
+    }
+
+    @Override public void telemetry() {
+        engine.telemetry.addData("Right Drive Power", robot.Opportunity.getPower());
+        engine.telemetry.addData("Left Drive Power", robot.Spirit.getPower());
+}
+
 @Override
 public void init() {
     lThrust = 0;
     rThrust = 0;
     orbitSpeed = 0;
-    robot.servoA.setPower(0);
-    robot.servoB.setPower(0);
-    robot.servoC.setPower(0);
-    robot.Opportunity.setPower(lThrust);
-    robot.Spirit.setPower(rThrust);
+    robot.servoA.setPower(orbitSpeed);
+    robot.servoB.setPower(orbitSpeed);
+    robot.servoC.setPower(orbitSpeed);
+    robot.Spirit.setPower(lThrust);
+    robot.Opportunity.setPower(rThrust);
 }
 
 @Override
 public void exec() {
 
     if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
-        lThrust = engine.gamepad2.left_stick_y;
-        robot.Opportunity.setPower(lThrust);
+        lThrust = engine.gamepad1.left_stick_y * 0.5;
+        robot.Spirit.setPower(lThrust);
     } else {
         lThrust = 0;
-        robot.Opportunity.setPower(lThrust);
+        robot.Spirit.setPower(lThrust);
     }
 
     if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
-        rThrust = engine.gamepad1.right_stick_y;
-        robot.Spirit.setPower(rThrust);
+        rThrust = engine.gamepad1.right_stick_y * 0.5;
+        robot.Opportunity.setPower(rThrust);
     } else {
         rThrust = 0;
-        robot.Spirit.setPower(rThrust);
+        robot.Opportunity.setPower(rThrust);
     }
 
     if (engine.gamepad1.right_trigger > 0.1) {
-        orbitSpeed = engine.gamepad1.right_trigger * 0.75;
+        orbitSpeed = engine.gamepad1.right_trigger * 0.5;
         robot.servoA.setPower(orbitSpeed);
         robot.servoB.setPower(orbitSpeed);
         robot.servoC.setPower(orbitSpeed);
     } else if (engine.gamepad1.left_trigger > 0.1) {
-        orbitSpeed = engine.gamepad1.left_trigger * 0.75;
+        orbitSpeed = engine.gamepad1.left_trigger * 0.5;
         robot.servoA.setPower(orbitSpeed);
         robot.servoB.setPower(orbitSpeed);
         robot.servoC.setPower(orbitSpeed);
     } else {
         orbitSpeed = 0;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
     }