diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index 9e05cf3..ee1c947 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -22,18 +22,22 @@ public class Arm { private double armTempPos; private double doorPos; private double wristPos; - public static double ARM_KP = 0.2; + public static double ARM_KP = 0.18; public static double doorOpenPos = 0.36; public static double doorClosePos = 0.61; public static double armStart = 0.24; public static double armScore = 0.95; + + public static double armScore_highMacro = 0.95; public static double wristStart = 0.29; public static double wristScore = 0.5; + public static double wristScore_highMacro = 0.3; + public enum Position { - INTAKE, SCORE + INTAKE, SCORE, SCOREHI } public enum DoorPosition { @@ -81,6 +85,9 @@ public class Arm { wristPos = wristStart; } else if (pos == Position.SCORE) { wristPos = wristScore; + } else if (pos == Position.SCOREHI) { + wristPos = wristScore_highMacro; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java index a61f7e4..e185e39 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -12,6 +13,8 @@ import com.qualcomm.robotcore.hardware.Servo; public class Intake { private Servo rServo; private Servo lServo; + private CRServo WheelServo; + private DcMotor dcMotor; private Position pos = Position.UP; @@ -45,6 +48,7 @@ public class Intake { lServo.setDirection(Servo.Direction.REVERSE); rServo = hardwareMap.get(Servo.class, "Right Intake Servo"); dcMotor = hardwareMap.get(DcMotor.class, "Intakemotor"); + WheelServo = hardwareMap.get(CRServo.class, "Wheel"); } @@ -92,6 +96,20 @@ public class Intake { } } + + public void wheel_spit(){ + + WheelServo.setPower(1); + } + + public void wheel_swallow(){ + WheelServo.setPower(-1); + } + + public void wheel_stop(){ + WheelServo.setPower(0); + } + public double getPower() { return dcMotor.getPower(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 014a3c1..b5a2943 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -106,6 +106,85 @@ public class Robot { break; } } + public void extendMacro_auto(int slidePos, double runTime, int slidepos2 ) { + switch(macroState) { + case(0): + macroStartTime = runTime; + slides.setTarget(slidePos); + arm.setDoor(CLOSE); + macroState ++; + break; + case(1): + if (runTime > macroStartTime + slideWait || slides.atTarget()) { + macroState ++; + } + break; + case(2): + arm.setArmPos(Arm.Position.SCORE); + arm.setWristPos(Arm.Position.SCORE); + macroState++; + break; + case (3): + if(arm.armAtPosition()){ + macroState = 0; + lastMacro = runningMacro; + runningMacro = 0; + macroState++; + } + break; + + case (4): + slides.setTarget(slidepos2); + break; + } + } + + + public void resetMacro_auto(int slidePos2, double runTime, int slidePos) { + switch(macroState) { + case(0): + macroStartTime = runTime; + arm.setDoor(OPEN); + macroState++; + break; + case(1): + if (runTime > macroStartTime + scoreWait) { + slides.setTarget(slidePos); + macroState ++; + } + break; + case(2): + macroStartTime = runTime; + arm.setArmPos(Arm.Position.INTAKE); + arm.setWristPos(Arm.Position.INTAKE); + macroState++; + break; + case(3): + if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) { + macroState ++; + } + break; + case(4): + slides.setTarget(slidePos2); + macroState++; + break; + case (5): + if (slides.atTarget()){ + macroState = 0; + lastMacro = runningMacro; + runningMacro = 0; + } + break; + } + } + + + + + + + + public void update(double runTime) { drive.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index a5f097c..c70140a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -17,7 +17,7 @@ public class Slides { // public static double d = 0; // public static double f = 0.01; //p was 0.0014 - public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.05,0,0.01); + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.04,0,0.01); public static double pTolerance = 20; private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); @@ -25,6 +25,8 @@ public class Slides { public static int targetMax = 830; public static int down = 0; + + public static int micro_tier1 = 50; public static int mini_tier1 = 250; public static int tier1 = 350; public static int tier2 = 500; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index 583d669..89a0eb7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -96,7 +96,7 @@ public class BlueBackStageAuto extends AutoBase { else { robot.intake.setDcMotor(0); robot.arm.setDoor(Arm.DoorPosition.CLOSE); - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1); builder = this.robot.getTrajectorySequenceBuilder(); //Scores yellow pixel switch (teamPropLocation) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index 1471f35..83b1649 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -97,7 +97,7 @@ public class RedBackStageAuto extends AutoBase { else { robot.intake.setDcMotor(0); robot.arm.setDoor(Arm.DoorPosition.CLOSE); - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro_auto(0, getRuntime(),Slides.mini_tier1); builder = this.robot.getTrajectorySequenceBuilder(); //Scores yellow pixel switch (teamPropLocation) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index c3aebb4..be34909 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -147,6 +147,13 @@ public class MainTeleOp extends OpMode { this.robot.endGameMechs.hang_init_pos(); } + if (controller1.getX().isPressed()) { + this.robot.intake.wheel_spit(); + } else if (controller1.getY().isPressed()) { + this.robot.intake.wheel_swallow(); + } else { + this.robot.intake.wheel_stop(); + } // Driver 2 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index d4535a4..d816e04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -43,7 +43,7 @@ public class Constants { public static final String WHEEL_FRONT_RIGHT = "frontRight"; public static final String WHEEL_BACK_LEFT = "backLeft"; public static final String WHEEL_BACK_RIGHT = "backRight"; - public static final String WEBCAM_NAME = "Targeting Webcam"; + public static final String WEBCAM_NAME = "FrontWebcam"; public static final String ARM = "wobbler"; public static final String CLAW = "claw"; public static final String INTAKE = "intake";