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
@@ -17,8 +17,9 @@ apply from: '../build.dependencies.gradle'
|
||||
|
||||
android {
|
||||
namespace = 'org.firstinspires.ftc.teamcode'
|
||||
buildFeatures {
|
||||
mlModelBinding true
|
||||
|
||||
packagingOptions {
|
||||
jniLibs.useLegacyPackaging true
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
package org.timecrafters.testing.engine;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.testing.states.AdafruitIMUState;
|
||||
@TeleOp (name = "Adafruit IMU")
|
||||
public class AdafruitIMU extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new AdafruitIMUState());
|
||||
}
|
||||
}
|
||||
@@ -18,6 +18,6 @@ public class PhoenixTeleOP extends CyberarmEngine {
|
||||
|
||||
robot = new PhoenixBot1(this);
|
||||
|
||||
addState(new PhoenixTeleOPState(robot));
|
||||
// addState(new PhoenixTeleOPState(robot));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import com.qualcomm.hardware.adafruit.AdafruitBNO055IMU;
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class AdafruitIMUState extends CyberarmState {
|
||||
AdafruitBNO055IMU imu;
|
||||
@Override
|
||||
public void init() {
|
||||
imu = engine.hardwareMap.get(AdafruitBNO055IMU.class, "adafruit_imu");
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
imu.initialize(parameters);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("First Angle", imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("Second Angle", imu.getAngularOrientation().secondAngle);
|
||||
engine.telemetry.addData("Third Angle", imu.getAngularOrientation().thirdAngle);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
public class LaserServoHeight extends CyberarmState {
|
||||
Servo leftUpLift, leftDownLift, rightUpLift, rightDownLift;
|
||||
Rev2mDistanceSensor CollectorHeight;
|
||||
double TargetHeight, Delay, LastChecked;
|
||||
@Override
|
||||
public void init() {
|
||||
leftUpLift = engine.hardwareMap.servo.get("LU-Lift");
|
||||
rightUpLift = engine.hardwareMap.servo.get("RU-Lift");
|
||||
CollectorHeight = engine.hardwareMap.get(Rev2mDistanceSensor.class, "CollectorHeight");
|
||||
TargetHeight = 380;
|
||||
Delay = 150;
|
||||
LastChecked = -1;
|
||||
leftUpLift.setDirection(Servo.Direction.REVERSE);
|
||||
rightUpLift.setDirection(Servo.Direction.FORWARD);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if(runTime() - LastChecked >= Delay) {
|
||||
LastChecked = runTime();
|
||||
|
||||
if (CollectorHeight.getDistance(DistanceUnit.MM) < TargetHeight) {
|
||||
leftUpLift.setPosition(leftUpLift.getPosition()+0.05);
|
||||
rightUpLift.setPosition(rightUpLift.getPosition()+0.05);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class PhoenixTeleOPv2 extends CyberarmState {
|
||||
PhoenixBot1 robot;
|
||||
public PhoenixTeleOPv2(PhoenixBot1 robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
addParallelState(new TeleOPArmDriver(robot));
|
||||
addParallelState(new TeleOPTankDriver(robot));
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,214 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.cyberarm.engine.V2.GamepadChecker;
|
||||
|
||||
public class TeleOPArmDriver extends CyberarmState {
|
||||
private final PhoenixBot1 robot;
|
||||
private long lastStepTime = 0;
|
||||
private int CyclingArmUpAndDown = 0;
|
||||
private GamepadChecker gamepad2Checker;
|
||||
|
||||
public TeleOPArmDriver(PhoenixBot1 robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("Arm Driver:");
|
||||
engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition());
|
||||
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());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot.HighRiserLeft.setDirection(Servo.Direction.REVERSE);
|
||||
robot.HighRiserRight.setDirection(Servo.Direction.FORWARD);
|
||||
robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
|
||||
robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
|
||||
robot.LowRiserLeft.setPosition(0.45);
|
||||
robot.LowRiserRight.setPosition(0.45);
|
||||
robot.HighRiserLeft.setPosition(0.45);
|
||||
robot.HighRiserRight.setPosition(0.45);
|
||||
|
||||
|
||||
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
|
||||
}
|
||||
|
||||
@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();
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,272 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.cyberarm.engine.V2.GamepadChecker;
|
||||
|
||||
public class TeleOPTankDriver extends CyberarmState {
|
||||
|
||||
private final PhoenixBot1 robot;
|
||||
private double drivePower = 1;
|
||||
private double RobotRotation;
|
||||
private double RotationTarget, DeltaRotation;
|
||||
private double MinimalPower = 0.2;
|
||||
private GamepadChecker gamepad1Checker;
|
||||
public TeleOPTankDriver(PhoenixBot1 robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("Tank Driver");
|
||||
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("Drive Power", drivePower);
|
||||
engine.telemetry.addData("Delta Rotation", DeltaRotation);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
|
||||
}
|
||||
|
||||
@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);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
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
|
||||
public void buttonUp(Gamepad gamepad, String button) {
|
||||
if (engine.gamepad1 == gamepad && button.equals("start")) {
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
|
||||
parameters.mode = BNO055IMU.SensorMode.IMU;
|
||||
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
||||
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
||||
parameters.loggingEnabled = false;
|
||||
|
||||
robot.imu.initialize(parameters);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user