diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java index 29fb109..ec0f7e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java @@ -8,14 +8,15 @@ import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoServos; import java.util.Arrays; +import java.util.Collections; import java.util.List; @Config public class IntakeArm extends QQMechanism { - public static double ARM_DROP_POSITION = 0.7; - public static double ARM_INTAKE_POSITION = 0.0; - public static double SEARCHING_POSITION = 0.12; - public static double TRANSFER_POSITION = 0.6; + private static final double MAX_SERVO_POS = 1; + public static double MANUAL_CHANGE_AMOUNT = 0.01; + private final static double ARM_DROP_POSITION = 0.1; + private final static double ARM_INTAKE_POSITION = 0.62; Servo leftArmServo; Servo rightArmServo; @@ -35,18 +36,34 @@ public void goToDropPos(){ public void goToIntake(){ goToPos(ARM_INTAKE_POSITION); } - public void searching(){ - goToPos(SEARCHING_POSITION); + + + public void rotateArmLeft(){ + rotateArm(MANUAL_CHANGE_AMOUNT); + } + public void rotateArmRight(){ + rotateArm(-MANUAL_CHANGE_AMOUNT); } - public void transfer(){ - goToPos(TRANSFER_POSITION); + + private void rotateArm(double manualChangeAmount){ + double leftPos = leftArmServo.getPosition(); + double rightPos = rightArmServo.getPosition(); + + leftPos = leftPos - manualChangeAmount; + rightPos = rightPos + manualChangeAmount; + + if(((leftPos <= MAX_SERVO_POS) && (leftPos >= 0)) && + ((rightPos <= MAX_SERVO_POS) && (rightPos >= 0))){ + leftArmServo.setPosition(leftPos); + rightArmServo.setPosition(rightPos); + } } @Override public List getTests() { - return Arrays.asList( - new TestTwoServos("arm_pos", ARM_DROP_POSITION, ARM_INTAKE_POSITION, leftArmServo, rightArmServo)); + return Collections.singletonList( + new TestTwoServos("arm_pos", ARM_INTAKE_POSITION, ARM_DROP_POSITION, leftArmServo, rightArmServo)); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java index f13c312..e78ce3a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java @@ -3,61 +3,93 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo; +import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoServos; import java.util.Arrays; import java.util.List; @Config public class IntakeClaw extends QQMechanism { + public static final double MAX_SERVO_POS = 1; public static double CLAW_CLOSE_POSITION = 1; public static double CLAW_OPEN_POSITION = 0.8; - public static double WRIST_FLAT_POSITION = 0.5; - public static double WRIST_START_POSITION = 0.15; - public static double WRIST_END_POSITION = 0.85; + public static double WRIST_START_POSITION = 0.0; + public static double WRIST_TRANSFER_POSITION = 0.65; + ElapsedTime openTimer = new ElapsedTime(); + ElapsedTime closedTimer = new ElapsedTime(); Servo clawServo; - Servo wristServo; - double wristServoPos; + Servo leftWristServo; + Servo rightWristServo; + double CLOSED_TIME = 0.5; + double OPEN_TIME = 0.5; + double leftWristServoPos; + double rightWristServoPos; + @Override public void init(HardwareMap hwMap) { clawServo = hwMap.get(Servo.class, "intake_claw_servo"); - wristServo = hwMap.get(Servo.class, "intake_wrist_servo"); + leftWristServo = hwMap.get(Servo.class, "left_intake_wrist"); + rightWristServo = hwMap.get(Servo.class,"right_intake_wrist"); + leftWristServo.setDirection(Servo.Direction.REVERSE); } @Override public List getTests() { return Arrays.asList( new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo), - new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo) + new TestTwoServos("wrist_movement", WRIST_START_POSITION, WRIST_TRANSFER_POSITION, leftWristServo,rightWristServo) ); } - - + public void wristTransfer(){ + leftWristServoPos = WRIST_START_POSITION; + rightWristServoPos = WRIST_START_POSITION; + } + public void wristIntake(){ + leftWristServoPos = WRIST_TRANSFER_POSITION; + rightWristServoPos = WRIST_TRANSFER_POSITION; + } public void open() { + openTimer.reset(); clawServo.setPosition(CLAW_OPEN_POSITION); } public void close() { + closedTimer.reset(); clawServo.setPosition(CLAW_CLOSE_POSITION); } - public void wristFlat(){ - wristServoPos = WRIST_FLAT_POSITION; - } public void adjustWrist(double manualChangeAmount){ - wristServoPos += manualChangeAmount; - wristServoPos = Math.min(WRIST_END_POSITION,wristServoPos); - wristServoPos = Math.max(WRIST_START_POSITION,wristServoPos); + if (leftWristServoPos <= MAX_SERVO_POS && rightWristServoPos <= MAX_SERVO_POS){ + leftWristServoPos += manualChangeAmount; + rightWristServoPos -= manualChangeAmount; + } + } public boolean isClawOpen() { + if ((clawServo.getPosition() == CLAW_OPEN_POSITION) && + (openTimer.seconds()> OPEN_TIME)) + return true; + + return false; } + public boolean isClawClosed() { + if ((clawServo.getPosition() == CLAW_CLOSE_POSITION) && + (closedTimer.seconds()> CLOSED_TIME)) + return true; + + return false; + } + @Override public void update(Telemetry telemetry){ - wristServo.setPosition(wristServoPos); + rightWristServo.setPosition(rightWristServoPos); + leftWristServo.setPosition(leftWristServoPos); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java index 792dd63..69e272e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java @@ -28,6 +28,7 @@ public class IntakeSlides extends QQMechanism { public static final int SLIDES_EXTENSION_BOUNDARY = FULL_EXTENSION_POSITION+10; private static final int HALF_EXTENSION_POSITION = 740; private static final int START_POSITION = 0; + public static int MANUAL_CHANGE_AMOUNT = 30; private DcMotor rightIntakeSlideMotor; private DcMotor leftIntakeSlideMotor; @@ -68,7 +69,13 @@ public void init(HardwareMap hwMap) { public void setPosition(int desiredPos){ this.desiredPos = desiredPos; } - public void manualPositionChange(int changeAmount){ + public void extend(){ + manualPositionChange(MANUAL_CHANGE_AMOUNT); + } + public void retract(){ + manualPositionChange(-MANUAL_CHANGE_AMOUNT); + } + private void manualPositionChange(int changeAmount){ desiredPos += changeAmount; } @Override @@ -106,6 +113,7 @@ public void update(Telemetry telemetry){ public boolean isSwitchPressed(){ return limitSwitch.isPressed(); } + public boolean isSafeToRotate() { return currentPos > HALF_EXTENSION_POSITION;} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java index 7dee871..9539bad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.ColorRangeSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -25,21 +24,18 @@ public class ScoringClaw extends QQMechanism { public static final double CLOSED_TIME = 0.25; public static double CLAW_CLOSE_POSITION = 0.5; public static double CLAW_OPEN_POSITION = 0; - public static double WRIST_START_POSITION = 0; - public static double WRIST_END_POSITION = 0.67; + ElapsedTime openTimer = new ElapsedTime(); ElapsedTime closedTimer = new ElapsedTime(); Servo clawServo; - Servo wristServo; ColorRangeSensor colorSensor; TouchSensor scoreSwitch; @Override public void init(HardwareMap hwMap) { clawServo = hwMap.get(Servo.class, "claw_servo"); - wristServo = hwMap.get(Servo.class, "wrist_servo"); colorSensor = hwMap.get(ColorRangeSensor.class, "scoreclaw_color"); scoreSwitch = hwMap.get(TouchSensor.class,"score_switch"); @@ -50,7 +46,6 @@ public void init(HardwareMap hwMap) { public List getTests() { return Arrays.asList( new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo), - new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo), new TestColorRangeSensor("scoreclaw_color", colorSensor), new TestSwitch("score_switch", scoreSwitch) ); @@ -80,13 +75,6 @@ public boolean isBlockGrabbable() { return false; } - public void wristStart() { - wristServo.setPosition(WRIST_START_POSITION); - } - - public void wristEnd() { - wristServo.setPosition(WRIST_END_POSITION); - } public void open() { clawServo.setPosition(CLAW_OPEN_POSITION); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java index a126293..8567bad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java @@ -5,125 +5,128 @@ import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; @TeleOp -public class NoTurnTeleop extends QQOpMode{ +public class NoTurnTeleop extends QQOpMode { public static final double TRIGGER_THRESHOLD = 0.5; - public static final int MANUAL_CHANGE = 15; + public static final int SCORE_ARM_MANUAL_CHANGE = 15; public static final double MANUAL_CHANGE_AMOUNT_WRIST = 0.03; - private boolean isPlacing = false; - private boolean isSearching = false; - private boolean isIntaking = false; + boolean isPlacing; boolean XWasPressed; boolean manipulatorXWasPressed; boolean chamberContactWasPressed; boolean clawWasClosed; boolean slidesSwitchWasPressed; - int contactLostPos; + boolean intakeClawWasClosed; - public void init(){ + public void init() { isPlacing = false; super.init(); - robot.intakeClaw.wristFlat(); + } - public void loop(){ + + public void loop() { super.loop(); double forward = -gamepad1.left_stick_y; double left = gamepad1.left_stick_x; - double rotate = gamepad1.right_stick_x; - + double rotate = 0; + if (gamepad1.right_stick_button) { + if (gamepad1.right_stick_y < -0.2) { + robot.intakeSlides.extend(); + } else if (gamepad1.right_stick_y > 0.2) { + robot.intakeSlides.retract(); + } + if (robot.intakeSlides.isSafeToRotate()) { + if (gamepad1.right_stick_x > 0.3) { + robot.intakeArm.rotateArmRight(); + } else if (gamepad1.right_stick_x < -0.3) { + robot.intakeArm.rotateArmLeft(); + } + } + }else{ + rotate = gamepad1.right_stick_x; + } nav.driveFieldRelative(forward, left, rotate); - if (gamepad1.left_trigger > TRIGGER_THRESHOLD ){ + if (gamepad1.left_trigger > TRIGGER_THRESHOLD) { robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO); } else if (gamepad1.left_bumper) { robot.mecanumDrive.setSpeed(MecanumDrive.Speed.SLOW); } else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL); - if (gamepad1.a){ - if (robot.intakeSlides.isSwitchPressed()){ + if (gamepad1.a) { + if (robot.intakeSlides.isSwitchPressed()) { robot.scoreArm.goToIntake(); robot.scoringClaw.open(); - robot.intakeArm.goToDropPos(); - }else { + } else { robot.intakeSlides.startPosition(); } - if (robot.intakeSlides.isSwitchPressed() && !slidesSwitchWasPressed){ - robot.scoreArm.goToIntake(); - robot.scoringClaw.open(); - robot.intakeArm.goToDropPos(); - } - }else if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { + if (robot.intakeSlides.isSwitchPressed() && !slidesSwitchWasPressed) { + robot.scoreArm.goToIntake(); + robot.scoringClaw.open(); + robot.intakeArm.goToDropPos(); + } + } else if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { robot.intakeClaw.open(); robot.scoreArm.goToPlace(); - robot.intakeArm.transfer(); - }else if (gamepad1.dpad_up){ - robot.scoreArm.manualPositionChange(MANUAL_CHANGE); - }else if (gamepad1.dpad_down){ - robot.scoreArm.manualPositionChange(-MANUAL_CHANGE); + robot.intakeArm.goToIntake(); + } else if (gamepad1.dpad_up) { + robot.scoreArm.manualPositionChange(SCORE_ARM_MANUAL_CHANGE); + } else if (gamepad1.dpad_down) { + robot.scoreArm.manualPositionChange(-SCORE_ARM_MANUAL_CHANGE); } - if (gamepad1.left_stick_button){ + if (gamepad1.left_stick_button) { robot.controlHub.resetGyro(); } - if(robot.scoreArm.isChamberContacted()) { + if (robot.scoreArm.isChamberContacted()) { robot.scoreArm.goToScoring(); } - if(gamepad1.y){ - robot.intakeClaw.close(); + if (gamepad1.y) { robot.intakeSlides.fullExtension(); } - if(gamepad1.b){ - robot.intakeClaw.close(); - robot.intakeSlides.halfExtension(); + if (gamepad1.b) { + robot.intakeArm.goToDropPos(); } - if (gamepad1.x && !XWasPressed){ - if(isSearching){ - robot.intakeClaw.close(); - robot.intakeArm.goToIntake(); - isIntaking = true; - isSearching = false; - }else { - robot.intakeArm.searching(); - isSearching = true; - } - }if (isIntaking && !gamepad1.x){ - robot.intakeClaw.close(); - robot.intakeClaw.wristFlat(); - robot.intakeArm.transfer(); - isIntaking = false; + if (gamepad1.x && !XWasPressed) { + robot.intakeClaw.wristIntake(); } - if (gamepad1.right_bumper){ + if (gamepad1.right_bumper) { robot.intakeClaw.open(); - }else if(gamepad1.right_trigger>TRIGGER_THRESHOLD){ + } else if (gamepad1.right_trigger > TRIGGER_THRESHOLD) { robot.intakeClaw.close(); } + if (robot.intakeClaw.isClawClosed() && !intakeClawWasClosed) { + robot.intakeClaw.wristTransfer(); + robot.intakeSlides.startPosition(); + robot.intakeArm.goToIntake(); + } - if (gamepad1.dpad_right){ + if (gamepad1.dpad_right) { robot.intakeClaw.adjustWrist(-MANUAL_CHANGE_AMOUNT_WRIST); - }else if(gamepad1.dpad_left){ + } else if (gamepad1.dpad_left) { robot.intakeClaw.adjustWrist(MANUAL_CHANGE_AMOUNT_WRIST); } - if (gamepad2.b){ + if (gamepad2.b) { robot.scoreArm.goToPlace(); - robot.intakeArm.transfer(); - }else if(gamepad2.x){ + robot.intakeArm.goToIntake(); + } else if (gamepad2.x) { robot.scoreArm.goToScoring(); - robot.intakeArm.transfer(); - }else if(manipulatorXWasPressed){ + robot.intakeArm.goToIntake(); + } else if (manipulatorXWasPressed) { robot.scoringClaw.open(); } else if (gamepad2.y) { robot.scoreArm.goToMove(); } - if (gamepad2.dpad_up){ - robot.intakeSlides.manualPositionChange(MANUAL_CHANGE); + if (gamepad2.dpad_up) { + robot.intakeSlides.extend(); } else if (gamepad2.dpad_down) { - robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE); + robot.intakeSlides.retract(); } - if (gamepad2.right_trigger>TRIGGER_THRESHOLD){ + if (gamepad2.right_trigger > TRIGGER_THRESHOLD) { robot.scoringClaw.open(); - }else if(gamepad2.right_bumper){ + } else if (gamepad2.right_bumper) { robot.scoringClaw.close(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java index e611535..b941071 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java @@ -5,7 +5,7 @@ @Autonomous public class PreloadAuto extends QQOpMode{ - private final int autodelay = 0; + private final int AUTO_DELAY_SECS = 0; ElapsedTime elapsedTime = new ElapsedTime(); private double step = 0; @@ -17,7 +17,7 @@ public void init(){ super.init(); robot.otos.setOtosPosition(7,-61.5,0); robot.scoringClaw.close(); - robot.intakeClaw.wristFlat(); + } public void start(){ elapsedTime.reset(); @@ -37,7 +37,7 @@ public void loop(){ if(step == 0){ robot.scoreArm.goToPlace(); - if(elapsedTime.seconds() > autodelay){ + if(elapsedTime.seconds() > AUTO_DELAY_SECS){ step = 1; } }else if(step == 1){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java index 05d7f73..b0e08ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java @@ -32,10 +32,6 @@ public void loop(){ }else if (gamepad1.b) { robot.scoringClaw.close(); robot.scoreArm.goToPlace(); - }else if (gamepad1.dpad_right){ - robot.scoringClaw.wristStart(); - }else if (gamepad1.dpad_left) { - robot.scoringClaw.wristEnd(); }else if (gamepad1.dpad_up){ robot.scoreArm.manualPositionChange(MANUAL_CHANGE); }else if (gamepad1.dpad_down){