diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java index e36e85dc..9ac82e17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -76,13 +76,23 @@ public static double rpmToVelocity(double rpm) { private final AprilTagCamera[] cameras; - private final Servo purpleServo; - public final DcMotorEx armMotor; - public final Servo wristServo; - public final AxonServo intakeServo; - public final Servo droneServo; +// private final Servo purpleServo; +// public final DcMotorEx armMotor; +// public final Servo wristServo; +// public final AxonServo intakeServo; +// public final Servo droneServo; +// public final DcMotorEx hangMotor; +// public final DcMotor intakeMotor; + + public final DcMotorEx shoulderMotor; public final DcMotorEx hangMotor; - public final DcMotor intakeMotor; + public final AxonServo elbowServo; + public final AxonServo wristTwistServo; + public final Servo wristLiftServo; + public final Servo pinchServo; + public final Servo purpleServo; + public final Servo planeAngleServo; + public final CRServo planeReleaseServo; public RoboticaBot(HardwareMap hardwareMap) { super(hardwareMap); @@ -92,15 +102,43 @@ public RoboticaBot(HardwareMap hardwareMap) { cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 5.5); - purpleServo = hardwareMap.get(Servo.class, "purple"); - armMotor = hardwareMap.get(DcMotorEx.class, "arm"); - armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - wristServo = hardwareMap.get(Servo.class, "cup_pivot"); - intakeServo = new AxonServo("intake_servo", "intake_analog_2", hardwareMap); - droneServo = hardwareMap.get(Servo.class, "drone"); + // plane_release crservo + // plane_angle servo + + // shoulder motor + // hang motor + // elbow axon servo + // wrist_twist axon servo + // wrist_lift servo + // pinch axon servo + // purple servo + + // axon_2 analog + // axon_3 analog + + planeAngleServo = hardwareMap.get(Servo.class, "plane_angle"); + planeReleaseServo = hardwareMap.get(CRServo.class, "plane_release"); + + shoulderMotor = hardwareMap.get(DcMotorEx.class, "shoulder"); hangMotor = hardwareMap.get(DcMotorEx.class, "hang"); - intakeMotor = hardwareMap.get(DcMotor.class, "intake"); + elbowServo = new AxonServo("elbow", "axon_2", hardwareMap); + wristTwistServo = new AxonServo("wrist_twist", "axon_3", hardwareMap); + wristLiftServo = hardwareMap.get(Servo.class, "wrist_lift"); + //pinchServo = hardwareMap.get(AxonServo.class, "pinch"); + //pinchServo = new AxonServo("pinch", null, hardwareMap); // TODO SWITCH TO NORMAL SERVO + pinchServo = hardwareMap.get(Servo.class, "pinch"); + //pinchServo.setPower(0.1); + purpleServo = hardwareMap.get(Servo.class, "purple"); + +// purpleServo = hardwareMap.get(Servo.class, "purple"); +// armMotor = hardwareMap.get(DcMotorEx.class, "arm"); +// armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// wristServo = hardwareMap.get(Servo.class, "cup_pivot"); +// intakeServo = new AxonServo("intake_servo", "intake_analog_2", hardwareMap); +// droneServo = hardwareMap.get(Servo.class, "drone"); +// hangMotor = hardwareMap.get(DcMotorEx.class, "hang"); +// intakeMotor = hardwareMap.get(DcMotor.class, "intake"); // TODO: Reverse Motors, encoders & such @@ -171,9 +209,18 @@ public void dropPurplePixel(boolean state) { } } + + public static double PLANE_START_ANGLE = 0.6; + public static double PLANE_LAUNCH_ANGLE = 0.80; + public static double PLANE_FOR_INCR = 0.001; + public static int PLANE_DELAY = 10; @Override public void launchPlane() { - // TODO + for (double i=PLANE_START_ANGLE; i -230) { - GlobalOpMode.opMode.sleep(100); - } - wristServo.setPosition(0.85); - intakeServo.setAdjustedPosition(-950, 0.1); - } - - public void dropOff() { - intakeServo.setAdjustedPosition(-1350, 0.1); - armMotor.setTargetPosition(100); - armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - armMotor.setPower(0.1); - while (armMotor.getCurrentPosition() < 90) { - GlobalOpMode.opMode.sleep(100); - } - wristServo.setPosition(0.5); - // Raise the arm the rest of the way around - armMotor.setTargetPosition(300); - armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - armMotor.setPower(0.1); - while (armMotor.getCurrentPosition() < 290) { - GlobalOpMode.opMode.sleep(100); - } - wristServo.setPosition(0); - } +// public void initializeIntakeSystem() { +// wristServo.setPosition(0.5); +// GlobalOpMode.opMode.sleep(1000); +// armMotor.setTargetPosition(100); +// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// armMotor.setPower(0.1); +// while (armMotor.getCurrentPosition() < 90) { +// GlobalOpMode.opMode.sleep(100); +// } +// //extendIntake(true); +// //intakeServo.setAdjustedPosition(-1350, 0.1); +// } +// +// public void readyForIntake() { +// intakeServo.setAdjustedPosition(-1350, 0.1); +// armMotor.setTargetPosition(-240); +// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// armMotor.setPower(0.1); +// while (armMotor.getCurrentPosition() > -230) { +// GlobalOpMode.opMode.sleep(100); +// } +// wristServo.setPosition(0.85); +// intakeServo.setAdjustedPosition(-950, 0.1); +// } +// +// public void dropOff() { +// intakeServo.setAdjustedPosition(-1350, 0.1); +// armMotor.setTargetPosition(100); +// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// armMotor.setPower(0.1); +// while (armMotor.getCurrentPosition() < 90) { +// GlobalOpMode.opMode.sleep(100); +// } +// wristServo.setPosition(0.5); +// // Raise the arm the rest of the way around +// armMotor.setTargetPosition(300); +// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// armMotor.setPower(0.1); +// while (armMotor.getCurrentPosition() < 290) { +// GlobalOpMode.opMode.sleep(100); +// } +// wristServo.setPosition(0); +// } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java index 0dabf38e..51a5107f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java @@ -20,7 +20,7 @@ public void runOpMode() throws InterruptedException { servoTest.innerServo.setPower(gamepad1.left_stick_x); //servoTest.innerAnalog.getVoltage(); //servoTest.getCurrentPosition(); - telemetry.addData("Degrees", servoTest.getCurrentPosition()); + //telemetry.addData("Degrees", servoTest.getCurrentPosition()); telemetry.addData("Power", gamepad1.left_stick_x); //telemetry.addData("Last Pos", servoTest.getLastPosDebug()); telemetry.addData("Adjusted Degrees", servoTest.getAdjustedPosition()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java index 1604cd1d..35c9c9da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java @@ -1,64 +1,105 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.util.GlobalOpMode; import org.firstinspires.ftc.teamcode.util.Timeout; +import java.util.ArrayList; + public class AxonServo { public CRServo innerServo; - public AnalogInput innerAnalog; + public AnalogInput innerAnalog = null; // Create a new background thread to count rotations - private final ServoCounter counter; - - private class ServoCounter extends Thread { - private int count = 0; - - public void run() { - try { - android.util.Log.w("AXON_THREAD", "Starting thread"); - double lastPosition = getCurrentPosition(); - while (!GlobalOpMode.opMode.isStopRequested()) { - //android.util.Log.w("AXON_THREAD", "LOOP"); - Thread.yield(); - double currentPosition = getCurrentPosition(); - // Positive wrap-around from 0 -> 360 - if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) { - count--; - } - // Negative wrap-around from 360 -> 0 - if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) { - count++; - } - lastPosition = currentPosition; - } - } catch (Exception e) { - android.util.Log.e("AXON_THREAD", "Crashed with error: " + e); - } + private int count = 0; + //private final ServoCounter counter; + +// private class ServoCounter extends Thread { +// private int count = 0; + + + +// public void run() { +// try { +// android.util.Log.w("AXON_THREAD", "Starting thread"); +// double lastPosition = getCurrentPosition(); +// while (!GlobalOpMode.opMode.isStopRequested()) { +// //android.util.Log.w("AXON_THREAD", "LOOP"); +// Thread.yield(); +// double currentPosition = getCurrentPosition(); +// // Positive wrap-around from 0 -> 360 +// if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) { +// count--; +// } +// // Negative wrap-around from 360 -> 0 +// if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) { +// count++; +// } +// lastPosition = currentPosition; +// } +// } catch (Exception e) { +// android.util.Log.e("AXON_THREAD", "Crashed with error: " + e); +// } +// } +// +// public int getCount() { +// return count; +// } +// } + + private double lastPosition = -999999999; + private void update() { + if (this.innerAnalog == null) { + return; } + if (lastPosition == -999999999) { + lastPosition = getCurrentPosition(); + } + //double lastPosition = getCurrentPosition(); - public int getCount() { - return count; + double currentPosition = getCurrentPosition(); + // Positive wrap-around from 0 -> 360 + if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) { + count--; + } + // Negative wrap-around from 360 -> 0 + if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) { + count++; + } + lastPosition = currentPosition; + } + + private static final ArrayList allServos = new ArrayList<>(); + + public static void updateAll() { + for (AxonServo servo : allServos) { + servo.update(); } } public AxonServo(String servoName, String analogName, HardwareMap hwMap) { this.innerServo = hwMap.get(CRServo.class, servoName); - this.innerAnalog = hwMap.get(AnalogInput.class, analogName); + if (analogName != null) { + this.innerAnalog = hwMap.get(AnalogInput.class, analogName); + allServos.add(this); + } - counter = new ServoCounter(); - counter.start(); + //counter = new ServoCounter(); + //counter.start(); } - public double getCurrentPosition() { + private double getCurrentPosition() { + if (this.innerAnalog == null) { + + } return this.innerAnalog.getVoltage()/3.3 * 360; } public double getAdjustedPosition() { - return getCurrentPosition() + counter.getCount() * 360; + return getCurrentPosition() + this.count * 360; } public void setAdjustedPosition(double position, double power) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java index fd30dd4e..1e8b7735 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java @@ -59,7 +59,7 @@ public void runOpMode() throws InterruptedException { // Set servo position right away so that it holds if (robot.getClass() == RoboticaBot.class) { - ((RoboticaBot) robot).wristServo.setPosition(0.2); + //((RoboticaBot) robot).wristServo.setPosition(0.2); } //Pose2d startPose = new Pose2d(12,-62, Math.toRadians(90)); // RED_BOARD diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java index 099fc6b8..2180fa85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java @@ -10,9 +10,7 @@ import org.firstinspires.ftc.teamcode.drive.Robot; import org.firstinspires.ftc.teamcode.drive.RoboticaBot; import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; -import org.firstinspires.ftc.teamcode.util.Debouncer; import org.firstinspires.ftc.teamcode.util.GlobalOpMode; -import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer; @TeleOp public class TestTeleOp extends LinearOpMode { @@ -29,7 +27,7 @@ public void runOpMode() throws InterruptedException { if (robot.getClass() == RoboticaBot.class) { ///((RoboticaBot) robot).extendIntake(true); - ((RoboticaBot) robot).initializeIntakeSystem(); + //((RoboticaBot) robot).initializeIntakeSystem(); } double wristPose = 0; @@ -38,7 +36,7 @@ public void runOpMode() throws InterruptedException { double purplePose = .5; int armPos = 0; if (robot.getClass() == RoboticaBot.class) { - armPos = ((RoboticaBot) robot).armMotor.getCurrentPosition(); + //armPos = ((RoboticaBot) robot).armMotor.getCurrentPosition(); } while (opModeIsActive()) { @@ -103,29 +101,31 @@ public void runOpMode() throws InterruptedException { android.util.Log.i("TELEOP", "LOOP2B"); if (gamepad1.left_trigger + gamepad1.right_trigger > 0.05) { - rrobot.armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - rrobot.armMotor.setVelocity((gamepad1.left_trigger - gamepad1.right_trigger) * 0.4); - armPos = rrobot.armMotor.getCurrentPosition(); + //rrobot.shoulderMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + //rrobot.shoulderMotor.set + //rrobot.shoulderMotor.setVelocity((gamepad1.left_trigger - gamepad1.right_trigger) * 20); + armPos = rrobot.shoulderMotor.getCurrentPosition(); + armPos += 10.0 * (gamepad1.left_trigger - gamepad1.right_trigger); } else { - //armPos = rrobot.armMotor.getCurrentPosition(); - rrobot.armMotor.setTargetPosition(armPos); - rrobot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rrobot.armMotor.setPower(0.3); + //armPos = rrobot.shoulderMotor.getCurrentPosition(); +// rrobot.shoulderMotor.setTargetPosition(armPos); +// rrobot.shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rrobot.shoulderMotor.setPower(0.3); } -// rrobot.armMotor.setTargetPosition(armPos); -// rrobot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); -// rrobot.armMotor.setPower(0.3); + rrobot.shoulderMotor.setTargetPosition(armPos); + rrobot.shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rrobot.shoulderMotor.setPower(1); - //rrobot.armMotor.setPower((gamepad1.left_trigger*0.5)-((gamepad1.right_trigger*0.5))); + //rrobot.shoulderMotor.setPower((gamepad1.left_trigger*1)-((gamepad1.right_trigger*1))); if (gamepad1.dpad_up) { - rrobot.intakeMotor.setPower(1); + //rrobot.intakeMotor.setPower(1); } else if (gamepad1.dpad_down) { - rrobot.intakeMotor.setPower(-1); + //rrobot.intakeMotor.setPower(-1); } else { - rrobot.intakeMotor.setPower(0); + // rrobot.intakeMotor.setPower(0); } android.util.Log.i("TELEOP", "LOOP2C"); @@ -139,7 +139,10 @@ public void runOpMode() throws InterruptedException { if (wristPose < 0) wristPose = 0; if (wristPose > 1) wristPose = 1; - rrobot.wristServo.setPosition(wristPose); + //rrobot.planeAngleServo.setPosition(wristPose); + + //rrobot..setPosition(wristPose); + //rrobot.wristServo.setPosition(wristPose); double p; if (gamepad1.dpad_left) { @@ -150,7 +153,10 @@ public void runOpMode() throws InterruptedException { p = 0; } - rrobot.intakeServo.innerServo.setPower(p); + //rrobot.intakeServo.innerServo.setPower(p); + if (gamepad1.a) { + rrobot.launchPlane(); + } if (gamepad1.x || gamepad2.x) { rrobot.hangMotor.setPower(1); @@ -159,7 +165,7 @@ public void runOpMode() throws InterruptedException { } else { rrobot.hangMotor.setPower(0); } - //telemetry.addData("Wrist Pose", wristPose); + telemetry.addData("Wrist Pose", wristPose); // telemetry.addData("Arm Pose", rrobot.armMotor.getCurrentPosition()); // telemetry.addData("Extend Amount", rrobot.intakeServo.getAdjustedPosition());