TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

fully programmed teleOp and started odo tuning

Browse Source
This commit is contained in:
SpencerPiha
2024-02-05 20:24:51 -06:00
parent 4dc220ecc3
commit 4450c7e48a
2 changed files with 69 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,9 @@ public class CompetitionRobotV1 extends Robot {
public static double xvp = -0.03, xvi = 0, xvd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0;
public static double yvp = 0.03, yvi = 0, yvd = 0;
public double Dnl1;
public double Dnr2;
public double xMultiplier = 1;
public double yMultiplier = 1;
public double positionX = 1000;
@@ -121,8 +124,8 @@ public class CompetitionRobotV1 extends Robot {
public int target;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 1;
public double shoulderDeposit = 1;
public double shoulderCollect = 0.86;
public double shoulderDeposit = 0.86;
public double shoulderPassive = 1;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
@@ -167,7 +170,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.FORWARD);
clawArm.setDirection(DcMotorSimple.Direction.REVERSE);
lift.setDirection(DcMotorSimple.Direction.FORWARD);
chinUp.setDirection(DcMotorSimple.Direction.FORWARD);
@@ -246,14 +249,17 @@ public class CompetitionRobotV1 extends Robot {
oldLeftPosition = currentLeftPosition;
oldAuxPosition = currentAuxPosition;
currentRightPosition = frontLeft.getCurrentPosition();
currentLeftPosition = -backRight.getCurrentPosition();
currentAuxPosition = backLeft.getCurrentPosition();
currentRightPosition = frontRight.getCurrentPosition();
currentLeftPosition = backLeft.getCurrentPosition();
currentAuxPosition = -frontLeft.getCurrentPosition();
int dnl1 = currentLeftPosition - oldLeftPosition;
int dnr2 = currentRightPosition - oldRightPosition;
int dna3 = currentAuxPosition - oldAuxPosition;
Dnl1 = dnl1;
Dnr2 = dnr2;
// the robot has turned and moved a tiny bit between two measurements
double dtheta = cm_per_tick * (dnr2 - dnl1) / L;
double dx = cm_per_tick * (dnl1 + dnr2) / 2.0;

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

@@ -70,6 +70,7 @@ public class CompetitionTeleOpState extends CyberarmState {
// chin up servo
public static double chinUpServoUp = 0.58;
public static double chinUpServoDown = 1;
public long lastExecutedTime;
@@ -201,30 +202,68 @@ public class CompetitionTeleOpState extends CyberarmState {
}
public void ArmPosControl() {
if (engine.gamepad2.a) {
if (engine.gamepad2.a && !engine.gamepad2.back) {
armPos = "collect";
depositMode = false;
} else if (engine.gamepad2.y) {
} else if (engine.gamepad2.y && !engine.gamepad2.back) {
armPos = "deposit";
depositMode = true;
} else if (engine.gamepad2.b) {
} else if (engine.gamepad2.b && !engine.gamepad2.start) {
armPos = "hover";
depositMode = true;
} else if (engine.gamepad2.x) {
} else if (engine.gamepad2.x && !engine.gamepad2.start) {
armPos = "passive";
depositMode = true;
} else if (engine.gamepad2.dpad_left) {
} else if (engine.gamepad2.dpad_left && !engine.gamepad2.start) {
armPos = "lift up";
depositMode = true;
} else if (engine.gamepad2.dpad_right) {
} else if (engine.gamepad2.dpad_right && !engine.gamepad2.start) {
armPos = "lift down";
depositMode = false;
} else if (engine.gamepad2.back) {
armPos = "reset";
} else if (engine.gamepad2.right_bumper) {
armPos = "tuning up";
} else if (engine.gamepad2.left_bumper) {
armPos = "tuning down";
}
if (Objects.equals(armPos, "tuning up")) {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
robot.lift.setPower(0);
if (System.currentTimeMillis() - lastExecutedTime > 200){
robot.shoulderCollect = robot.shoulderCollect + 0.02;
lastExecutedTime = System.currentTimeMillis();
}
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.chinUpServo.setPosition(chinUpServoDown);
target = 0;
armPos = "collect";
}
}
if (Objects.equals(armPos, "tuning down")) {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
robot.lift.setPower(0);
if (System.currentTimeMillis() - lastExecutedTime > 200) {
robot.shoulderCollect = robot.shoulderCollect - 0.02;
lastExecutedTime = System.currentTimeMillis();
}
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.chinUpServo.setPosition(chinUpServoDown);
target = 0;
armPos = "collect";
}
}
if (Objects.equals(armPos, "collect")) {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
@@ -296,11 +335,13 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lastExecutedTime = System.currentTimeMillis();
}
@Override
public void exec() {
robot.OdometryLocalizer();
if (engine.gamepad2.start && engine.gamepad2.x){
robot.shootServo.setPosition(releasePos);
@@ -372,6 +413,14 @@ public class CompetitionTeleOpState extends CyberarmState {
@Override
public void telemetry () {
engine.telemetry.addData("Dnl1", robot.Dnl1);
engine.telemetry.addData("Dnr2", robot.Dnr2);
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("Lift Encoder Pos", robot.lift.getCurrentPosition());
engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));