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

Slowing the button rotation on TeleOP, Sprint 3 TeleOP, take 6. (CAW caw caw CAWAW awcaw CAW)

Browse Source
This commit is contained in:
Sodi
2022-10-30 15:06:24 -05:00
parent 0dedca5f7f
commit 8e5fffee7e
1 changed files with 90 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
View File

@@ -26,12 +26,27 @@ public class PrototypeTeleOPState extends CyberarmState {
private int DriveForwardAndBack, AutoChain; private int DriveForwardAndBack, AutoChain;
private int RobotPosition, RobotStartingPosition; private int RobotPosition, RobotStartingPosition;
private double RobotRotation; private double RobotRotation;
private double RotationTarget; private double RotationTarget, DeltaRotation;
public PrototypeTeleOPState(PrototypeBot1 robot) { public PrototypeTeleOPState(PrototypeBot1 robot) {
this.robot = robot; this.robot = robot;
} }
public void CalculateDeltaRotation() {
if (RotationTarget >= 0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation);
}
else if (RotationTarget <= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation);
}
else if (RotationTarget >= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation);
}
else if (RotationTarget <=0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RobotRotation - RotationTarget);
}
}
@Override @Override
public void telemetry() { public void telemetry() {
@@ -42,6 +57,7 @@ public class PrototypeTeleOPState extends CyberarmState {
engine.telemetry.addData("AutoChain", AutoChain); engine.telemetry.addData("AutoChain", AutoChain);
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle); engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("Drive Power", drivePower); engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
// engine.telemetry.addData("Cycling Arm up / Down Case", CyclingArmUpAndDown); // engine.telemetry.addData("Cycling Arm up / Down Case", CyclingArmUpAndDown);
} }
@@ -140,6 +156,8 @@ public class PrototypeTeleOPState extends CyberarmState {
!engine.gamepad1.x && !engine.gamepad1.x &&
!engine.gamepad1.a && !engine.gamepad1.a &&
!engine.gamepad1.b && !engine.gamepad1.b &&
!engine.gamepad1.dpad_left &&
!engine.gamepad1.dpad_right &&
Math.abs (engine.gamepad1.left_stick_y) < 0.1 && Math.abs (engine.gamepad1.left_stick_y) < 0.1 &&
Math.abs(engine.gamepad1.right_stick_y) < 0.1) { Math.abs(engine.gamepad1.right_stick_y) < 0.1) {
drivePower = 0; drivePower = 0;
@@ -152,15 +170,16 @@ public class PrototypeTeleOPState extends CyberarmState {
if (engine.gamepad1.a) { if (engine.gamepad1.a) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -180; RotationTarget = -180;
CalculateDeltaRotation();
if (RobotRotation < 0 && RobotRotation > -177) { if (RobotRotation < 0 && RobotRotation > -177) {
drivePower = (1 * ((RobotRotation) - (RotationTarget))/180) + 0.3; drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation > 0) { if (RobotRotation > 0) {
drivePower = (-1 * ((RobotRotation) - (RotationTarget))/180) - 0.3; drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -178,15 +197,16 @@ public class PrototypeTeleOPState extends CyberarmState {
if (engine.gamepad1.y) { if (engine.gamepad1.y) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 0; RotationTarget = 0;
CalculateDeltaRotation();
if (RobotRotation < -3) { if (RobotRotation < -3) {
drivePower = (-1 * ((RobotRotation) - (RotationTarget))/180) - 0.3; drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation > 3) { if (RobotRotation > 3) {
drivePower = (1 * ((RobotRotation) - (RotationTarget))/180) + 0.3; drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -200,18 +220,19 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
} }
if (engine.gamepad1.x) { if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 135; RotationTarget = 135;
CalculateDeltaRotation();
if (RobotRotation > -45 && RobotRotation < 132) {//CCW if (RobotRotation > -45 && RobotRotation < 132) {//CCW
drivePower = (-1 * ((RobotRotation) - (RotationTarget))/180) - 0.3; drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation < -45 || RobotRotation > 138) {//CW if (RobotRotation < -45 || RobotRotation > 138) {//CW
drivePower = (1 * ((RobotRotation) - (RotationTarget))/180) + 0.3; drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -225,18 +246,47 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
} }
if (engine.gamepad1.b) {
if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 90;
CalculateDeltaRotation();
if (RobotRotation > -90 && RobotRotation < 87) {//CCW
drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -90 || RobotRotation > 93) {//CW
drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 93 && RobotRotation > 87) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.dpad_right) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -135; RotationTarget = -135;
CalculateDeltaRotation();
if (RobotRotation < 45 && RobotRotation > -132) {//CCW if (RobotRotation < 45 && RobotRotation > -132) {//CCW
drivePower = (1 * ((RobotRotation) - (RotationTarget))/180) + 0.3; drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation > 45 || RobotRotation < -138) {//CW if (RobotRotation > 45 || RobotRotation < -138) {//CW
drivePower = (-1 * ((RobotRotation) - (RotationTarget))/180) - 0.3; drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -251,6 +301,33 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
} }
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -90;
CalculateDeltaRotation();
if (RobotRotation < 90 && RobotRotation > -87) {//CCW
drivePower = (1 * DeltaRotation/180) + 0.3;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 90 || RobotRotation < -93) {//CW
drivePower = (-1 * DeltaRotation/180) - 0.3;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -87 && RobotRotation > -93) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad2.dpad_left) { if (engine.gamepad2.dpad_left) {
robot.collectorLeft.setPower(-1); robot.collectorLeft.setPower(-1);
robot.collectorRight.setPower(-1); robot.collectorRight.setPower(-1);
@@ -283,7 +360,7 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
if (robot.HighRiserLeft.getPosition() < 1) { if (robot.HighRiserLeft.getPosition() < 0.84) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
@@ -291,7 +368,7 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
} }
if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() == 1) { if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) { if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);