diff --git a/.idea/deploymentTargetDropDown.xml b/.idea/deploymentTargetDropDown.xml new file mode 100644 index 0000000..7fb6c55 --- /dev/null +++ b/.idea/deploymentTargetDropDown.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java index e9b551c..704c077 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java @@ -13,7 +13,7 @@ public class GamepadChecker { private final Gamepad gamepad; private final HashMap buttons = new HashMap<>(); private final HashMap buttonsDebounceInterval = new HashMap<>(); - private final long debounceTime = 20L; // ms + private final long debounceTime = 0L; // ms public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) { this.engine = engine; 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 ee9c218..c603ebf 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 @@ -7,6 +7,7 @@ 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; @@ -32,7 +33,8 @@ import java.util.concurrent.TimeUnit; public class Robot { private static final String TAG = "CHIRON | Robot"; public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm; - public final ServoImplEx gripper, wrist; + public final ServoImplEx gripper; + public final CRServoImplEx wrist; public final IMU imu; public final ColorSensor indicatorA, indicatorB; public LynxModule expansionHub; @@ -102,7 +104,7 @@ public class Robot { arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ? - wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist"); // SERVO PORT: ? + wrist = engine.hardwareMap.get(CRServoImplEx.class, "wrist"); // SERVO PORT: ? indicatorA = engine.hardwareMap.colorSensor.get("indicator_A"); // I2C indicatorB = engine.hardwareMap.colorSensor.get("indicator_B"); // I2C @@ -141,17 +143,19 @@ public class Robot { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // MOTOR POWER - arm.setVelocity( - angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value())); +// arm.setVelocity( +// angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value())); + arm.setPower(0.75); // SERVOS (POSITIONAL) // Gripper gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); gripper.setPosition(tuningConfig("gripper_initial_position").value()); + // SERVOS (CONTINUOUS) // Wrist - wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); - wrist.setPosition(tuningConfig("wrist_initial_position").value()); + wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + wrist.setPower(tuningConfig("wrist_up_power").value()); // SENSORS // COLOR SENSORS @@ -232,7 +236,7 @@ public class Robot { engine.telemetry.addLine(); - engine.telemetry.addData(" Arm", "%d (%8.2f in)", arm.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, arm.getTargetPosition())); + engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition())); // Motor Velocity engine.telemetry.addLine("Motor Velocity"); @@ -283,7 +287,7 @@ public class Robot { engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled()); engine.telemetry.addLine(); engine.telemetry.addData(" Wrist Direction", wrist.getDirection()); - engine.telemetry.addData(" Wrist Position", wrist.getPosition()); + engine.telemetry.addData(" Wrist Power", wrist.getPower()); engine.telemetry.addData(" Wrist Enabled", wrist.isPwmEnabled()); engine.telemetry.addLine(); @@ -293,6 +297,7 @@ public class Robot { 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(); @@ -566,7 +571,10 @@ public class Robot { double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time); double deltaTime = (time - lastTiming) * 0.001; - double error = targetVelocity - arm.getVelocity(); + 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; @@ -574,7 +582,9 @@ public class Robot { motorTargetVelocity.put("Arm", newTargetVelocity); motorVelocityLastTiming.put("Arm", time); - arm.setVelocity(newTargetVelocity); +// arm.setVelocity(newTargetVelocity); + + arm.setPower(0.75); } public double getVoltage() { @@ -584,11 +594,12 @@ public class Robot { public double facing() { double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - return (imuDegrees + imuAngleOffset + 360.0) % 360.0; + return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; } public double heading() { - return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); +// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); } public double turnRate() { diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java index 8dceebd..fda2ea3 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java @@ -8,7 +8,7 @@ public class Move extends CyberarmState { private final Robot robot; private final String groupName, actionName; - private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS; + private final double targetDistance, tolerance, facing, targetVelocity, timeInMS; private final double maxVelocity; private double speed; @@ -21,11 +21,10 @@ public class Move extends CyberarmState { this.groupName = groupName; this.actionName = actionName; - positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value(); - positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value(); - toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value(); + targetDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetDistanceInInches").value()); + tolerance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value()); facing = robot.getConfiguration().variable(groupName, actionName, "facing").value(); - targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value()); + targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value()); timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; @@ -36,7 +35,7 @@ public class Move extends CyberarmState { @Override public void start() { // TODO: Use a dead wheel for this - distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition(); + distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition(); } @Override @@ -55,23 +54,33 @@ public class Move extends CyberarmState { return; } - // TODO: Double check maths -// int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled); -// int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches); -// int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches); -// -// if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) { -// stop(); -// -// setHasFinished(true); -// -// return; -// } + int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled; - move(); + if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) { + stop(); + + setHasFinished(true); + + return; + } + + moveDirectional(travelledDistance); } - private void move() { + private void moveDirectional(double travelledDistance) { + // TODO: Ease-in-out velocity + double velocity = targetVelocity; + + if (targetDistance > travelledDistance) { + robot.frontRightDrive.setVelocity(-velocity); + robot.backLeftDrive.setVelocity(-velocity); + } else { + robot.frontRightDrive.setVelocity(velocity); + robot.backLeftDrive.setVelocity(velocity); + } + } + + private void moveOmni() { if (Math.abs(speed) > maxVelocity) { speed = speed < 0 ? -maxVelocity : maxVelocity; } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java index 9536ce7..937e852 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java @@ -78,18 +78,22 @@ public class ArmDriverControl extends CyberarmState { return; } - double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value(); - double stepSize = robot.tuningConfig("wrist_manual_step_size").value(); + double stepPower = robot.tuningConfig("wrist_manual_step_power").value(); - if ((controller.dpad_left || controller.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) { - lastArmManualControlTime = runTime(); + if ((controller.dpad_left || controller.dpad_right)) { + robot.wristManuallyControlled = true; if (controller.dpad_left) { // Wrist Left - robot.wrist.setPosition(robot.wrist.getPosition() - stepSize); + robot.wrist.setPower(stepPower); - } else if (controller.dpad_right) { // Wrist Right - robot.wrist.setPosition(robot.wrist.getPosition() + stepSize); } + if (controller.dpad_right) { // Wrist Right + robot.wrist.setPower(-stepPower); + } + } + + if (!controller.dpad_left && !controller.dpad_right) { + robot.wrist.setPower(0); } } @@ -103,7 +107,7 @@ public class ArmDriverControl extends CyberarmState { private void automatics() { if (!robot.hardwareFault) { - automaticWrist(); +// automaticWrist(); automaticArmVelocity(); } @@ -118,9 +122,9 @@ public class ArmDriverControl extends CyberarmState { double angle = robot.tuningConfig("wrist_auto_rotate_angle").value(); if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= angle) { - robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); + robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value()); } else { - robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value()); + robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value()); } } @@ -209,11 +213,13 @@ public class ArmDriverControl extends CyberarmState { if (button.equals("dpad_down")) { robot.wristManuallyControlled = false; - robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); - } else if (button.equals("dpad_up")) { + robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value()); + } + + if (button.equals("dpad_up")) { robot.wristManuallyControlled = false; - robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value()); + robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value()); } // Automatic Arm Control diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java index cd0ccf6..881643a 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java @@ -1,5 +1,6 @@ package org.timecrafters.minibots.cyberarm.chiron.states.teleop; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; @@ -48,6 +49,7 @@ public class DrivetrainDriverControl extends CyberarmState { double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y; double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_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);