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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-01-28 18:32:37 -06:00
parent 733939c0b6 79ea2ceeb1
commit 4cf664843d
2 changed files with 173 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -41,12 +41,12 @@ public class PhoenixBot1 {
public TFObjectDetector tfod; public TFObjectDetector tfod;
public Servo LowRiserLeft, LowRiserRight, CameraServo; public Servo LowRiserLeft, LowRiserRight, /*HighRiserLeft, HighRiserRight, */CameraServo;
private final CyberarmEngine engine; private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance, /*downSensor,*/ leftPoleDistance, rightPoleDistance; public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizonatal, HighRiseMotor; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft, ArmMotor, armMotorEncoder;
public CRServo collectorLeft, collectorRight; public CRServo collectorLeft, collectorRight;
@@ -145,64 +145,64 @@ public class PhoenixBot1 {
// Arm // Arm
LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft"); LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft");
LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight"); LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
// HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
// HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor");
armMotorEncoder = engine.hardwareMap.dcMotor.get("Arm Motor Encoder");
//motors direction and encoders //motors direction and encoders
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// Dead Wheel encoder for driving // Dead Wheel encoder for driving
OdometerEncoderRight = engine.hardwareMap.dcMotor.get("odometerEncoderR"); OdometerEncoder = engine.hardwareMap.dcMotor.get("odometerEncoderR");
OdometerEncoderRight.setDirection(DcMotorSimple.Direction.REVERSE); OdometerEncoder.setDirection(DcMotorSimple.Direction.REVERSE);
OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
OdometerEncoderLeft = engine.hardwareMap.dcMotor.get("odometerEncoderL"); OdometerEncoderLeft = engine.hardwareMap.dcMotor.get("odometerEncoderL");
OdometerEncoderLeft.setDirection(DcMotorSimple.Direction.REVERSE); OdometerEncoderLeft.setDirection(DcMotorSimple.Direction.REVERSE);
OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
OdometerEncoderHorizonatal = engine.hardwareMap.dcMotor.get("odometerEncoderH");
OdometerEncoderHorizonatal.setDirection(DcMotorSimple.Direction.REVERSE);
OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
HighRiseMotor = engine.hardwareMap.dcMotor.get("High Riser Motor");
HighRiseMotor.setDirection(DcMotorSimple.Direction.REVERSE);
HighRiseMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
HighRiseMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
HighRiseMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// HighRiserLeft.setDirection(Servo.Direction.REVERSE);
// HighRiserRight.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE); LowRiserRight.setDirection(Servo.Direction.REVERSE);
armMotorEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
armMotorEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
CameraServo.setDirection(Servo.Direction.FORWARD); CameraServo.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setPosition(0.35); LowRiserLeft.setPosition(0.35);
LowRiserRight.setPosition(0.35); LowRiserRight.setPosition(0.35);
// HighRiserLeft.setPosition(0.40);
// HighRiserRight.setPosition(0.40);
CameraServo.setPosition(0.775); CameraServo.setPosition(0.775);

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

@@ -1,5 +1,6 @@
package org.timecrafters.TeleOp.states; package org.timecrafters.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
@@ -11,17 +12,21 @@ public class TeleOPArmDriver extends CyberarmState {
private long lastStepTime = 0; private long lastStepTime = 0;
private int CyclingArmUpAndDown = 0; private int CyclingArmUpAndDown = 0;
private GamepadChecker gamepad2Checker; private GamepadChecker gamepad2Checker;
private int Opportunity, Endeavour; private int Opportunity, Endeavour, Peanut;
private double drivePower; private double drivePower, armPower;
private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05; private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
private double servoCollectLow = 0.40; //Low servos, A button private double servoCollectLow = 0.40; //Low servos, A button
private double servoCollectHigh = 0.40; //High servos, A button // private double servoCollectHigh = 0.40; //High servos, A button
private double servoLowLow = 0.5; //Low servos, X button private double servoLowLow = 0.5; //Low servos, X button
private double servoLowHigh = 0.75; //High servos, X button // private double servoLowHigh = 0.75; //High servos, X button
private double servoMedLow = 0.5; //Low servos, B button private double servoMedLow = 0.5; //Low servos, B button
private double servoMedHigh = 0.9; //High servos, B button // private double servoMedHigh = 0.9; //High servos, B button
private double servoHighLow = 0.8; //Low servos, Y button private double servoHighLow = 0.8; //Low servos, Y button
private double servoHighHigh = 0.9; //High servos, Y button // private double servoHighHigh = 0.9; //High servos, Y button
private int armMotorCollect = 0;
private int armMotorLow = 100;
private int armMotorMed = 1000;
private int armMotorHigh = 1600;
public TeleOPArmDriver(PhoenixBot1 robot) { public TeleOPArmDriver(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -30,31 +35,30 @@ public class TeleOPArmDriver extends CyberarmState {
@Override @Override
public void telemetry() { public void telemetry() {
engine.telemetry.addLine("Arm Driver:"); engine.telemetry.addLine("Arm Driver:");
// engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition()); engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
// 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 Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
} }
@Override @Override
public void init() { public void init() {
// robot.HighRiserLeft.setDirection(Servo.Direction.REVERSE); robot.ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
// robot.HighRiserRight.setDirection(Servo.Direction.FORWARD);
robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD); robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
robot.LowRiserLeft.setPosition(0.45); robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45); robot.LowRiserRight.setPosition(0.45);
// robot.HighRiserLeft.setPosition(0.45); robot.ArmMotor.setPower(0);
// robot.HighRiserRight.setPosition(0.45); robot.armMotorEncoder.setPower(0);
Opportunity = 0; Opportunity = 0;
Endeavour = 0; Endeavour = 0;
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
} }
@Override @Override
public void exec() { public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO MM HEIGHTS!!
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
Endeavour = 4; Endeavour = 4;
@@ -70,127 +74,147 @@ public class TeleOPArmDriver extends CyberarmState {
} }
if (Endeavour == 4) { if (Endeavour == 4) {
// if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01)/* <-- high level too low*/ { if (robot.armMotorEncoder.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ {
// if (System.currentTimeMillis() - lastStepTime >= 100) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
}
if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
// if (robot.HighRiserLeft.getPosition() >= servoHighHigh &&
// robot.LowRiserLeft.getPosition() >= servoHighLow) {
// Endeavour = 0;
// }
}
if (Endeavour == 3) {
if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.ArmMotor.setPower(0.5);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoMedLow - 0.01)/* <-- low level too low*/ { if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
} }
// if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 && if (robot.armMotorEncoder.getCurrentPosition() >= armMotorHigh &&
// robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ { robot.LowRiserLeft.getPosition() >= servoHighLow) {
// if (System.currentTimeMillis() - lastStepTime >= 100) { Endeavour = 0;
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
// }
// }
// if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 &&
// robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ {
// if (System.currentTimeMillis() - lastStepTime >= 100) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
// }
// if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 &&
// robot.LowRiserLeft.getPosition() <= servoMedLow &&
// robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 &&
// robot.HighRiserLeft.getPosition() <= servoMedHigh) {
// Endeavour = 0;
// }
// }
if (Endeavour == 2) {
if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
// if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
// robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
// robot.HighRiserLeft.getPosition() > servoLowHigh)/* <-- high level too high*/ {
// if (System.currentTimeMillis() - lastStepTime >= 100) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
// }
// }
// if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
// robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01)/* <-- high level too low*/ {
// if (System.currentTimeMillis() - lastStepTime >= 100) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
// }
// if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
// robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
// robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 &&
// robot.HighRiserLeft.getPosition() <= servoLowHigh + 0.01) {
// Endeavour = 0;
// }
// }
if (Endeavour == 1) {
if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
// } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
// robot.HighRiserLeft.getPosition() > servoCollectHigh)/* <-- high level too high*/ {
// if (System.currentTimeMillis() - lastStepTime >= 100) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
// }
// } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 &&
// robot.HighRiserLeft.getPosition() <= servoCollectHigh) {
// Endeavour = 0;
// }
}
}
} }
} }
if (Endeavour == 3) {
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow - 5)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
}
}
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.armMotorEncoder.getCurrentPosition() > armMotorMed - 5) {
Endeavour = 0;
}
}
if (Endeavour == 2) {
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoLowLow - 5)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
}
}
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorLow - 5) {
Endeavour = 0;
}
}
if (Endeavour == 1) {
if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.armMotorEncoder.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() <= armMotorCollect) {
Endeavour = 0;
}
}
if (engine.gamepad2.dpad_left && Peanut != 1) {
Peanut = 1;
}
if (engine.gamepad2.dpad_right && Peanut != 2) {
Peanut = 2;
}
if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
}
if (Peanut == 1) {
robot.collectorRight.setPower(1);
robot.collectorLeft.setPower(-1);
}
if (Peanut == 2) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(-1);
}
if (engine.gamepad2.right_trigger > 0.1) {
armPower = engine.gamepad2.right_trigger;
robot.ArmMotor.setPower(armPower);
} else if (engine.gamepad2.left_trigger > 0.1) {
armPower = -(engine.gamepad2.left_trigger);
robot.ArmMotor.setPower(armPower);
}
} }
} }