diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java new file mode 100644 index 0000000..734b352 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java @@ -0,0 +1,56 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Competition Blue Close") + +public class CompetitionBlueClose extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.imu.resetYaw(); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Competition Blue Close"); + this.robot.setup(); + addState(new ClawFingerState(robot,"Competition Blue Close", "01-0-00")); + // drive to and face towards the right scenario + addState(new DriveToCoordinatesState(robot,"Competition Blue Close", "02-0-01")); + // check if its in the correct Position; + addState(new ClawArmState(robot,"Competition Blue Close", "03-0-02")); + addState(new ClawFingerState(robot,"Competition Blue Close", "04-0-04")); + addState(new DriveToCoordinatesState(robot,"Competition Blue Close", "05-0-01")); + addState(new ClawArmState(robot,"Competition Blue Close", "06-0-06")); + + + + + + + + + + + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java index 94c40de..8784259 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java @@ -1,8 +1,10 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; @@ -15,16 +17,40 @@ public class CompetitionRedClose extends CyberarmEngine { CompetitionRobotV1 robot; + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.imu.resetYaw(); + } @Override public void setup() { this.robot = new CompetitionRobotV1("Competition Red Close"); this.robot.setup(); + this.robot.setup(); + addState(new ClawFingerState(robot,"Competition Red Close", "01-0-00")); + // drive to and face towards the right scenario + addState(new DriveToCoordinatesState(robot,"Competition Red Close", "02-0-01")); + // check if its in the correct Position; + addState(new ClawArmState(robot,"Competition Red Close", "03-0-02")); + addState(new ClawFingerState(robot,"Competition Red Close", "04-0-04")); + addState(new DriveToCoordinatesState(robot,"Competition Red Close", "05-0-01")); + addState(new ClawArmState(robot,"Competition Red Close", "06-0-06")); + + + + + + + + + + + - addState(new DriveToCoordinatesState(robot,"Competition Red Close", "00-1")); - addState(new DistanceCheckState(robot,"Competition Red Close", "00-2")); - addState(new ClawArmState(robot,"Competition Red Close", "01-0")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java new file mode 100644 index 0000000..1af6af9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java @@ -0,0 +1,119 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class CameraVisionState extends CyberarmState { + + CompetitionRobotV1 robot; + ExamplePipeline pipeline; + + public CameraVisionState(CompetitionRobotV1 robot) { + this.robot = robot; + } + + @Override + public void init() { + super.init(); + + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + robot.webcam1 = OpenCvCameraFactory.getInstance().createWebcam(robot.webCamName, cameraMonitorViewId); + robot.webcam1.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + public void onOpened() { + robot.webcam1.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); + } + + public void onError(int errorCode) { + } + }); + + pipeline = new ExamplePipeline(); + robot.webcam1.setPipeline(pipeline); + + } + + @Override + public void exec() { + // odometry driving ALWAYS +// robot.DriveToCoordinates(); +// robot.OdometryLocalizer(); + + + } + + @Override + public void telemetry() { + engine.telemetry.addData("object pos", pipeline.objectPos()); + engine.telemetry.addData("left side pixel amount", pipeline.leftavgfin); + engine.telemetry.addData("middle side pixel amount", pipeline.middleavgfin); + engine.telemetry.addData("right side pixel amount", pipeline.rightavgfin); + engine.telemetry.addData("object pos", pipeline.objectPos()); + } + + class ExamplePipeline extends OpenCvPipeline { + Mat YCbCr = new Mat(); + Mat leftCrop; + Mat middleCrop; + Mat rightCrop; + double leftavgfin; + double middleavgfin; + double rightavgfin; + Mat output = new Mat(); + Scalar rectColor = new Scalar(255.0, 0.0, 0.0); + + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, YCbCr, Imgproc.COLOR_RGB2YCrCb); + + Rect leftRect = new Rect(1, 1, 212, 359); + Rect rightRect = new Rect(213, 1, 212, 359); + Rect middleRect = new Rect(426, 1, 212, 359); + + input.copyTo(output); + Imgproc.rectangle(output, leftRect, rectColor, 2); + Imgproc.rectangle(output, rightRect, rectColor, 2); + Imgproc.rectangle(output, middleRect, rectColor, 2); + + leftCrop = YCbCr.submat(leftRect); + rightCrop = YCbCr.submat(rightRect); + middleCrop = YCbCr.submat(middleRect); + + Core.extractChannel(leftCrop, leftCrop, 2); + Core.extractChannel(middleCrop, middleCrop, 2); + Core.extractChannel(rightCrop, rightCrop, 2); + + Scalar leftavg = Core.mean(leftCrop); + Scalar middleavg = Core.mean(middleCrop); + Scalar rightavg = Core.mean(rightCrop); + + leftavgfin = leftavg.val[0]; + rightavgfin = rightavg.val[0]; + middleavgfin = middleavg.val[0]; + + return (output); + + } + + public String objectPos() { + if (leftavgfin > rightavgfin && leftavgfin > middleavgfin) { + return "LEFT"; + } + else if (rightavgfin > leftavgfin && rightavgfin > middleavgfin) { + return "RIGHT"; + } + else { + return "MIDDLE"; + } + } + } +} + diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java index b3a713e..5838612 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java @@ -61,8 +61,8 @@ public class ClawArmState extends CyberarmState { robot.armPos = armPos; robot.clawArmControl(); - if (robot.clawArm.getCurrentPosition() >= robot.target - 20 && robot.clawArm.getCurrentPosition() <= robot.target + 20) { -// setHasFinished(true); + if (robot.power < 0.1) { + setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawFingerState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawFingerState.java new file mode 100644 index 0000000..37d8607 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawFingerState.java @@ -0,0 +1,84 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config +public class ClawFingerState extends CyberarmState { + + CompetitionRobotV1 robot; + public boolean leftOpen; + public boolean rightOpen; + public long waitTime; + public boolean armDrive; + public long initTime; + + + + public ClawFingerState(CompetitionRobotV1 robot, String groupName, String actionName) { + this.robot = robot; + this.leftOpen = robot.configuration.variable(groupName, actionName, "leftOpen").value(); + this.rightOpen = robot.configuration.variable(groupName, actionName, "rightOpen").value(); + this.waitTime = robot.configuration.variable(groupName, actionName, "waitTime").value(); + this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value(); + + + } + + @Override + public void start() { + initTime = System.currentTimeMillis(); + } + + @Override + public void exec() { + // odometry driving ALWAYS + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); + if (armDrive) { + robot.clawArmControl(); + } + + if (!leftOpen){ + robot.leftClaw.setPosition(0.25); + } else { + robot.leftClaw.setPosition(0.6); + } + + if (!rightOpen){ + robot.rightClaw.setPosition(0.6); + } else { + robot.rightClaw.setPosition(0.25); + } + + if (runTime() > waitTime){ + setHasFinished(true); + } + + + + + } + + + @Override + public void telemetry() { + engine.telemetry.addData("x pos", robot.positionX); + engine.telemetry.addData("y pos", robot.positionY); + engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); + engine.telemetry.addData("aux encoder", robot.currentAuxPosition); + engine.telemetry.addData("left encoder", robot.currentLeftPosition); + engine.telemetry.addData("right encoder", robot.currentRightPosition); + engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("front right power", robot.frontRightPower); + engine.telemetry.addData("front left power", robot.frontLeftPower); + engine.telemetry.addData("back right power", robot.backRightPower); + engine.telemetry.addData("back left power", robot.backLeftPower); + engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition()); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java index 5c1315f..9ce755a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java @@ -12,11 +12,12 @@ import dev.cyberarm.engine.V2.CyberarmState; public class DistanceCheckState extends CyberarmState { CompetitionRobotV1 robot; - public String armPos; public long lastCheckedTIme = System.currentTimeMillis(); + public String objectPosCheck; public DistanceCheckState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; + this.objectPosCheck = robot.configuration.variable(groupName, actionName, "objectPosCheck").value(); } @Override @@ -25,32 +26,26 @@ public class DistanceCheckState extends CyberarmState { robot.DriveToCoordinates(); robot.OdometryLocalizer(); - if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > -47 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < -43){ - robot.objectPos = "Right"; - } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000){ - robot.hTarget = 0; - } - if (robot.objectPos != "right") - if (Math.abs(robot.backLeftPower) < 0.15 && + if (robot.armPos.equals("right") || robot.armPos.equals("middle") || robot.armPos.equals("left")){ + setHasFinished(true); + } else { + if (Math.abs(robot.backLeftPower) < 0.15 && Math.abs(robot.backRightPower) < 0.15 && Math.abs(robot.frontLeftPower) < 0.15 && - Math.abs(robot.frontRightPower) < 0.15 && System.currentTimeMillis() - lastCheckedTIme > 2000){ - if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 2000){ - robot.objectPos = "middle"; - } else { - robot.hTarget = 45; - robot.objectPos = "left"; + Math.abs(robot.frontRightPower) < 0.15) { + if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) { + robot.objectPos = objectPosCheck; + setHasFinished(true); + } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) { + if (robot.loopCheck == 1 && robot.objectPos != objectPosCheck){ + robot.objectPos = "left"; + setHasFinished(true); + } + robot.loopCheck += 1; + setHasFinished(true); + } } } - - if (robot.objectPos.equals("left") || robot.objectPos.equals("right") || robot.objectPos.equals("middle")){ - setHasFinished(true); - } - - - - - } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java index 7ef6c60..25fae43 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -12,11 +12,13 @@ import dev.cyberarm.engine.V2.CyberarmState; public class DriveToCoordinatesState extends CyberarmState { CompetitionRobotV1 robot; - public static double xTarget = 0; - public static double yTarget = 0; - public static double hTarget= 0; + public double xTarget = 0; + public double yTarget = 0; + public double hTarget = 0; public boolean posAchieved = false; public boolean armDrive; + public String objectPos; +// public boolean posSpecific; public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; @@ -24,6 +26,8 @@ public class DriveToCoordinatesState extends CyberarmState { this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value(); + this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value(); +// this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value(); } @Override @@ -33,23 +37,24 @@ public class DriveToCoordinatesState extends CyberarmState { robot.xTarget = xTarget; robot.DriveToCoordinates(); robot.OdometryLocalizer(); - - - if (armDrive){ + if (armDrive) { robot.clawArmControl(); } if (posAchieved){ setHasFinished(true); } else { - if (Math.abs(robot.backLeftPower) < 0.15 && - Math.abs(robot.backRightPower) < 0.15 && - Math.abs(robot.frontLeftPower) < 0.15 && - Math.abs(robot.frontRightPower) < 0.15){ - posAchieved = true; +// if (objectPos.equals(robot.objectPos) || objectPos.equals("everything") ) { + if (Math.abs(robot.backLeftPower) < 0.15 && + Math.abs(robot.backRightPower) < 0.15 && + Math.abs(robot.frontLeftPower) < 0.15 && + Math.abs(robot.frontRightPower) < 0.15) { + posAchieved = true; +// } + } } } - } + @Override @@ -65,6 +70,8 @@ public class DriveToCoordinatesState extends CyberarmState { engine.telemetry.addData("front left power", robot.frontLeftPower); engine.telemetry.addData("back right power", robot.backRightPower); engine.telemetry.addData("back left power", robot.backLeftPower); + engine.telemetry.addData("global object position", robot.objectPos); + engine.telemetry.addData("local object position", objectPos); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/CameraTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/CameraTestEngine.java new file mode 100644 index 0000000..08208f2 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/CameraTestEngine.java @@ -0,0 +1,40 @@ +package org.timecrafters.CenterStage.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "camera test") + +public class CameraTestEngine extends CyberarmEngine { + + CompetitionRobotV1 robot; + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.imu.resetYaw(); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Competition camera test"); + this.robot.setup(); + addState(new CameraVisionState(robot)); + + + + + + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index 7d827d5..ff2559b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -12,7 +12,12 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvWebcam; import org.timecrafters.Library.Robot; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; @@ -28,13 +33,17 @@ public class CompetitionRobotV1 extends Robot { public String objectPos; // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: + public double power; + public OpenCvWebcam webcam1 = null; + public WebcamName webCamName; public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp; public DcMotor odometerR, odometerL, odometerA; public IMU imu; - public Servo shoulder, elbow, leftClaw, rightClaw; + public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo; public DistanceSensor customObject; + public int loopCheck = 0; // ----------------------------------------------------------------------------------------------------------------- odometry variables: public static double Hp = 0.8, Hi = 0, Hd = 0; @@ -47,9 +56,9 @@ public class CompetitionRobotV1 extends Robot { public double positionX = 1000; public double positionY = 1000; public double positionH = 0; - public double xTarget; - public double yTarget; - public double hTarget; + public double xTarget = 1000; + public double yTarget = 1000; + public double hTarget = 0; public int currentRightPosition = 0; public int currentLeftPosition = 0; @@ -79,15 +88,20 @@ public class CompetitionRobotV1 extends Robot { public double frontRightPower; public double backRightPower; + public double yMaxPower = 1; + public double xMaxPower = 1; + public double pidX; + public double pidY; + //-------------------------------------------------------------------------------------------------------------- arm sequence variables: PIDController pidController; public String armPos; public int target; public static double p = 0.0015, i = 0, d = 0, f = 0; - public static double shoulderCollect = 0.25; + public static double shoulderCollect = 0.27; public static double shoulderDeposit = 0.32; public static double shoulderPassive = 0.8; - public static double elbowCollect = 0; + public static double elbowCollect = 0.02; public static double elbowDeposit = 0; public static double elbowPassive = 0; @@ -114,6 +128,8 @@ public class CompetitionRobotV1 extends Robot { this.hardwareMap = CyberarmEngine.instance.hardwareMap; this.engine = CyberarmEngine.instance; + webCamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + imu = engine.hardwareMap.get(IMU.class, "imu"); //-------------------------------------------------------------------------------------------------------------------------- MOTORS: @@ -137,7 +153,6 @@ public class CompetitionRobotV1 extends Robot { frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -153,6 +168,7 @@ public class CompetitionRobotV1 extends Robot { backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //IMU IMU.Parameters parameters = new IMU.Parameters( @@ -162,8 +178,6 @@ public class CompetitionRobotV1 extends Robot { imu.initialize(parameters); - imu.resetYaw(); - configuration = new TimeCraftersConfiguration("Blue Crab"); initConstants(); @@ -177,6 +191,8 @@ public class CompetitionRobotV1 extends Robot { elbow = hardwareMap.servo.get("elbow"); leftClaw = hardwareMap.servo.get("leftClaw"); rightClaw = hardwareMap.servo.get("rightClaw"); + chinUpServo = hardwareMap.servo.get("chinUpServo"); + elbow.setDirection(Servo.Direction.REVERSE); @@ -225,6 +241,14 @@ public class CompetitionRobotV1 extends Robot { } return radians; } + + /** + * A Controller taking error of the heading position and converting to a power in the direction of a target. + * @param reference reference is the target position + * @param current current is the measured sensor value. + * @return A power to the target the position + * + */ public double HeadingPIDControl(double reference, double current){ double error = angleWrap(current - reference); headingIntegralSum += error * headingTimer.seconds(); @@ -265,8 +289,20 @@ public class CompetitionRobotV1 extends Robot { double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); - double pidX = YPIDControl(yTarget, positionY); - double pidY = XPIDControl(xTarget, positionX); + if (xMaxPower > XPIDControl(xTarget, positionX)){ + pidX = xMaxPower; + } else { + pidX = YPIDControl(yTarget, positionY); + } + + if (yMaxPower > YPIDControl(yTarget, positionY)){ + pidY = yMaxPower; + } else { + pidY = XPIDControl(xTarget, positionX); + } + + pidY = XPIDControl(xTarget, positionX); + double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); @@ -294,7 +330,7 @@ public class CompetitionRobotV1 extends Robot { int armPos = clawArm.getCurrentPosition(); double pid = pidController.calculate(armPos, target); - double power = pid; + power = pid; clawArm.setPower(power); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java index 246e8d1..abfbbc8 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -60,6 +60,10 @@ public class CompetitionTeleOpState extends CyberarmState { // ---------------------------------------------------------------------------------------------------------------Arm control Variables: public String armPos = "passive"; + // chin up servo + public static double chinUpServoUp = 1; + public static double chinUpServoDown = 0.25; + @@ -197,6 +201,7 @@ public class CompetitionTeleOpState extends CyberarmState { public void ArmPosControl(){ + if (engine.gamepad2.a){ armPos = "collect"; depositMode = false; @@ -210,15 +215,23 @@ public class CompetitionTeleOpState extends CyberarmState { else if (engine.gamepad2.b){ armPos = "hover"; depositMode = true; + } else if (engine.gamepad2.dpad_left){ + armPos = "lift up"; + depositMode = true; + } else if (engine.gamepad2.dpad_right){ + armPos = "lift down"; + depositMode = false; } if (armPos == "collect"){ if (robot.lift.getCurrentPosition() >= 1){ + robot.chinUpServo.setPosition(chinUpServoDown); robot.lift.setPower(-0.6); } else { robot.lift.setPower(0); robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); + robot.chinUpServo.setPosition(chinUpServoDown); target = 10; } @@ -226,8 +239,10 @@ public class CompetitionTeleOpState extends CyberarmState { if (armPos == "passive"){ if (robot.lift.getCurrentPosition() >= 1){ robot.lift.setPower(-0.6); + robot.chinUpServo.setPosition(chinUpServoDown); } else { robot.lift.setPower(0); + robot.chinUpServo.setPosition(chinUpServoDown); robot.shoulder.setPosition(robot.shoulderPassive); robot.elbow.setPosition(robot.elbowPassive); target = 850; @@ -237,23 +252,43 @@ public class CompetitionTeleOpState extends CyberarmState { robot.shoulder.setPosition(robot.shoulderDeposit); robot.elbow.setPosition(robot.elbowDeposit); target = 370; + robot.chinUpServo.setPosition(chinUpServoDown); + } - if (armPos == "hover"){ - + if (armPos == "hover") { robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); target = 120; - } + if (armPos == "lift up") { + robot.shoulder.setPosition(robot.shoulderDeposit); + robot.elbow.setPosition(robot.elbowDeposit); + target = 120; + robot.chinUpServo.setPosition(chinUpServoUp); + } + + if (armPos == "lift down"){ + if (robot.lift.getCurrentPosition() >= 1){ + robot.lift.setPower(-0.6); + robot.chinUpServo.setPosition(chinUpServoDown); + } else { + robot.lift.setPower(0); + robot.chinUpServo.setPosition(chinUpServoDown); + robot.shoulder.setPosition(robot.shoulderPassive); + robot.elbow.setPosition(robot.elbowPassive); + target = 850; + } + } + } @Override public void init() { - robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - pidController = new PIDController(p, i, d); + super.init(); + pidController = new PIDController(p, i, d); + }