diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java index 4713f2f..6dc1d78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java @@ -4,21 +4,27 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment; - +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; @TeleOp(name = "CometBot Drive V2.137", group = "Competition") public class CometBotTeleOpCompetition extends OpMode { - + public DualMotorSliderSubsystem slide; public CometBotTeleOpDevelopment runMode; @Override public void init() { runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2); + slide = new DualMotorSliderSubsystem(hardwareMap); runMode.init(); } @Override public void loop() { runMode.update(); + telemetry.update(); } public void stop(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java index 20381bf..95b2238 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java @@ -1,21 +1,16 @@ package org.firstinspires.ftc.teamcode; -import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.SleepAction; -import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.State; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; @Autonomous(name = "Specimen Auto", group = "Development") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java index 852911c..d5c2d4e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java @@ -1,17 +1,16 @@ package org.firstinspires.ftc.teamcode.cometbots; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; public class CometBotTeleOpDevelopment { @@ -32,9 +31,10 @@ public class CometBotTeleOpDevelopment { public Gamepad currentGamepad2; public Gamepad previousGamepad1; public Gamepad previousGamepad2; - public boolean grabBlock; - + public boolean wristPickup; + public boolean armParked; + public boolean goClaw; private Follower follower; public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) { @@ -43,7 +43,7 @@ public class CometBotTeleOpDevelopment { arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); skyhook = new HangMotorSubsystem(hardwareMap); - grabBlock = false; + this.gamepad1 = gamepad1; this.gamepad2 = gamepad2; currentGamepad1 = new Gamepad(); @@ -51,6 +51,7 @@ public class CometBotTeleOpDevelopment { previousGamepad1 = new Gamepad(); previousGamepad2 = new Gamepad(); follower = new Follower(hardwareMap); + } public void init() { @@ -61,6 +62,9 @@ public class CometBotTeleOpDevelopment { skyhook.init(); follower.setMaxPower(MAX_POWER); follower.startTeleopDrive(); + wristPickup = false; + armParked = true; + goClaw = false; } public void update() { @@ -82,15 +86,10 @@ public class CometBotTeleOpDevelopment { openClaw(); openThumb(); //hang - hang(); follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); } - - - - private void openClaw() { if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) { claw.switchState(); @@ -100,58 +99,68 @@ public class CometBotTeleOpDevelopment { private void openThumb() { if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) { claw.switchTState(); + } } private void armAndWristToFloor() { + if (currentGamepad2.a && !previousGamepad2.a && wristPickup) { + if (wrist.getState() != WristSubsystem.WristState.FLOOR) { + wrist.toFloorPositionTeleop(); + } else if (wristPickup) { + claw.openClaw(); + wrist.toPickupPosition(); + } + } if (currentGamepad2.a && !previousGamepad2.a) { arm.toFloorPositionTeleOp(); - if (arm.getState() == ArmSubsystem.ArmState.FLOOR) { - wrist.switchState(); - } else if (arm.getState() != ArmSubsystem.ArmState.FLOOR) { - wrist.toPickupPosition(); - } else { - wrist.toFloorPosition(); - } + + wristPickup = true; + armParked = false; } } private void armToBucketPosition() { if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) { + armParked = true; arm.toBucketPosition(); wrist.toBucketPosition(); + wristPickup = false; } } private void armToParkPosition() { if (currentGamepad2.x && !previousGamepad2.x) { + armParked = true; arm.toParkPosition(); - wrist.toFloorPosition(); + wrist.toFloorPositionTeleop(); + wristPickup = false; + } } private void dualSlidesToHighBucketPosition() { - if (currentGamepad2.y && !previousGamepad2.y) { + if (currentGamepad2.y && !previousGamepad2.y && armParked) { dualSlides.toHighBucketPosition(); } } private void dualSlidesToFloorPosition() { - if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { + if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down && armParked) { dualSlides.toFloorPosition(); } } private void dualSlidesToLowBucketPosition() { - if (currentGamepad2.b && !previousGamepad2.b) { + if (currentGamepad2.b && !previousGamepad2.b && armParked) { dualSlides.toLowBucketPosition(); } } private void hang(){ if (gamepad1.a) { claw.grabBlueberry(); - arm.setPosition(0.13); + arm.setPosition(0.15); arm.grabBlueberrySkyhook(); //claw Open small amount @@ -161,51 +170,46 @@ public class CometBotTeleOpDevelopment { if(gamepad1.b) { //confirm grab claw.closeClaw(); - grabBlock = true; + } - if (gamepad1.x && grabBlock) { + if (gamepad1.x && claw.getState() == ClawSubsystem.ClawState.CLOSED) { //now slap on bar, first wrist, then arm, then claw then driver must drive away - dualSlides.toFixedPosition(300); + dualSlides.toFixedPosition(500); dualSlides.update(); } + if (gamepad1.y) { arm.hangBlueberrySkyhook(); wrist.hangBlueberrySkyhook(); try { - Thread.sleep(1500); + Thread.sleep(500); } catch (InterruptedException e) { throw new RuntimeException(e); } - dualSlides.toFixedPosition(2100); + goClaw = true; + dualSlides.toFixedPosition(2200); dualSlides.update(); } if (gamepad1.right_bumper) { claw.openClaw(); - try { - Thread.sleep(1000); - } catch (InterruptedException e) { - throw new RuntimeException(e); + if(goClaw == true) { + try { + Thread.sleep(500); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + dualSlides.toFloorPosition(); + dualSlides.update(); + arm.toParkPosition(); + wrist.toPickupPosition(); } - dualSlides.toFloorPosition(); - claw.closeClaw(); - arm.toParkPosition(); - wrist.toFloorPosition(); } - skyhook.setPower(0); + skyhook.setPowerForward(-gamepad2.right_stick_y); + - /*if (gamepad1.dpad_down) { - skyhook.disableMotor(); - skyhook.setPower(1); - }*/ - if(gamepad1.right_trigger > 0){ - skyhook.setPower(true, 1); - } - if(gamepad1.left_trigger > 0){ - skyhook.setPower(false, 1); - } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java index afed453..7d6bfc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -10,10 +10,10 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; @Autonomous(name = "Auto Test Competition V2", group = "Dev") public class CometBotDriveV2 extends OpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java index b1c3596..741ecb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.cometbots.paths; import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -11,11 +10,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.states.FieldStates; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; public class CometBotTeleopCompetition { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java index 19872b8..6fcb580 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java @@ -34,7 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; @TeleOp(name = "Arm Test v2", group = "Debug") public class ArmTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java index 20848f4..373008d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java @@ -34,7 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; @TeleOp(name = "Claw Test v2", group = "Debug") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java index 980c22c..b0b58d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java @@ -29,13 +29,12 @@ package org.firstinspires.ftc.teamcode.cometbots.tests; -import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; @TeleOp(name = "Wrist Test v2", group = "Debug") public class WristTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index db24a10..72720cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -4,8 +4,10 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public final static double clawClose = 1; - public final static double clawOpen = 0.05; + public final static double clawClose = 0.95; + public final static double clawOpen = 0.35; + + public final static double armFloor = 0.7; public final static double armSubmarine = 0.55; @@ -13,8 +15,8 @@ public class RobotConstants { public final static double armPark = 0.33; //value for grabbing the hook Specimen public final static double grabBlueberry = 0.69; - public final static double armGrabBlueberrySkyhook = 0.05; - public final static double wristGrabBlueberrySkyhook = 0.1; + public final static double armGrabBlueberrySkyhook = 0.045; + public final static double wristGrabBlueberrySkyhook = 0.08; public final static double armHangBlueberrySkyhook = 0.18; public final static double wristHangBlueberrySkyhook = 0; public final static int slideHangBlueberrySkyhook = 500; @@ -26,7 +28,7 @@ public class RobotConstants { public final static double armBucket = 0.45; public final static double armSpecimen = 0.155; public final static double armInit = 0.13; - public final static double wristInit = 0.125; + public final static double wristInit = 0; public final static double wristPickup = 0.425; public final static double wristBucket = 0.56; public final static double wristSpecimenPrep = 0.63; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java index 55ae281..2bc1ad0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java index af1c2ba..54aba24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME; import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DualMotorSliderSubsystem.java similarity index 83% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DualMotorSliderSubsystem.java index 2adfbd4..6fe0fc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DualMotorSliderSubsystem.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; @@ -43,7 +43,10 @@ public class DualMotorSliderSubsystem { a "getter" to get the target position value. */ public void setTargetPosition(int value) { - targetPosition = value; + if(value >= liftToHighBucketPos) + targetPosition = liftToHighBucketPos; + else if(targetPosition <= liftToHighBucketPos) + targetPosition = value; } private int getTargetPosition() { return targetPosition; } @@ -79,6 +82,9 @@ public class DualMotorSliderSubsystem { public int getCurrentPosition(){ return liftSlideLeft.getCurrentPosition(); } + public double getCurrentPower(){ + return liftSlideLeft.getPower(); + } public void init() { /* Initialize the motors with the following settings (assuming slide is at the very bottom position): @@ -91,7 +97,8 @@ public class DualMotorSliderSubsystem { liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE); + //rotation is different because spools spin opposite directions now + liftSlideLeft.setDirection(DcMotorSimple.Direction.FORWARD); liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE); liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -108,8 +115,12 @@ public class DualMotorSliderSubsystem { */ public void update() { double power = calculatePower(); + if(power < 0.1 && power > 0.05){ + power = 0.12; + } liftSlideLeft.setPower(power); liftSlideRight.setPower(power); + } /* @@ -124,6 +135,9 @@ public class DualMotorSliderSubsystem { */ private double calculatePower() { double error = getTargetPosition() - liftSlideLeft.getCurrentPosition(); + if(error < 20 && error > 0){ + error = 20; + }else if(error > -20 && error < 0) integralSum += error * timer.seconds(); double derivative = (error - lastError) / timer.seconds(); lastError = error; @@ -142,6 +156,27 @@ public class DualMotorSliderSubsystem { public void toFixedPosition(int value) { setTargetPosition(value); } + + /** + * go to a specified position (measure in CM) + * Measured from the top of the left Slide + *At the floor, measurement is 34 cm + * @param centimeters height you want to go to, lowest is 34 + * UNFINISHED + */ + public void toCentimeterMeasurement(int centimeters){ + int ticksPerCm = 60; + int HeightWhenAtFloor = 34; + if(centimeters > 0) { + liftSlideLeft.setTargetPosition((centimeters+ 34)* ticksPerCm); + liftSlideRight.setTargetPosition((centimeters+ 34) * ticksPerCm); + } + } + public int getCM(){ + int ticksPerCm = 60; + int HeightWhenAtFloor = 34; + return 34 +(liftSlideLeft.getCurrentPosition()/ticksPerCm); + } public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);} public void toHangBelowFloor(){ setTargetPosition(slideBelowFloor); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java index 7fce805..773d58d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java @@ -1,8 +1,6 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.HOOK; -import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -23,6 +21,7 @@ public class HangMotorSubsystem { return Return; } public void init(){ + //init hook, set runmodes hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); hang.setDirection(DcMotorSimple.Direction.FORWARD); hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -43,7 +42,12 @@ public class HangMotorSubsystem { hang.setPower(0); } - public void setPower(double power){ + public void setPowerForward(double power){ + hang.setDirection(DcMotorSimple.Direction.FORWARD); + hang.setPower(power); + } + public void setPowerReverse(double power){ + hang.setDirection(DcMotorSimple.Direction.REVERSE); hang.setPower(power); } public void stopMotor(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HookTest.java similarity index 80% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HookTest.java index 4a07556..bba0486 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HookTest.java @@ -1,15 +1,9 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -@TeleOp(name = "Hook Test") +@TeleOp(name = "Hook Test", group = "Debug") public class HookTest extends OpMode { private HangMotorSubsystem skyhook; @@ -81,10 +75,19 @@ public class HookTest extends OpMode { if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) { skyhook.hangRobot(); } - skyhook.setPower(0); - if (gamepad1.dpad_down) { + if (gamepad1.left_trigger > 0) { skyhook.disableMotor(); - skyhook.setPower(1); + skyhook.setPowerReverse(1.0); + } + else if(gamepad1.left_trigger == 0){ + skyhook.setPowerReverse(0.0); + } + if (gamepad1.right_trigger > 0) { + skyhook.disableMotor(); + skyhook.setPowerForward(1.0); + } + else if(gamepad1.right_trigger == 0){ + skyhook.setPowerForward(0.0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java new file mode 100644 index 0000000..e87a3e6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem; +@TeleOp(name = "Inspection", group = "Debug") +public class Inspection extends OpMode { + private DualMotorSliderSubsystem dualSlides; + private ClawSubsystem claw; + private WristSubsystem wrist; + private ArmSubsystem arm; + private HangMotorSubsystem skyhook; + + @Override + public void init() { + dualSlides = new DualMotorSliderSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + skyhook = new HangMotorSubsystem(hardwareMap); + dualSlides.init(); + claw.init(); + wrist.initTeleOp(); + arm.initTeleOp(); + skyhook.init(); + } + + @Override + public void loop() { + telemetry.addData("a: ", "init"); + telemetry.addData("b: ", "armFloor/WristFloor"); + telemetry.addData("y: ", "GrabSkyhook"); + if(gamepad1.a){ + wrist.InitAuto(); + arm.initAuto(); + } + if(gamepad1.b){ + arm.toFloorPosition(); + wrist.toFloorPositionTeleop(); + } + if (gamepad1.y) { + claw.grabBlueberry(); + arm.setPosition(0.15); + arm.grabBlueberrySkyhook(); + + //claw Open small amount + wrist.grabBlueberrySkyhook(); + //wrist grab in strange way + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LiftArmWrist.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LiftArmWrist.java index d33bb7d..d8effba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LiftArmWrist.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; /* Copyright (c) 2021 FIRST. All rights reserved. * @@ -29,19 +29,11 @@ package org.firstinspires.ftc.teamcode.subsystem; * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.SleepAction; -import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; - @TeleOp(name = "Lift Wrist Arm Test", group = "Debug") public class LiftArmWrist extends LinearOpMode { @@ -118,12 +110,7 @@ public class LiftArmWrist extends LinearOpMode { lift.setTargetPosition(lift.getCurrentPosition() - 250); } - if(currentGamepad1.left_trigger > 0){ - hang.setPower((double) currentGamepad1.left_trigger); - } - if(currentGamepad1.right_trigger > 0){ - hang.setPower((double) currentGamepad1.right_trigger); - } + lift.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java index 77067bb..0644cd2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; @@ -10,12 +10,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/WristSubsystem.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/WristSubsystem.java index 617c433..f1a447c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/WristSubsystem.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.subsystem; +package org.firstinspires.ftc.teamcode.subsystems; import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket; @@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlu import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenHang; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel; @@ -17,7 +16,6 @@ import androidx.annotation.NonNull; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.ServoImplEx; @@ -68,7 +66,7 @@ public class WristSubsystem { public void toFloorPositionTeleop() { setState(WristState.FLOOR); - wrist.setPosition(.425); + wrist.setPosition(0.69); } public void toBucketPosition() { @@ -126,7 +124,7 @@ public class WristSubsystem { public void initTeleOp() { wrist.resetDeviceConfigurationForOpMode(); - toPickupPosition(); + toFloorPositionTeleop(); } public void setPosition(double position) {