mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 21:22:33 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
SpencerPiha
@@ -0,0 +1,23 @@
|
||||
package org.timecrafters.testing.engine;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.testing.states.PhoenixBot1;
|
||||
import org.timecrafters.testing.states.PhoenixTeleOPState;
|
||||
|
||||
@TeleOp (name = "PhoenixTeleOP")
|
||||
|
||||
public class PhoenixTeleOP extends CyberarmEngine {
|
||||
|
||||
PhoenixBot1 robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
robot = new PhoenixBot1(this);
|
||||
|
||||
addState(new PhoenixTeleOPState(robot));
|
||||
}
|
||||
}
|
||||
@@ -1,23 +0,0 @@
|
||||
package org.timecrafters.testing.engine;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.testing.states.PrototypeBot1;
|
||||
import org.timecrafters.testing.states.PrototypeTeleOPState;
|
||||
|
||||
@TeleOp (name = "PrototypeTeleOP")
|
||||
|
||||
public class PrototypeTeleOP extends CyberarmEngine {
|
||||
|
||||
PrototypeBot1 robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
robot = new PrototypeBot1(this);
|
||||
|
||||
addState(new PrototypeTeleOPState(robot));
|
||||
}
|
||||
}
|
||||
@@ -16,7 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
|
||||
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
public class PrototypeBot1 {
|
||||
public class PhoenixBot1 {
|
||||
|
||||
private static final String TFOD_MODEL_ASSET = "PowerPlay.tflite";
|
||||
|
||||
@@ -47,7 +47,7 @@ public class PrototypeBot1 {
|
||||
public TimeCraftersConfiguration configuration;
|
||||
|
||||
|
||||
public PrototypeBot1(CyberarmEngine engine) {
|
||||
public PhoenixBot1(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
|
||||
initVuforia();
|
||||
@@ -10,30 +10,18 @@ import com.vuforia.Vuforia;
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.cyberarm.engine.V2.GamepadChecker;
|
||||
|
||||
public class PrototypeTeleOPState extends CyberarmState {
|
||||
public class PhoenixTeleOPState extends CyberarmState {
|
||||
|
||||
private final PrototypeBot1 robot;
|
||||
public boolean A;
|
||||
public boolean X;
|
||||
public boolean Y;
|
||||
private boolean bprev; // sticky key variable for the gamepad
|
||||
private final PhoenixBot1 robot;
|
||||
private double drivePower = 1;
|
||||
private boolean UpDPad;
|
||||
private double collectorRiserPosition;
|
||||
private boolean raiseHighRiser = true;
|
||||
private long lastStepTime = 0, BeginningofActionTime;
|
||||
private boolean raiseLowRiser = true;
|
||||
private double speed = 1; // used for the normal speed while driving
|
||||
private double slowSpeed = 0.5; // used for slow mode speed while driving
|
||||
private long lastStepTime = 0;
|
||||
private int CyclingArmUpAndDown = 0;
|
||||
private int DriveForwardAndBack, AutoChain;
|
||||
private int RobotPosition, RobotStartingPosition;
|
||||
private double RobotRotation;
|
||||
private double RotationTarget, DeltaRotation;
|
||||
private double MinimalPower;
|
||||
private double MinimalPower = 0.2;
|
||||
private GamepadChecker gamepad1Checker, gamepad2Checker;
|
||||
|
||||
public PrototypeTeleOPState(PrototypeBot1 robot) {
|
||||
public PhoenixTeleOPState(PhoenixBot1 robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@@ -59,12 +47,9 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition());
|
||||
engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
|
||||
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
|
||||
engine.telemetry.addData("AutoChain", AutoChain);
|
||||
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("Drive Power", drivePower);
|
||||
engine.telemetry.addData("Delta Rotation", DeltaRotation);
|
||||
// engine.telemetry.addData("Cycling Arm up / Down Case", CyclingArmUpAndDown);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -87,49 +72,6 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
//Gamepad Read
|
||||
|
||||
A = engine.gamepad1.a;
|
||||
X = engine.gamepad1.x;
|
||||
Y = engine.gamepad1.y;
|
||||
UpDPad = engine.gamepad1.dpad_up;
|
||||
|
||||
//drive speed toggle
|
||||
//
|
||||
// boolean b = engine.gamepad1.b;
|
||||
//
|
||||
// if (b && !bprev) {
|
||||
// bprev = true;
|
||||
// if (drivePower == speed) {
|
||||
// drivePower = slowSpeed;
|
||||
// } else {
|
||||
// drivePower = speed;
|
||||
// }
|
||||
// }
|
||||
// if (!b){
|
||||
// bprev = false;
|
||||
// }
|
||||
|
||||
// //Drivetrain Variables
|
||||
// double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
|
||||
// double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
|
||||
// double rx = engine.gamepad1.left_stick_x;
|
||||
//
|
||||
// // Denominator is the largest motor power (absolute value) or 1
|
||||
// // This ensures all the powers maintain the same ratio, but only when
|
||||
// // at least one is out of the range [-1, 1]
|
||||
//
|
||||
// double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
// double frontLeftPower = (y + x + rx) / denominator;
|
||||
// double backLeftPower = (y - x + rx) / denominator;
|
||||
// double frontRightPower = (y - x - rx) / denominator;
|
||||
// double backRightPower = (y + x - rx) / denominator;
|
||||
//
|
||||
// robot.frontLeftDrive.setPower(-frontLeftPower * speed);
|
||||
// robot.backLeftDrive.setPower(backLeftPower * speed);
|
||||
// robot.frontRightDrive.setPower(-frontRightPower * speed);
|
||||
// robot.backRightDrive.setPower(backRightPower * speed);
|
||||
|
||||
if (engine.gamepad1.right_trigger > 0) {
|
||||
drivePower = engine.gamepad1.right_trigger;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
@@ -180,14 +122,14 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RotationTarget = 180;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation < 0 && RobotRotation > -179) {
|
||||
drivePower = (1 * DeltaRotation/180) + 0.3;
|
||||
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) - 0.3;
|
||||
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
@@ -206,21 +148,21 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
|
||||
RotationTarget = 0;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation < -3) {
|
||||
drivePower = (-1 * DeltaRotation/180) - 0.3;
|
||||
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 > 3) {
|
||||
drivePower = (1 * DeltaRotation/180) + 0.3;
|
||||
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 > -3 && RobotRotation < 3) {
|
||||
if (RobotRotation > -1 && RobotRotation < 1) {
|
||||
drivePower = 0;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
@@ -233,14 +175,14 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RotationTarget = 135;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation > -45 && RobotRotation < 134) {//CCW
|
||||
drivePower = (-1 * DeltaRotation/180) - 0.3;
|
||||
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) + 0.3;
|
||||
drivePower = (1 * DeltaRotation/180) + MinimalPower;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
@@ -260,14 +202,14 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RotationTarget = 90;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation > -90 && RobotRotation < 89) {//CCW
|
||||
drivePower = (-1 * DeltaRotation/180) - 0.3;
|
||||
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) + 0.3;
|
||||
drivePower = (1 * DeltaRotation/180) + MinimalPower;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
@@ -287,14 +229,14 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RotationTarget = -135;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation < 45 && RobotRotation > -134) {//CCW
|
||||
drivePower = (1 * DeltaRotation/180) + 0.3;
|
||||
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) - 0.3;
|
||||
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
@@ -314,14 +256,14 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
RotationTarget = -90;
|
||||
CalculateDeltaRotation();
|
||||
if (RobotRotation < 90 && RobotRotation > -89) {//CCW
|
||||
drivePower = (1 * DeltaRotation/180) + 0.3;
|
||||
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) - 0.3;
|
||||
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
@@ -336,18 +278,6 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
}
|
||||
}
|
||||
|
||||
// if (engine.gamepad1.start) {
|
||||
// BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
//
|
||||
// robot.imu.initialize(parameters);
|
||||
//
|
||||
// parameters.mode = BNO055IMU.SensorMode.IMU;
|
||||
// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
||||
// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
||||
// parameters.loggingEnabled = false;
|
||||
//
|
||||
// }
|
||||
|
||||
if (engine.gamepad2.dpad_left) {
|
||||
robot.collectorLeft.setPower(-1);
|
||||
robot.collectorRight.setPower(-1);
|
||||
@@ -461,127 +391,6 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
|
||||
|
||||
|
||||
//
|
||||
// }
|
||||
//
|
||||
// if (engine.gamepad2.b) {
|
||||
//
|
||||
// robot.collectorLeft.setPower(1);
|
||||
// robot.collectorRight.setPower(-1);
|
||||
//
|
||||
// }
|
||||
//
|
||||
// if (engine.gamepad2.x) {
|
||||
//
|
||||
// robot.collectorLeft.setPower(-1);
|
||||
// robot.collectorRight.setPower(1);
|
||||
//
|
||||
// }
|
||||
////
|
||||
//// }
|
||||
////
|
||||
//// if (engine.gamepad2.right_stick_y < -0.1) {
|
||||
//// robot.LowRiserRight.setPosition(0.6);
|
||||
//// robot.LowRiserLeft.setPosition(0.6);
|
||||
//// }
|
||||
////
|
||||
//// if (engine.gamepad2.right_stick_y > 0.1) {
|
||||
//// robot.LowRiserRight.setPosition(0.45);
|
||||
//// robot.LowRiserLeft.setPosition(0.45);
|
||||
//// }
|
||||
//
|
||||
//// if (engine.gamepad2.start) {
|
||||
//// if (System.currentTimeMillis() - lastStepTime >= 150) {
|
||||
//// lastStepTime = System.currentTimeMillis();
|
||||
////
|
||||
//// if (raiseHighRiser) {
|
||||
//// if (robot.HighRiserLeft.getPosition() >= 1) {
|
||||
//// if (raiseLowRiser) {
|
||||
//// raiseHighRiser = false;
|
||||
//// }
|
||||
//// } else {
|
||||
//// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
|
||||
//// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
|
||||
//// }
|
||||
//// } else {
|
||||
//// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.035);
|
||||
//// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.035);
|
||||
////
|
||||
//// if (robot.HighRiserLeft.getPosition() <= 0.45) {
|
||||
//// raiseHighRiser = true;
|
||||
//// }
|
||||
//// }
|
||||
//// }
|
||||
//// }
|
||||
// // SPENCER____________________________________________________________________
|
||||
// if (engine.gamepad1.start) {
|
||||
// RobotPosition = robot.backRightDrive.getCurrentPosition();
|
||||
//
|
||||
// switch (DriveForwardAndBack) {
|
||||
//
|
||||
// case 0:
|
||||
// RobotStartingPosition = RobotPosition;
|
||||
// drivePower = 1;
|
||||
//
|
||||
// DriveForwardAndBack += 1;
|
||||
// break;
|
||||
//
|
||||
// case 1:
|
||||
// if (RobotPosition - RobotStartingPosition < 6250){
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// DriveForwardAndBack += 1;
|
||||
// drivePower = -1;
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// if (RobotPosition - RobotStartingPosition >= 0) {
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// DriveForwardAndBack += 1;
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// if (robot.imu.getAngularOrientation().firstAngle > -90) {
|
||||
//// *+90 degrees is counterclockwise, -90 is clockwise*
|
||||
// drivePower = 0.4;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(-drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(-drivePower);
|
||||
//
|
||||
// } else
|
||||
// {
|
||||
// DriveForwardAndBack += 1;
|
||||
// }
|
||||
// break;
|
||||
// case 4:
|
||||
// robot.backLeftDrive.setPower(0);
|
||||
// robot.backRightDrive.setPower(0);
|
||||
// robot.frontLeftDrive.setPower(0);
|
||||
// robot.frontRightDrive.setPower(0);
|
||||
// break;
|
||||
//
|
||||
//
|
||||
// } // switch ending
|
||||
// } // if gamepad 1 start ending
|
||||
// else {
|
||||
//
|
||||
//// 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) {
|
||||
@@ -636,225 +445,6 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
}// end of time if statement
|
||||
}// end of start button press
|
||||
|
||||
// if (engine.gamepad2.left_stick_y > 0.1) {
|
||||
// robot.HighRiserLeft.setPosition(0.5);
|
||||
// robot.HighRiserRight.setPosition(-0.5);
|
||||
// }
|
||||
//
|
||||
// if (engine.gamepad2.left_stick_y < -0.1) {
|
||||
// robot.HighRiserLeft.setPosition(-1);
|
||||
// robot.HighRiserRight.setPosition(1);
|
||||
// }
|
||||
|
||||
// if (engine.gamepad2.right_bumper) {
|
||||
// robot.LowRiserRight.setPosition(1);
|
||||
// robot.LowRiserLeft.setPosition(0);
|
||||
// }
|
||||
|
||||
// if (engine.gamepad2.left_bumper) {
|
||||
// robot.HighRiserRight.setPosition(0);
|
||||
// robot.HighRiserLeft.setPosition(1);
|
||||
// }
|
||||
// For raising, high risers ALWAYS raise first, for lowering, low risers ALWAYS lower first.
|
||||
|
||||
// if (engine.gamepad2.back) {
|
||||
// RobotPosition = robot.backRightDrive.getCurrentPosition();
|
||||
//
|
||||
// switch (AutoChain) {
|
||||
//
|
||||
// case 0://Initialize
|
||||
// RobotStartingPosition = RobotPosition;
|
||||
// AutoChain += 1;
|
||||
// break;
|
||||
//
|
||||
// case 1://Drive 1 square forward
|
||||
// if (RobotPosition - RobotStartingPosition < 2500){
|
||||
// drivePower = 1;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 2://Rotate Counterclockwise for 45 degrees
|
||||
// RobotRotation = robot.imu.getAngularOrientation().firstAngle;
|
||||
// if (RobotRotation <= 45) {
|
||||
// drivePower = 0.4;
|
||||
// robot.backLeftDrive.setPower(-drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(-drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// AutoChain += 1;
|
||||
// drivePower = 0;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 3://Raise upper arm fully
|
||||
// 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);
|
||||
// }
|
||||
// } else {
|
||||
// AutoChain +=1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 4://Raise lower arm fully
|
||||
// if (robot.LowRiserLeft.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);
|
||||
// }
|
||||
// } else {
|
||||
// AutoChain += 1;
|
||||
//
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 5://initialize for moving forward
|
||||
// RobotStartingPosition = robot.backRightDrive.getCurrentPosition();
|
||||
// AutoChain += 1;
|
||||
// break;
|
||||
//
|
||||
// case 6://Drive forward 1/4 square
|
||||
// if (RobotPosition - RobotStartingPosition < 1200){
|
||||
// drivePower = 1;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// drivePower = 0;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 7://Lower low arm fully
|
||||
// if (robot.LowRiserLeft.getPosition() > 0.5) {
|
||||
// if (System.currentTimeMillis() - lastStepTime >= 150) {
|
||||
// lastStepTime = System.currentTimeMillis();
|
||||
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
|
||||
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
|
||||
//
|
||||
// }
|
||||
// }else
|
||||
// {
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 8:
|
||||
// BeginningofActionTime = System.currentTimeMillis();
|
||||
// AutoChain += 1;
|
||||
//
|
||||
// break;
|
||||
// case 9://Eject
|
||||
// if (System.currentTimeMillis() - BeginningofActionTime < 2000) {
|
||||
// robot.collectorRight.setPower(-1);
|
||||
// robot.collectorLeft.setPower(1);
|
||||
//
|
||||
// } else {
|
||||
// robot.collectorLeft.setPower(0);
|
||||
// robot.collectorRight.setPower(0);
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 10://Raise low arm
|
||||
// if (robot.LowRiserLeft.getPosition() < 1) {
|
||||
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
|
||||
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
|
||||
// } else {
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 11://Initialize backup
|
||||
// RobotStartingPosition = RobotPosition;
|
||||
// AutoChain += 1;
|
||||
// break;
|
||||
//
|
||||
// case 12://Drive backwards 1/4 square
|
||||
// if (RobotPosition - RobotStartingPosition > -1200){
|
||||
// drivePower = -1;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// drivePower = 0;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// AutoChain += 1;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 13://Turn 45 degrees clockwise
|
||||
// RobotRotation = robot.imu.getAngularOrientation().firstAngle;
|
||||
// if (RobotRotation > 0) {
|
||||
// drivePower = 0.4;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(-drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(-drivePower);
|
||||
// } else
|
||||
// {
|
||||
// AutoChain += 1;
|
||||
// drivePower = 0;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 14://Initialize
|
||||
// RobotStartingPosition = RobotPosition;
|
||||
// AutoChain += 1;
|
||||
// break;
|
||||
//
|
||||
// case 15://Drive 1 square forward
|
||||
// if (RobotPosition - RobotStartingPosition < 2500){
|
||||
// drivePower = 1;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// } else
|
||||
// {
|
||||
// AutoChain = 999;
|
||||
// drivePower = 0;
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 999:
|
||||
//
|
||||
// break;
|
||||
|
||||
|
||||
gamepad1Checker.update();
|
||||
Reference in New Issue
Block a user