TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Finished camera state, fixed memory leak problem. fixed my odometry code so it actually drives the robot again (had to remove max power) need to fix arm controls in autonomous.

Browse Source
This commit is contained in:
SpencerPiha
2023-12-29 22:35:24 -06:00
parent ae85337b5e
commit 08cfacf2ea
9 changed files with 288 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java Normal file
View File

@@ -0,0 +1,58 @@
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.CameraVisionState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "BURNSVILLE blue backdrop")
public class Competition2BlueBackStage 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();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville backdrop blue");
this.robot.setup();
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "0-00-0"));
addState(new ClawArmState(robot,"burnsville backdrop blue", "0-01-0"));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"burnsville backdrop blue", "0-02-0"));
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0"));
}
}

1
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java
View File

@@ -5,7 +5,6 @@ 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;

1
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java
View File

@@ -5,7 +5,6 @@ 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;

55
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java
View File

@@ -17,6 +17,8 @@ public class CameraVisionState extends CyberarmState {
CompetitionRobotV1 robot;
ExamplePipeline pipeline;
private long initTime;
private boolean finish = false;
public CameraVisionState(CompetitionRobotV1 robot) {
this.robot = robot;
@@ -26,6 +28,7 @@ public class CameraVisionState extends CyberarmState {
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() {
@@ -42,11 +45,25 @@ public class CameraVisionState extends CyberarmState {
}
@Override
public void start() {
super.start();
initTime = System.currentTimeMillis();
}
@Override
public void exec() {
robot.clawArmControl();
if (System.currentTimeMillis() - initTime > 3000){
robot.objectPos = pipeline.objectPos();
setHasFinished(true);
}
// odometry driving ALWAYS
// robot.DriveToCoordinates();
// robot.OdometryLocalizer();
robot.DriveToCoordinates();
robot.OdometryLocalizer();
}
@@ -58,10 +75,12 @@ public class CameraVisionState extends CyberarmState {
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 HSV = new Mat();
Mat leftCrop;
Mat middleCrop;
Mat rightCrop;
@@ -72,7 +91,7 @@ public class CameraVisionState extends CyberarmState {
Scalar rectColor = new Scalar(255.0, 0.0, 0.0);
public Mat processFrame(Mat input) {
Imgproc.cvtColor(input, YCbCr, Imgproc.COLOR_RGB2YCrCb);
Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV);
Rect leftRect = new Rect(1, 1, 212, 359);
Rect rightRect = new Rect(213, 1, 212, 359);
@@ -83,13 +102,13 @@ public class CameraVisionState extends CyberarmState {
Imgproc.rectangle(output, rightRect, rectColor, 2);
Imgproc.rectangle(output, middleRect, rectColor, 2);
leftCrop = YCbCr.submat(leftRect);
rightCrop = YCbCr.submat(rightRect);
middleCrop = YCbCr.submat(middleRect);
leftCrop = HSV.submat(leftRect);
rightCrop = HSV.submat(rightRect);
middleCrop = HSV.submat(middleRect);
Core.extractChannel(leftCrop, leftCrop, 2);
Core.extractChannel(middleCrop, middleCrop, 2);
Core.extractChannel(rightCrop, rightCrop, 2);
Core.extractChannel(leftCrop, leftCrop, 1);
Core.extractChannel(middleCrop, middleCrop, 1);
Core.extractChannel(rightCrop, rightCrop, 1);
Scalar leftavg = Core.mean(leftCrop);
Scalar middleavg = Core.mean(middleCrop);
@@ -99,21 +118,29 @@ public class CameraVisionState extends CyberarmState {
rightavgfin = rightavg.val[0];
middleavgfin = middleavg.val[0];
HSV.release();
leftCrop.release();
middleCrop.release();
rightCrop.release();
return (output);
}
public String objectPos() {
public int objectPos() {
if (leftavgfin > rightavgfin && leftavgfin > middleavgfin) {
return "LEFT";
return 1;
}
else if (rightavgfin > leftavgfin && rightavgfin > middleavgfin) {
return "RIGHT";
return 2;
}
else {
return "MIDDLE";
return 3;
}
}
}
}

40
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
@@ -12,7 +13,6 @@ public class ClawArmState extends CyberarmState {
CompetitionRobotV1 robot;
public String armPos;
public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) {
this.robot = robot;
this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value();
@@ -25,40 +25,38 @@ public class ClawArmState extends CyberarmState {
robot.OdometryLocalizer();
// driving arm to pos
if (armPos.equals("collect")){
if (robot.lift.getCurrentPosition() >= 1){
if (armPos == "collect") {
if (robot.lift.getCurrentPosition() >= 20) {
robot.lift.setPower(-0.6);
} else {
robot.lift.setPower(0);
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.target = 10;
robot.target = 30;
}
}
if (armPos.equals("passive")){
if (robot.lift.getCurrentPosition() >= 1){
robot.lift.setPower(-0.6);
} else {
robot.lift.setPower(0);
robot.shoulder.setPosition(robot.shoulderPassive);
robot.elbow.setPosition(robot.elbowPassive);
robot.target = 850;
}
}
if (armPos.equals("deposit")){
if (armPos == "deposit") {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
robot.target = 370;
robot.target = 400;
}
if (armPos.equals("hover")) {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.target = 150;
if (armPos == "hover") {
if (robot.lift.getCurrentPosition() >= 20) {
robot.lift.setPower(-0.6);
} else {
robot.shoulder.setPosition(0.38);
robot.target = 200;
}
}
if (armPos.equals("search")) {
robot.shoulder.setPosition(0.15);
robot.target = 570;
}
robot.armPos = armPos;
robot.clawArmControl();
if (robot.power < 0.1) {

69
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java
View File

@@ -1,69 +0,0 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import com.acmerobotics.dashboard.config.Config;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmState;
@Config
public class DistanceCheckState extends CyberarmState {
CompetitionRobotV1 robot;
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
public void exec() {
// odometry driving ALWAYS
robot.DriveToCoordinates();
robot.OdometryLocalizer();
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) {
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);
}
}
}
}
@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());
engine.telemetry.addData("distance ", robot.customObject.getDistance(DistanceUnit.MM));
engine.telemetry.addData("object location ", robot.objectPos);
}
}

62
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -12,48 +12,64 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class DriveToCoordinatesState extends CyberarmState {
CompetitionRobotV1 robot;
public double xTarget = 0;
public double yTarget = 0;
public double hTarget = 0;
public double xTarget;
public double yTarget;
public double hTarget;
public boolean posAchieved = false;
public boolean armDrive;
public String objectPos;
// public boolean posSpecific;
public int objectPos;
public boolean posSpecific;
public double maxXPower;
public double maxYPower;
public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
this.robot = robot;
this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value();
this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").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();
this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
}
@Override
public void exec() {
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.DriveToCoordinates();
robot.OdometryLocalizer();
if (!posSpecific) {
// enter loop
} else {
if (objectPos != robot.objectPos) {
// enter the ending loop
setHasFinished(true);
}
}
if (posAchieved) {
setHasFinished(true);
}
if (armDrive) {
robot.clawArmControl();
}
if (posAchieved){
setHasFinished(true);
} else {
// 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;
// }
}
}
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.DriveToCoordinates();
robot.OdometryLocalizer();
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;
}
}

65
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -30,26 +31,24 @@ public class CompetitionRobotV1 extends Robot {
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
public String objectPos;
public int 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, chinUpServo;
public DistanceSensor customObject;
public TouchSensor touchLeftArm, touchRightArm;
public int loopCheck = 0;
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.03, Xi = 0, Xd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0.0013;
private double drivePower = 1;
public static double Xp = -0.035, Xi = 0, Xd = 0;
public static double Yp = 0.035, Yi = 0, Yd = 0.0013;
public double rx;
public double xMultiplier = 1;
public double yMultiplier = 1;
@@ -75,7 +74,6 @@ public class CompetitionRobotV1 extends Robot {
public double headingIntegralSum = 0;
public double xIntegralSum = 0;
public double yIntegralSum = 0;
public double targetHeading;
public final double cm_per_tick = (2 * Math.PI * R) / N;
private double headingLastError = 0;
private double xLastError = 0;
@@ -83,6 +81,7 @@ public class CompetitionRobotV1 extends Robot {
ElapsedTime headingTimer = new ElapsedTime();
ElapsedTime xTimer = new ElapsedTime();
ElapsedTime yTimer = new ElapsedTime();
public double frontLeftPower;
public double backLeftPower;
public double frontRightPower;
@@ -95,15 +94,14 @@ public class CompetitionRobotV1 extends Robot {
//-------------------------------------------------------------------------------------------------------------- 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.27;
public static double shoulderDeposit = 0.32;
public static double shoulderPassive = 0.8;
public static double elbowCollect = 0.02;
public static double elbowDeposit = 0;
public static double elbowPassive = 0;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 0.38;
public double shoulderDeposit = 0.36;
public double shoulderPassive = 0.8;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
public double elbowPassive = 0;
private HardwareMap hardwareMap;
@@ -145,7 +143,7 @@ public class CompetitionRobotV1 extends Robot {
backRight.setDirection(DcMotorSimple.Direction.REVERSE);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
clawArm.setDirection(DcMotorSimple.Direction.REVERSE);
clawArm.setDirection(DcMotorSimple.Direction.FORWARD);
lift.setDirection(DcMotorSimple.Direction.FORWARD);
chinUp.setDirection(DcMotorSimple.Direction.FORWARD);
@@ -170,7 +168,7 @@ public class CompetitionRobotV1 extends Robot {
chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//IMU
// ----------------------------------------------------------------------------------------------------------------------------- IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
@@ -184,7 +182,10 @@ public class CompetitionRobotV1 extends Robot {
pidController = new PIDController(p, i, d);
// ------------------------------------------------------------------------------------------------------------------------- SENSOR:
customObject = engine.hardwareMap.get(Rev2mDistanceSensor.class, "customObject");
touchLeftArm = engine.hardwareMap.get(TouchSensor.class, "left touch");
touchRightArm = engine.hardwareMap.get(TouchSensor.class, "right touch");
//--------------------------------------------------------------------------------------------------------------------------- SERVO:
shoulder = hardwareMap.servo.get("shoulder");
@@ -196,15 +197,13 @@ public class CompetitionRobotV1 extends Robot {
elbow.setDirection(Servo.Direction.REVERSE);
// shoulder.setPosition(shoulderPassive);
// elbow.setPosition(elbowPassive);
}
// -------------------------------------------------------------------------------------------------------------------------- Functions:
public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer:
// update positions
@@ -283,25 +282,25 @@ public class CompetitionRobotV1 extends Robot {
}
public void DriveToCoordinates () {
// determine the velocities needed for each direction
// this uses PID to adjust needed Power for robot to move to target
// if (yMaxPower < XPIDControl(xTarget, positionX)){
// pidX = yMaxPower;
// } else {
// pidX = YPIDControl(yTarget, positionY);
// }
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
if (xMaxPower > XPIDControl(xTarget, positionX)){
pidX = xMaxPower;
} else {
pidX = YPIDControl(yTarget, positionY);
}
// if (xMaxPower < YPIDControl(yTarget, positionY)){
// pidY = xMaxPower;
// } else {
// pidY = XPIDControl(xTarget, positionX);
// }
if (yMaxPower > YPIDControl(yTarget, positionY)){
pidY = yMaxPower;
} else {
pidY = XPIDControl(xTarget, positionX);
}
pidY = XPIDControl(xTarget, positionX);
pidX = YPIDControl(yTarget, positionY);
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
@@ -335,4 +334,4 @@ public class CompetitionRobotV1 extends Robot {
clawArm.setPower(power);
}
}
}

159
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -18,7 +18,7 @@ public class CompetitionTeleOpState extends CyberarmState {
private CompetitionRobotV1 robot;
// ------------------------------------------------------------------------------------------------------------robot claw arm variables:
private PIDController pidController;
public static double p = 0.0015, i = 0, d = 0, f = 0;
public static double p = 0.007, i = 0, d = 0.0001, f = 0;
public static int target = 0;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
@@ -28,6 +28,7 @@ public class CompetitionTeleOpState extends CyberarmState {
public double backDropLock = Math.toRadians(90);
public double power;
public double armPower;
private double currentHeading;
private boolean headingLock = false;
@@ -52,6 +53,8 @@ public class CompetitionTeleOpState extends CyberarmState {
private boolean lbsVar1;
private double drivePower = 1;
public double rx;
public double minPower = 0;
// ------------------------------------------------------------------------------------------------------------------- Slider Variables:
private int maxExtension = 2000;
@@ -59,10 +62,10 @@ public class CompetitionTeleOpState extends CyberarmState {
public boolean depositMode = false;
// ---------------------------------------------------------------------------------------------------------------Arm control Variables:
public String armPos = "passive";
public String armPos = "collect";
// chin up servo
public static double chinUpServoUp = 1;
public static double chinUpServoDown = 0.25;
public static double chinUpServoUp = 0.58;
public static double chinUpServoDown = 1;
@@ -107,7 +110,7 @@ public class CompetitionTeleOpState extends CyberarmState {
if (headingLock){
robot.rx = HeadingPIDControl(targetHeading, currentHeading);
} else {
robot.rx = engine.gamepad1.right_stick_x;
robot.rx = engine.gamepad1.right_stick_x / 2;
}
@@ -121,9 +124,24 @@ public class CompetitionTeleOpState extends CyberarmState {
}
lbsVar1 = lbs1;
double y = engine.gamepad1.left_stick_y;
double x = -engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
// // Improve control?
// if (y < 0) {
// y = -Math.sqrt(-y);
// } else {
// y = Math.sqrt(y);
// }
//
// if (x < 0) {
// x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
// } else {
// x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing;
// }
double x = engine.gamepad1.left_stick_x;
double y = -engine.gamepad1.left_stick_y;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
// angle math to make things field oriented
@@ -139,7 +157,7 @@ public class CompetitionTeleOpState extends CyberarmState {
// setting each power determined previously from the math above
// as well as multiplying it by a drive power that can be changed.
robot.backLeft.setPower(backLeftPower * drivePower);
robot.backLeft.setPower(backLeftPower * drivePower + minPower);
robot.backRight.setPower(-backRightPower * drivePower);
robot.frontLeft.setPower(frontLeftPower * drivePower);
robot.frontRight.setPower(frontRightPower * drivePower);
@@ -199,32 +217,33 @@ public class CompetitionTeleOpState extends CyberarmState {
rbsVar2 = rbs2;
}
public void ArmPosControl(){
public void ArmPosControl() {
if (engine.gamepad2.a){
if (engine.gamepad2.a) {
armPos = "collect";
depositMode = false;
} else if (engine.gamepad2.x){
armPos = "passive";
depositMode = false;
} else if (engine.gamepad2.y){
// } else if (engine.gamepad2.x) {
// armPos = "passive";
// depositMode = false;
} else if (engine.gamepad2.y) {
armPos = "deposit";
depositMode = true;
}
else if (engine.gamepad2.b){
} else if (engine.gamepad2.b) {
armPos = "hover";
depositMode = true;
} else if (engine.gamepad2.dpad_left){
} else if (engine.gamepad2.dpad_left) {
armPos = "lift up";
depositMode = true;
} else if (engine.gamepad2.dpad_right){
} else if (engine.gamepad2.dpad_right) {
armPos = "lift down";
depositMode = false;
} else if (engine.gamepad2.back) {
armPos = "reset";
}
if (armPos == "collect"){
if (robot.lift.getCurrentPosition() >= 1){
if (armPos == "collect") {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
@@ -232,57 +251,71 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.chinUpServo.setPosition(chinUpServoDown);
target = 10;
target = 30;
}
}
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;
}
}
if (armPos == "deposit"){
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
target = 370;
robot.chinUpServo.setPosition(chinUpServoDown);
// if (armPos == "passive") {
// if (robot.lift.getCurrentPosition() >= 20) {
// 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;
// }
// }
if (armPos == "deposit") {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
target = 400;
robot.chinUpServo.setPosition(chinUpServoDown);
}
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 (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
target = 200;
}
if (armPos == "lift down"){
if (robot.lift.getCurrentPosition() >= 1){
}
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);
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
target = 850;
}
}
if (armPos == "reset") {
robot.shoulder.setPosition(robot.shoulderPassive);
if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) {
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
armPos = "collect";
}
}
}
@Override
public void init() {
@@ -294,10 +327,11 @@ public class CompetitionTeleOpState extends CyberarmState {
@Override
public void exec() {
if (engine.gamepad2.dpad_up) {
robot.chinUp.setPower(1);
} else if (engine.gamepad2.dpad_down){
robot.chinUp.setPower(-1);
} else if (engine.gamepad2.dpad_down){
robot.chinUp.setPower(1);
} else {
robot.chinUp.setPower(0);
}
@@ -336,14 +370,17 @@ public class CompetitionTeleOpState extends CyberarmState {
// ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
pidController.setPID(p, i, d);
int armPos = robot.clawArm.getCurrentPosition();
double pid = pidController.calculate(armPos, target);
// double ff = Math.cos(Math.toRadians(target / ticksInDegree)) * f;
int armCurrentPos = robot.clawArm.getCurrentPosition();
double pid = pidController.calculate(armCurrentPos, target);
double power = pid;
// double power = pid + ff;
if (armPos == "reset"){
armPower = -0.2;
} else if (armPos != "reset"){
armPower = pid;
robot.clawArm.setPower(power);
}
robot.clawArm.setPower(armPower);
// ------------------------------------------------------------------------------------------------------------------- Lift Control:
SliderTeleOp();
@@ -369,6 +406,8 @@ public class CompetitionTeleOpState extends CyberarmState {
engine.telemetry.addData("Kd", Kd);
engine.telemetry.addData("arm pos", armPos);
engine.telemetry.addData("arm pos ticks", robot.clawArm.getCurrentPosition());
engine.telemetry.addData("left touch", robot.touchLeftArm.isPressed());
engine.telemetry.addData("right touch", robot.touchRightArm.isPressed());
}
}