diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 959ceaf..b912144 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -14,13 +14,13 @@ // Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' - +/* repositories { flatDir { dirs 'libs', 'C:\\Users\\rfgar\\navx-micro\\android\\libs' } } - +*/ android { namespace = 'org.firstinspires.ftc.teamcode' diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java index 418fbb2..621510a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Chassis.java @@ -17,7 +17,6 @@ import org.firstinspires.ftc.teamcode.subsystems.DroneLauncherSubsystem; import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.IMU; @@ -41,280 +40,140 @@ public class Chassis extends LinearOpMode { boolean hasTarget=false; public double gyr; - private boolean mix = true; - public ChassisSubsystem chassis; void ElevatorMethods(ElevatorSubsystem elevator){ - /* - if(controller1.right_trigger>0.1){ - elevator.goUp(controller1.right_trigger); - }else if(controller1.left_trigger<0.1){ - elevator.goDown(controller1.left_trigger); - } - */ - elevator.getPosition(telemetry); - if(!controller2.start) { - /* - if (controller1.dpad_up && controller1.b) { - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down && controller1.b) { - elevator.elevator1.setPower(1); - } else if (controller1.dpad_up && controller1.a) { - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down && controller1.a) { - elevator.elevator1.setPower(1); - } else if (controller1.dpad_up) { - elevator.elevator2.setPower(-1); - elevator.elevator1.setPower(-1); - } else if (controller1.dpad_down) { - elevator.elevator2.setPower(1); - elevator.elevator1.setPower(1); - } else { - elevator.elevator1.setPower(0); - elevator.elevator2.setPower(0); - } - */ + if (!controller2.start) { if(Math.abs(controller2.right_stick_y)>0.3) { elevator.DebugSpeed = Math.abs(controller2.right_stick_y); - telemetry.addData("lsthikkk",controller2.right_stick_y); if (controller2.right_stick_y > 0.3) { elevator.goDown(); } else if (controller2.right_stick_y < -0.3) { elevator.goUp(); } - }else { + } else { elevator.stop(); } } } + void ChassisMethods(ChassisSubsystem chassis, GyroscopeSubsystem gyroscope){ - //IMUMethods(); gyr = gyroscope.getRotation(); - //gyr=0; telemetry.addData("gyr",gyr); if(controller1.back){ if(controller1.a){ gyroscope.reset(); chassis.updateTargetAngle(); } - } - if(controller1.y){ + + if (controller1.y) { chassis.FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); chassis.BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - }else{ + } else { chassis.FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chassis.BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } - if(controller1.start){ - if (controller1.dpad_up){ + + if (controller1.start) { + if (controller1.dpad_up) { gyroscope.setOffset(180); chassis.updateTargetAngle(); } - if (controller1.dpad_right){ + + if (controller1.dpad_right) { gyroscope.setOffset(270); chassis.updateTargetAngle(); } - if (controller1.dpad_down){ + + if (controller1.dpad_down) { gyroscope.setOffset(0); chassis.updateTargetAngle(); } - if (controller1.dpad_left){ + + if (controller1.dpad_left) { gyroscope.setOffset(90); chassis.updateTargetAngle(); } } + chassis.arcadeDrive(controller1.left_stick_x,-controller1.left_stick_y,controller1.right_stick_x,speed,gyr); - if(controller1.right_trigger>0.1){ - speed=baseSpeed*(1.4-controller1.right_trigger); - }else{ + if (controller1.right_trigger>0.1) { + speed=baseSpeed * (1.4-controller1.right_trigger); + } else { speed=baseSpeed; } - - if(controller2.x){ - if(controller1.back){ - intake.setPower(-1); - }else { - intake.setPower(1); - } - }else{ - intake.setPower(0); - } - - } - void IMUMethods(){ - orientation=imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - //telemetry.addData("GyroX", AGyroscope.getX()); - //telemetry.addData("GyroY", AGyroscope.getY()); - telemetry.addData("Gyro", orientation.firstAngle); - - - if(controller1.right_stick_button){ - //imu.resetYaw(); + void PivotMethods(PivotSubsystem pivot) { + if (gamepad2.a) { + pivot.pickup(); + } else if (gamepad2.x) { + pivot.down(); + } else if (gamepad2.b) { + pivot.up(); + } else if (gamepad2.left_bumper) { + pivot.close(); + } else if (gamepad2.right_bumper) { + pivot.open(); } } - void ArmMethods(ArmSubsystem arm, PivotSubsystem pivot){ - - if(info.name=="gobilda"){ - arm.going_down =Chassis.controller2.left_stick_y < -0.2; - arm.going_down = false; - } - - - - if(!controller2.start){ - if (controller2.dpad_right) { - if(info.name=="gobilda") { - arm.setPosition(0.5); - arm.updateArm(); - }else { - pivot.up(); - } - - } else if(controller2.dpad_left) { - if(info.name=="gobilda") { - if(!arm.open){ - arm.setZero(); - arm.updateArm(); - } - }else { - pivot.down(); - } - - }else if(controller2.dpad_up){ - if(info.name=="gobilda") { - arm.setPosition(0.7); - arm.updateArm(); - }else { - - } - - } - } - - if (controller2.right_bumper) { - if(info.name=="rev"){ - pivot.open(); - }else{ - arm.open(); - } - - } else if(controller2.left_bumper) { - if(info.name=="rev"){ - pivot.close(); - }else{ - arm.close(); - } - } - if(info.name=="gobilda") { - if (arm.servoDelta.seconds() > 0.5) { - arm.servoDelta.reset(); - arm.showPositions(); - - } - telemetry.addData("angle", arm.angle); - } - - - } @Override public void runOpMode() { - //this.telemetry = telemetry; control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); - //imu = hardwareMap.get(IMU.class, "imu"); - intake = hardwareMap.get(DcMotor.class, "intake"); - - //control_Hub.setConstant(3); - telemetry.addData("Status", "Initialized"); - ///////////////////////////// + + //WORKS SO FAR I THINK ChassisSubsystem chassis=ChassisSubsystem.getInstance(hardwareMap,telemetry); ElevatorSubsystem elevator = ElevatorSubsystem.getInstance(hardwareMap,telemetry); - //Odometry odometry = Odometry.getInstance(hardwareMap,chassis); GyroscopeSubsystem gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); + //TESTING NEEDED DroneLauncherSubsystem launcher = DroneLauncherSubsystem.getInstance(hardwareMap,telemetry); - PivotSubsystem pivot = null; - ArmSubsystem arm = null; - if(info.name=="rev") { - pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); - }else{ - arm = ArmSubsystem.getInstance(hardwareMap, telemetry); - } - ///////////////////////////// - //orientation=imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - - //GyroscopeSubsystem gyroscope=GyroscopeSubsystem.getInstance(); - - //telemetry.addData("GyroX", orientation.secondAngle); - //telemetry.addData("GyroY", AndroidOrientation().getAngle().secondPosition()); - ////telemetry.update(); - + PivotSubsystem pivot = PivotSubsystem.getInstance(hardwareMap,telemetry); + //Maybe this messes something up?? idk maybe maybe + gyroscope.reset(); //launcher.reset(); waitForStart(); - chassis.updateTargetAngle(); - - if(info.name=="gobilda"){ - arm.setZero(); - arm.updateArm(); - } - - - //arm.setZero(); - controller1 = this.gamepad1; controller2 = this.gamepad2; - if(this.gamepad2.start&this.gamepad2.back){ + if (this.gamepad2.start&this.gamepad2.back) { controller1=this.gamepad2; } - while(opModeIsActive()) { - - if(this.gamepad2.start&this.gamepad2.back&this.gamepad2.y){ - controller1=this.gamepad2; - } + while (opModeIsActive()) { ChassisMethods(chassis,gyroscope); ElevatorMethods(elevator); - ArmMethods(arm,pivot); +// ArmMethods(arm,pivot); - telemetry.update(); + PivotMethods(pivot); if (controller2.y&&!(controller2.start||controller2.back)) { launcher.launch(); } else { - //launcher.reset(); + launcher.reset(); } - + //telemetry.addData("angle", arm.angle); + telemetry.update(); } - /* - odometry.update(); - telemetry.addData("X", odometry.position().x); - - if(hasTarget){ - odometry.moveToCoords(2000,3000); - } - */ } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java index b59f19a..75cc1a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous.java @@ -2,21 +2,329 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Blinker; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; -import org.firstinspires.ftc.teamcode.subsystems.CameraSubsystem; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; +import org.firstinspires.ftc.teamcode.subsystems.OpenCVSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ElevatorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.GyroscopeSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.Parameters; +import org.firstinspires.ftc.teamcode.subsystems.PivotSubsystem; @Autonomous public class autonomous extends LinearOpMode { - private CameraSubsystem cameraSubsystem; + private OpenCVSubsystem camera; + private ChassisSubsystem chassis; + private GyroscopeSubsystem gyroscope; + private ElevatorSubsystem elevator; + private OdometrySubsystem odometry; + private ArmSubsystem arm; + private PivotSubsystem pivot; + private Blinker control_Hub; + private double speed = 1; + private IMU imu; + private double baseSpeed = 0.5; + private Orientation orientation; + private DcMotor intake; + boolean hasTarget = false; + public double gyr; + @Override public void runOpMode() { - cameraSubsystem = CameraSubsystem.getInstance(hardwareMap); + control_Hub = hardwareMap.get(Blinker.class, "Control Hub"); + + camera = OpenCVSubsystem.getInstance(hardwareMap, telemetry); + chassis = ChassisSubsystem.getInstance(hardwareMap, telemetry); + gyroscope = GyroscopeSubsystem.getInstance(hardwareMap); +// elevator = ElevatorSubsystem.getInstance(hardwareMap); + odometry = OdometrySubsystem.getInstance(hardwareMap, chassis, telemetry); +// arm = ArmSubsystem.getInstance(hardwareMap, telemetry); + pivot = PivotSubsystem.getInstance(hardwareMap, telemetry); + + odometry.resetEncoders(); + //Maybe maybe break + gyroscope.reset(); waitForStart(); while (opModeIsActive()) { - telemetry.addData("Status", "Running"); - telemetry.update(); + + /////////////////////////////////////////////////////////// + //////////////////////REV AUTONOMOUS/////////////////////// + /////////////////////////////////////////////////////////// + boolean dolor = true; + + int objectSide = camera.findObjectSide(); + + /*if (info.name == "popo"); { +// pivot.close(); +// sleep(50); +// pivot.up(); +// elevator.goUp(); + odometry.resetEncoders(); + + switch (objectSide) { + case 0: + telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); + sleep(5000); + break; + + case 1: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), -90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); + + sleep(300); + pivot.up(); + pivot.open(); + + dolor = true; + while(dolor) { + dolor = odometry.goTo(20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.goTo(0,-100); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + case 2: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(10,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + sleep(300); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-20); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(-100,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + case 3: + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-63); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while(dolor) { + dolor = odometry.rotateTo(gyroscope.getRotation(), 90); + } + chassis.setMotors(0,0,0,0); + gyroscope.reset(); + odometry.resetEncoders(); + + sleep(300); + pivot.up(); + pivot.open(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(20,0); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + dolor = true; + while (dolor) { + dolor = odometry.goTo(0,-50); + } + chassis.setMotors(0,0,0,0); + odometry.resetEncoders(); + + break; + }*/ + + + /////////////////////////////////////////////////////////// + ///////// GOBILDA AUTONOMOUS ////////// + /////////////////////////////////////////////////////////// + + ///////////// BLUE TEAM ///////////// + +// if (info.name == "gobilda") { +// arm.close(); +// arm.setPosition(1); +// arm.updateArm(0); +// elevator.goUp(); +// +// odometry.resetEncoders(); +// +// switch (objectSide) { +// case 0: +// telemetry.addData("OBJECT NOT FOUND. ", "SKILL ISSUE IM AFRAID"); +// sleep(5000); +// break; +// +// case 1: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.rotateTo(gyroscope.getRotation(), -90); +// } +// chassis.setMotors(0,0,0,0); +// gyroscope.reset(); +// odometry.resetEncoders(); +// +// sleep(300); +// arm.setPosition(1); +// arm.updateArm(1); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.goTo(20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.goTo(0,-100); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// case 2: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(10,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// sleep(300); +// pivot.open(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(-20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-20); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(-100,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// case 3: +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-63); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while(dolor) { +// dolor = odometry.rotateTo(gyroscope.getRotation(), 90); +// } +// chassis.setMotors(0,0,0,0); +// gyroscope.reset(); +// odometry.resetEncoders(); +// +// sleep(300); +// pivot.up(); +// pivot.open(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(20,0); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// dolor = true; +// while (dolor) { +// dolor = odometry.goTo(0,-50); +// } +// chassis.setMotors(0,0,0,0); +// odometry.resetEncoders(); +// +// break; +// } +// +// } + + telemetry.addData("object side: ", objectSide); + telemetry.update(); + } } } -} +//} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java index dc7b269..181533f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/info.java @@ -1,6 +1,9 @@ package org.firstinspires.ftc.teamcode; public class info { - //public static final String name = "rev"; - public static final String name = "gobilda"; + public static final String name = "rev"; + //public static final String name = "gobilda"; + //public static final String OPMODE = "autonomous"; + public static final String OPMODE = "teleop"; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index bc64a17..096a3cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -1,8 +1,10 @@ package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -10,6 +12,9 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Chassis; +import org.firstinspires.ftc.teamcode.info; + +import javax.crypto.spec.DESedeKeySpec; public class ArmSubsystem { private static ArmSubsystem instance; @@ -44,8 +49,8 @@ public static ArmSubsystem getInstance(HardwareMap hardwareMap, Telemetry teleme return instance; } public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ - this.servoL=hardwareMap.get(Servo.class, "servoL"); - this.servoR=hardwareMap.get(Servo.class, "servoR"); + //this.servoL=hardwareMap.get(Servo.class, "servoL"); + //this.servoR=hardwareMap.get(Servo.class, "servoR"); this.claw=hardwareMap.get(Servo.class, "claw"); this.axis=hardwareMap.get(Servo.class, "axis"); //this.crservoL=hardwareMap.get(CRServo.class, "crservoL"); @@ -55,10 +60,12 @@ public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ } public void showPositions(){ - telemetry.addData("servoLl", servoL.getPosition()); - telemetry.addData("servoRr", servoR.getPosition()); + //telemetry.addData("servoLl", servoL.getPosition()); + //telemetry.addData("servoRr", servoR.getPosition()); //telemetry.addData("crservoL", crservoL.getPower()); //telemetry.addData("crservoR", crservoR.getPower()); + telemetry.addData("Axis: ", axis.getPosition()); + telemetry.addData("Claw: ", claw.getPosition()); } public void setZero(){ @@ -71,7 +78,7 @@ public void setPosition(double position){ extended = true; } - public void updateArm(){ + public void updateArm(/*int axisPosition*/) { if(going_down){ servoL.setPosition(angle-0.3); servoR.setPosition(servoR.MAX_POSITION-angle+0.3); @@ -79,9 +86,18 @@ public void updateArm(){ servoL.setPosition(angle); servoR.setPosition(servoR.MAX_POSITION-angle); } +/* + if (axisPosition == 0) { + setZero(); + } + if (axisPosition == 1) { + updateAxis(); + } - updateAxis(); + */ } + + public void updateAxis(){ if(angle>0.1){ outside=true; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java deleted file mode 100644 index d825e2c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java +++ /dev/null @@ -1,96 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import android.graphics.Bitmap; -import android.graphics.BitmapFactory; -import android.util.Size; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; - -public class CameraSubsystem { - private static HardwareMap hardwareMap; - private static CameraSubsystem instance; - - private static VisionPortal visionPortal; - - private TfodProcessor tfod; - private CameraSubsystem(HardwareMap hardwareMap){ - this.hardwareMap = hardwareMap; - initVisionPortal(); - - } - public static CameraSubsystem getInstance(HardwareMap hardwareMap){ - if (instance == null) { - instance = new CameraSubsystem(hardwareMap); - } - return instance; - } - private void initVisionPortal(){ - VisionPortal.Builder builder = new VisionPortal.Builder(); - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); - builder.setCameraResolution(new Size(1280, 720)); - builder.enableLiveView(true); - - tfod = TfodProcessor.easyCreateWithDefaults(); - - builder.addProcessor(tfod); - - visionPortal = builder.build(); - - getFrame(); - averageColor("blue", 10); - - } - private void getFrame() { - visionPortal.saveNextFrameRaw("frame.png"); // Assuming this returns the file path - - } - - private void averageColor(String color, int threshold) { - String filePath = "frame.png"; - Bitmap bitmap = BitmapFactory.decodeFile(filePath); - - if (bitmap != null) { - long totalColorValue = 0; - int count = 0; - int width = bitmap.getWidth(); - int height = bitmap.getHeight(); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - int pixel = bitmap.getPixel(x, y); - int value = 0; - - switch (color.toLowerCase()) { - case "red": - value = (pixel >> 16) & 0xff; - break; - case "green": - value = (pixel >> 8) & 0xff; - break; - case "blue": - value = pixel & 0xff; - break; - default: - throw new IllegalArgumentException("Invalid color: " + color); - } - - if (value >= threshold) { - totalColorValue += value; - count++; - } - } - } - - if (count > 0) { - float averageColorValue = totalColorValue / (float) count; - System.out.println("Average " + color + " Value: " + averageColorValue); - } else { - System.out.println("No pixels met the threshold for " + color); - } - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk deleted file mode 100644 index 378616c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CameraSubsystem.java.bkk +++ /dev/null @@ -1,39 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import android.util.Size; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.vision.VisionPortal; - -import org.firstinspires.ftc.robotcore.external.stream.*; - -import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; - -public class CameraSubsystem { - private static HardwareMap hardwareMap; - private static CameraSubsystem instance; - - private static CameraStreamClient camera; - - private static VisionPortal visionPortal; - private CameraSubsystem(HardwareMap hardwareMap){ - this.hardwareMap = hardwareMap; - - - } - public static CameraSubsystem getInstance(HardwareMap hardwareMap){ - if (instance == null) { - instance = new CameraSubsystem(hardwareMap); - } - return instance; - } - private void initVisionPortal(){ - camera = CameraStreamClient.getInstance(); - } - - private void getFrame(){ - vulforiaFrame = visionPortal.getFrameOnce(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java index 667baad..816ff29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ChassisSubsystem.java @@ -51,27 +51,27 @@ public void rotateAngle(int angle) { public ChassisSubsystem(HardwareMap hardwareMap, Telemetry telemetry){ this.hardwareMap=hardwareMap; + this.telemetry = telemetry; /* FrontLeftMotor = hardwareMap.get(DcMotor.class, "BackRightMotor");// FrontRightMotor = hardwareMap.get(DcMotor.class, "BackLeftMotor"); BackLeftMotor = hardwareMap.get(DcMotor.class, "FrontRightMotor"); BackRightMotor = hardwareMap.get(DcMotor.class, "FrontLeftMotor"); */ - this.telemetry=telemetry; FrontLeftMotor = hardwareMap.get(DcMotor.class, "FL");// FrontRightMotor = hardwareMap.get(DcMotor.class, "FR"); BackLeftMotor = hardwareMap.get(DcMotor.class, "BL"); BackRightMotor = hardwareMap.get(DcMotor.class, "BR"); - //FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //FrontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //FrontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //BackLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //BackRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } public void updateTargetAngle(){ targetAngle = lastDir; @@ -82,9 +82,46 @@ public static ChassisSubsystem getInstance(HardwareMap hardwareMap, Telemetry te } return instance; } + public void moveY(double speed) { + + frontLeftPower= -speed*(1); + backLeftPower = -speed*(1); + backRightPower = -speed*(1); + frontRightPower = -speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } + + public void moveX(double speed) { + frontLeftPower= speed*(1); + backLeftPower = -speed*(1); + backRightPower = -speed*(1); + frontRightPower = speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } + + public void moveR(double speed) { + frontLeftPower= -speed*(1); + backLeftPower = -speed*(1); + backRightPower = speed*(1); + frontRightPower = speed*(1); + + setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); + } + + public void arcadeDrive(double x, double y, double r, double speed, double degrees) { +// if (info.name == "rev") { +// double theta = Math.atan2(y, x); +// double power = Math.hypot(x, y); +// double sin = Math.sin(theta - Math.PI / 4); +// double cos = Math.cos(theta - Math.PI / 4); +// double max = Math.max(Math.abs(sin), Math.abs(cos)); +// } + telemetry.addData("targetAngle",targetAngle); if(Math.abs(targetAngle-180)<20){ targetAngle=180; @@ -112,12 +149,16 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre } }else{ - angularError = calculateRotation((int) degrees, (int) targetAngle); + /*angularError = calculateRotation((int) degrees, (int) targetAngle); + final double KPPP = 0.03; + double errror = targetAngle - degrees; + double speeed = errror * KPPP; + telemetry.addData("angularError",angularError); double angularThreshold = 10; - if(Math.abs(angularError)>angularThreshold){ - r=angularError*0.5; - } + //if(Math.abs(angularError)>angularThreshold) { + //} + r = speeed;*/ } } @@ -165,7 +206,7 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre if(info.name=="rev"){ - frontLeftPower=-speed*(y - x + r); + frontLeftPower= -speed*(y - x + r); backLeftPower = -speed*(y + x + r); backRightPower = -speed*(y + x - r); @@ -173,10 +214,6 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre }else{ } - // = /*-*/speed*(y + x - r); - - - if(info.name=="rev"){ telemetry.addData("NotRobot","Rev"); @@ -185,9 +222,6 @@ public void arcadeDrive(double x, double y, double r, double speed, double degre } - - - setMotors(frontLeftPower, frontRightPower, backLeftPower, backRightPower); @@ -224,6 +258,4 @@ public void rotateDeg(double deg){ } } - - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java index 414906b..90349f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DroneLauncherSubsystem.java @@ -27,12 +27,12 @@ public DroneLauncherSubsystem(HardwareMap hardwareMap,Telemetry telemetry){ } - + //POSSIBLY SWAP MAX_POSITION AND MIN_POSITION OUT public void launch(){ - launcher.setPosition(launcher.MIN_POSITION); + launcher.setPosition(launcher.MAX_POSITION); telemetry.addData("launcher",launcher.getPosition()); } - public void reset(){launcher.setPosition(launcher.MAX_POSITION); + public void reset(){launcher.setPosition(launcher.MIN_POSITION); telemetry.addData("launcher",launcher.getPosition()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java index 6930de7..fc012cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/GyroscopeSubsystem.java @@ -27,10 +27,14 @@ public GyroscopeSubsystem(HardwareMap hardwareMap){ public double getRotation(){ return navx.getYaw()+offset; } + public double robotYaw() { + return navx.getYaw(); + } public void reset(){ navx.zeroYaw(); } + public void setOffset(int degrees){ offset = degrees; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java deleted file mode 100644 index 965535d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Odometry.java +++ /dev/null @@ -1,90 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Odometry { - public int fieldX; - public int fieldY; - - public int robotX=0; - public int robotY=0; - - private HardwareMap hardwareMap; - - private DcMotor xEncoder; - private DcMotor yEncoder; - - private ChassisSubsystem chassis; - - private static Odometry instance; - public Odometry(HardwareMap hardwareMap, ChassisSubsystem chassis){ - this.hardwareMap=hardwareMap; - this.xEncoder = hardwareMap.get(DcMotor.class, "xEncoder"); - this.yEncoder = hardwareMap.get(DcMotor.class, "yEncoder"); - this.chassis = chassis; - } - public static Odometry getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis){ - if (instance == null) { - instance = new Odometry(hardwareMap, chassis); - } - return instance; - } - public class coordinates{ - public int x; - public int y; - public coordinates(int x, int y){ - this.x=x; - this.y=y; - } - } - public coordinates position(){ - return new coordinates(fieldX,fieldY); - } - public void update(){ - fieldX = robotX + xEncoder.getCurrentPosition(); - fieldY = robotY + yEncoder.getCurrentPosition(); - } - public void moveToCoords(int x, int y){ - int xDistance = x - fieldX; - int yDistance = y - fieldY; - - int maxpower = 1; - int minpower = 0; - int xpower = 0; - int ypower = 0; - int breakDistance = 10000; - - - if(xDistance>breakDistance){ - xpower = maxpower; - } - else if(xDistance<-breakDistance){ - xpower = -maxpower; - }else if (xDistance>0){ - xpower = maxpower*(xDistance/breakDistance); - }else if (xDistance<0){ - xpower = -maxpower*(xDistance/breakDistance); - } - - if(yDistance>breakDistance){ - ypower = maxpower; - } - else if(yDistance<-breakDistance){ - ypower = -maxpower; - } else if (yDistance>0){ - ypower = maxpower*(yDistance/breakDistance); - }else if (yDistance<0){ - ypower = -maxpower*(yDistance/breakDistance); - } - chassis.arcadeDrive(xpower,ypower,0,0.4,0); - - } - - - - - - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java new file mode 100644 index 0000000..4a80ecc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -0,0 +1,111 @@ +package org.firstinspires.ftc.teamcode.subsystems; + + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.text.BreakIterator; +import java.time.Year; + +public class OdometrySubsystem { + public int fieldX; + public int fieldY; + + public int robotX=0; + public int robotY=0; + + private HardwareMap hardwareMap; + + public DcMotor xEncoder; + public DcMotor yEncoder; + private Telemetry telemetry; + private ChassisSubsystem chassis; + private static OdometrySubsystem instance; + + public OdometrySubsystem(HardwareMap hardwareMap, ChassisSubsystem chassis, Telemetry telemetry){ + this.hardwareMap=hardwareMap; + this.xEncoder = hardwareMap.get(DcMotor.class, "xEncoder"); + this.yEncoder = hardwareMap.get(DcMotor.class, "FL"); + this.chassis = chassis; + this.telemetry = telemetry; + } + public static OdometrySubsystem getInstance(HardwareMap hardwareMap, ChassisSubsystem chassis, Telemetry telemetry){ + if (instance == null) { + instance = new OdometrySubsystem(hardwareMap, chassis, telemetry); + } + return instance; + } + + public class coordinates{ + public int x; + public int y; + public coordinates(int x, int y){ + this.x=x; + this.y=y; + } + } + + public coordinates position(){ + return new coordinates(fieldX,fieldY); + } + public void update(){ + fieldX = robotX + xEncoder.getCurrentPosition(); + fieldY = robotY + yEncoder.getCurrentPosition(); + } + + public void resetEncoders(){ + xEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + yEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + xEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + yEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + final double WHEELCIRCUMFERENCE = 4.8 * Math.PI; + final int TICKS = 2000; + + public double getXDist() { + return (xEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; + } + + public double getYDist() { + return (yEncoder.getCurrentPosition() * WHEELCIRCUMFERENCE) / TICKS; + } + + public void printEncoders() { + telemetry.addData("xEncoder", xEncoder.getCurrentPosition()); + telemetry.addData("yEncoder", yEncoder.getCurrentPosition()); + telemetry.addData("X: ", getXDist()); + telemetry.addData("Y: ", getYDist()); + } + private final double KP = 0.03; + + public boolean goTo(int x, int y) { + double yError = y - getYDist() * -1; + double xError = x - getXDist() * 1; + double ySpeed = yError * KP; + double xSpeed = xError * KP; + telemetry.addData("yError", yError); + telemetry.addData("xError: ", xError); + telemetry.addData("poss", getYDist()); + + chassis.moveY(ySpeed); + chassis.moveX(xSpeed); + telemetry.addData("aaaa", xError); + telemetry.addData("aaaayyy", yError); + telemetry.update(); + + return (Math.abs(xError) + Math.abs(yError) > 5) ? true : false; + } + + public boolean rotateTo(double degree, double target) { + final double KP2 = 0.018; + double rError = target - degree; + double rSpeed = rError * KP2; + + chassis.moveR(rSpeed); + + return (Math.abs(rError) > 2); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java new file mode 100644 index 0000000..7978f12 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OpenCVSubsystem.java @@ -0,0 +1,129 @@ +package org.firstinspires.ftc.teamcode.subsystems; +import static java.lang.Thread.sleep; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class OpenCVSubsystem { + + + public HardwareMap hardwareMap; + public Telemetry telemetry; + public OpenCVSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + initOpenCV(); + this.telemetry=telemetry; + } + public static OpenCVSubsystem getInstance(HardwareMap hardwareMap, Telemetry telemetry){ + OpenCVSubsystem instance = new OpenCVSubsystem(hardwareMap, telemetry); + if (instance == null) { + } + return instance; + } + double cX = 0; + double cY = 0; + public double width = 0; + private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK + private static final int CAMERA_WIDTH = 640; // width of wanted camera resolution + private static final int CAMERA_HEIGHT = 360; // height of wanted camera resolution + public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units + public static final double focalLength = 728; // Replace with the focal length of the camera in pixels + + private void initOpenCV() { + + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + controlHubCam.setPipeline(new PropDetectionPipeline()); + + controlHubCam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + controlHubCam.startStreaming(640,480,OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + //telemetry.addData("errorFUCK", errorCode); + } + }); + //controlHubCam.openCameraDevice(); + //telemetry.addLine("Waiting for start"); + //telemetry.update(); + } + public static double cx; + public static double cy; + + public class PropDetectionPipeline extends OpenCvPipeline{ + Mat mat = new Mat(); + Mat sleek = new Mat(); + Moments moment = new Moments(); + Mat bgrImage = new Mat(); + + @Override + public Mat processFrame(Mat input){ + //Mat mat = input.clone(); + Imgproc.cvtColor(input,mat,Imgproc.COLOR_RGB2HSV); + Scalar lowerBlue = new Scalar(94,120,140,0); + Scalar upperBlue = new Scalar(108,255,255,255); + Core.inRange(mat,lowerBlue,upperBlue,mat); + //kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(1,1)); + //Imgproc.morphologyEx(mat,mat,Imgproc.MORPH_OPEN, kernel); + //kernel.release(); + //Mat[] hsvChannels = new Mat[3]; + //Core.split(mat, Arrays.asList(hsvChannels)); + moment = Imgproc.moments(mat); + OpenCVSubsystem.cx = moment.get_m10()/moment.get_m00(); + OpenCVSubsystem.cy = moment.get_m01()/moment.get_m00(); + Point centroid = new Point(cx,cy); + Imgproc.cvtColor(mat,bgrImage, Imgproc.COLOR_GRAY2RGB); + Imgproc.line(bgrImage,centroid,centroid,new Scalar(0,255,0),10); + //Imgproc.rectangle(input + // new Point(input.cols()/4,)); + return bgrImage; + } + + } + + final int LEFTSIDE = 1; + final int CENTERSIDE = 2; + final int RIGHTSIDE = 3; + + public int findObjectSide() { + int side = 0; + final int CALIBRATION_REPETITIONS = 50; + for (int i = 0; i < CALIBRATION_REPETITIONS; i++) { + if (getcx() < 213) { + side = LEFTSIDE; + } else if (getcx() > 213 && getcx() < 425) { + side = CENTERSIDE; + } else if (getcx() > 425) { + side = RIGHTSIDE; + } + } + controlHubCam.stopStreaming(); + + return side; + } + public double getcx() { + return cx; + } + + public double getcy() { + return cy; + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java index bfc28c2..12056e1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Parameters.java @@ -2,5 +2,5 @@ public class Parameters { - public static String robot="gobilda"; + public static String robot="marvin"; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java index ebca4ec..99f3d00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/PivotSubsystem.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import android.icu.text.IDNA; + import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -9,20 +11,19 @@ public class PivotSubsystem { private static PivotSubsystem instance; public Servo servoPivot; public Servo servoClaw; + public Servo axis; private Telemetry telemetry; - private double CLAW_OPEN = 0.3; + private final double CLAW_OPEN = 1; private final double CLAW_CLOSE = 0; - private final double PIVOT_UP = 0; - - private final double PIVOT_DOWN = 0.5; + private final double PIVOT_UP = 1; + private final double PIVOT_PICKUP = 0.3; + private final double PIVOT_DOWN = 0; public PivotSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { this.servoClaw = hardwareMap.get(Servo.class, "claw"); this.servoPivot = hardwareMap.get(Servo.class, "pivot"); this.telemetry = telemetry; - - this.CLAW_OPEN = 90; } public static PivotSubsystem getInstance(HardwareMap hardwareMap, Telemetry telemetry) { @@ -49,12 +50,26 @@ public void down() { servoPivot.setPosition(PIVOT_DOWN); } + public void pickup() { + servoPivot.setPosition(PIVOT_PICKUP); + } + + public void pivotControls(boolean buttonA, boolean buttonB, boolean buttonX, boolean leftBumper, boolean rightBumper) { + if (buttonA) { + pickup(); + } else if (buttonB) { + up(); + } else if (rightBumper) { + open(); + } else if (leftBumper) { + close(); + } else if (buttonX) { + down(); + } + } public void printPositions() { telemetry.addData("claw", servoClaw.getPosition()); telemetry.addData("pivot", servoPivot.getPosition()); telemetry.update(); } - - - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d index eac79e1..b8b0a47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/EncoderTest.java.d @@ -10,7 +10,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Orientation; import com.qualcomm.robotcore.hardware.Blinker; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.subsystems.ChassisSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.Odometry; +import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.teamcode.subsystems.Parameters;