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

Adding a rewritten teleop

Browse Source
This commit is contained in:
Sodi
2023-01-17 19:10:26 -06:00
parent 45ec239c51
commit 938c18a22e
3 changed files with 22 additions and 374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java
View File

@@ -3,6 +3,7 @@ package org.timecrafters.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;
public class PhoenixTeleOPv2 extends CyberarmState {
private double drivePower = 1;
PhoenixBot1 robot;
public PhoenixTeleOPv2(PhoenixBot1 robot) {
this.robot = robot;
@@ -12,11 +13,32 @@ public class PhoenixTeleOPv2 extends CyberarmState {
public void start() {
addParallelState(new TeleOPArmDriver(robot));
addParallelState(new TeleOPTankDriver(robot));
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
double x = engine.gamepad1.left_stick_x;
double rx = engine.gamepad1.right_stick_x;
double backLeftPower = (y - x + rx);
double backRightPower = (y + x - rx);
double frontLeftPower = (y + x + rx);
double frontRightPower = (y - x - rx);
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
}
@Override
public void init() {
}
@Override
public void exec() {
if (engine.gamepad1.left_stick_x > 0.1) {
}
}
}

168
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java
View File

@@ -41,174 +41,6 @@ public class TeleOPArmDriver extends CyberarmState {
@Override
public void exec() {
if (engine.gamepad2.dpad_left) {
robot.collectorLeft.setPower(-1);
robot.collectorRight.setPower(-1);
} else if (engine.gamepad2.dpad_right) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
} else {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
}
if (engine.gamepad2.dpad_up) {
if (robot.HighRiserLeft.getPosition() < 1.0) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
}
if (engine.gamepad2.dpad_down) {
if (robot.HighRiserLeft.getPosition() > 0.45) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
}
if (engine.gamepad2.y) {
if (robot.HighRiserLeft.getPosition() < 0.9) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
}//end of y
if (engine.gamepad2.a) {
if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() > 0.45) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
}//end of a
if (engine.gamepad2.back) {
robot.backLeftDrive.setPower(1);
robot.backRightDrive.setPower(1);
robot.frontLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
if (System.currentTimeMillis() - lastStepTime >= 1500) {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
}
if (System.currentTimeMillis() - lastStepTime >= 150) {
if (robot.HighRiserLeft.getPosition() < 1) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
}
if (System.currentTimeMillis() - lastStepTime >= 150) {
if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
}
if (System.currentTimeMillis() >= 250) {
robot.backLeftDrive.setPower(1);
robot.backRightDrive.setPower(1);
robot.frontLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
if (System.currentTimeMillis() - lastStepTime >= 250) {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
}
}
}
if (engine.gamepad2.start) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
switch (CyclingArmUpAndDown) {
// upper arm up
case 0:
if (robot.HighRiserLeft.getPosition() < 1) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// lower arm up
case 1:
if (robot.LowRiserLeft.getPosition() < 1) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// lower arm down
case 2:
if (robot.LowRiserLeft.getPosition() >= 0.44) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// upper arm down
case 3:
if (robot.HighRiserLeft.getPosition() >= 0.45) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = 0;
}
break;
default:
break;
} // end of switch
}// end of time if statement
}// end of start button press
gamepad2Checker.update();
}
}

206
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java
View File

@@ -33,213 +33,7 @@ public class TeleOPTankDriver extends CyberarmState {
@Override
public void exec() {
if (engine.gamepad1.right_trigger > 0) {
drivePower = engine.gamepad1.right_trigger;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
if (engine.gamepad1.left_trigger > 0) {
drivePower = engine.gamepad1.left_trigger;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
drivePower = engine.gamepad1.left_stick_y;
robot.backRightDrive.setPower(drivePower * 0.95);
robot.frontRightDrive.setPower(drivePower * 0.95);
}
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
drivePower = engine.gamepad1.right_stick_y;
robot.backLeftDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
}
if (engine.gamepad1.right_trigger < 0.1 &&
engine.gamepad1.left_trigger < 0.1 &&
!engine.gamepad1.y &&
!engine.gamepad1.x &&
!engine.gamepad1.a &&
!engine.gamepad1.b &&
!engine.gamepad1.dpad_left &&
!engine.gamepad1.dpad_right &&
Math.abs (engine.gamepad1.left_stick_y) < 0.1 &&
Math.abs(engine.gamepad1.right_stick_y) < 0.1) {
drivePower = 0;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (engine.gamepad1.a) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 180;
CalculateDeltaRotation();
if (RobotRotation < 0 && RobotRotation > -179) {
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
else if (RobotRotation > 0) {
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
else if (RobotRotation <= -179 || RobotRotation >= 179) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.y) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 0;
CalculateDeltaRotation();
if (RobotRotation < -1) {
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 1) {
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > -1 && RobotRotation < 1) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -45;
CalculateDeltaRotation();
if (RobotRotation > -45 && RobotRotation <= 135) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -45 || RobotRotation > 136) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -44 && RobotRotation > -46) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -90;
CalculateDeltaRotation();
if (RobotRotation < 90 && RobotRotation < -89) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower * 0.95);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower * 0.95);
}
if (RobotRotation > 90 || RobotRotation < -91) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > -91 && RobotRotation < -89) {
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;
RotationTarget = 45;
CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -135 || RobotRotation < 46) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 46 && RobotRotation > 44) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 90;
CalculateDeltaRotation();
if (RobotRotation > -90 && RobotRotation < 89) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -90 || RobotRotation > 91) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 91 && RobotRotation > 89) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
gamepad1Checker.update();
}
public void CalculateDeltaRotation() {
if (RotationTarget >= 0 && RobotRotation >= 0) {