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.

Browse Source
This commit is contained in:
Sodi
2022-10-29 11:04:28 -05:00
parent dd0d92077c
commit a5dd4e20b3
1 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,6 +26,7 @@ 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;
public PrototypeTeleOPState(PrototypeBot1 robot) { public PrototypeTeleOPState(PrototypeBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -149,15 +150,16 @@ public class PrototypeTeleOPState extends CyberarmState {
if (engine.gamepad1.a) { if (engine.gamepad1.a) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation < 0) { RotationTarget = -180;
drivePower = 0.9; if (RobotRotation < 0 && RobotRotation > -177) {
drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/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 = -0.9; drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/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);
@@ -174,15 +176,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;
if (RobotRotation < -3) { if (RobotRotation < -3) {
drivePower = -0.9; drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/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 = 0.9; drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/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);
@@ -198,15 +201,16 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.x) { if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 135;
if (RobotRotation > -45 && RobotRotation < 132) {//CCW if (RobotRotation > -45 && RobotRotation < 132) {//CCW
drivePower = -0.9; drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/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 = 0.9; drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/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);
@@ -222,15 +226,16 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.b) { if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -135;
if (RobotRotation < 45 && RobotRotation > -132) {//CCW if (RobotRotation < 45 && RobotRotation > -132) {//CCW
drivePower = 0.9; drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/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 = -0.9; drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/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);