diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index d9e1a1d..44eec90 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -29,6 +29,11 @@ public class PhoenixBot1 { public static double leftCompensatorGlobal; public static double RightCompensatorGlobal; public double VEER_COMPENSATION_DBL; // some place around 1, .99 is 1% power reduction + public double DRIVETRAIN_MINIMUM_POWER; + public double ROTATION_MINIMUM_POWER; + public double STRAFE_MINIMUM_POWER; + public double DRIVE_TOLERANCE; + public double ROTATION_TOLERANCE; // private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite"; private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite"; @@ -86,6 +91,11 @@ public class PhoenixBot1 { public void initConstants(){ VEER_COMPENSATION_DBL = configuration.variable("Robot", "Tuning", "VEER_COMPENSATION_DBL").value(); + DRIVETRAIN_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "DRIVETRAIN_MINIMUM_POWER").value(); + ROTATION_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "ROTATION_MINIMUM_POWER").value(); + STRAFE_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "STRAFE_MINIMUM_POWER").value(); + DRIVE_TOLERANCE = configuration.variable("Robot", "Tuning", "DRIVE_TOLERANCE").value(); + ROTATION_TOLERANCE = configuration.variable("Robot", "Tuning", "ROTATION_TOLERANCE").value(); } private void initVuforia(){ @@ -217,11 +227,10 @@ public class PhoenixBot1 { // HighRiserRight.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserRight.setDirection(Servo.Direction.REVERSE); - ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE); - ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE); +// ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); CameraServo.setDirection(Servo.Direction.FORWARD); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index ecb6aea..df52009 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -43,7 +43,7 @@ public class Robot { public final ColorSensor indicatorA, indicatorB; public LynxModule expansionHub; - public final double imuAngleOffset; + public final double imuAngleOffset, initialFacing; public boolean wristManuallyControlled = false, armManuallyControlled = false; public boolean automaticAntiTipActive = false; public boolean hardwareFault = false; @@ -191,6 +191,9 @@ public class Robot { 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); @@ -725,6 +728,10 @@ public class Robot { arm.setPower(tuningConfig("arm_automatic_power").value()); } + public double initialFacing() { + return initialFacing; + } + public double facing() { double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java index 4bfd70a..db11705 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java @@ -32,7 +32,7 @@ public class Rotate extends CyberarmState { stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; - facing = (robot.facing() + targetFacing + 360.0) % 360.0; + facing = (robot.initialFacing() + targetFacing + 360.0) % 360.0; velocity = targetVelocity; }