mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 16:52:35 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
NerdyBirdy460
@@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.States.AutoStateScrimmage;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.Common.ServoTestRobot;
|
||||
@@ -10,17 +11,16 @@ import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@Autonomous(name = "Servo Test", group = "Testing")
|
||||
@Autonomous(name = "Scrimmage Auto", group = "PROTOTYPE", preselectTeleOp = "Prototype Robot")
|
||||
|
||||
public class ServoTestEngine extends CyberarmEngine {
|
||||
|
||||
ServoTestRobot robot;
|
||||
PrototypeRobot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new ServoTestRobot("Hello World");
|
||||
this.robot = new PrototypeRobot("Hello World");
|
||||
this.robot.setup();
|
||||
|
||||
addState(new DepositorArmPosState(robot, "ServoPositions", "01-0"));
|
||||
|
||||
addState(new AutoStateScrimmage(robot));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class AutoStateScrimmage extends CyberarmState {
|
||||
|
||||
private int distanceDriven;
|
||||
private int targetDistance = 38000;
|
||||
private int stepsFinished = 0;
|
||||
private boolean firstDrivePos = false;
|
||||
private boolean secondDrivePos = false;
|
||||
private boolean armToFIrstPos = false;
|
||||
private boolean armToSecondPos = false;
|
||||
private boolean collect = false;
|
||||
PrototypeRobot robot;
|
||||
public AutoStateScrimmage(PrototypeRobot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
|
||||
robot.odometerR.setMode(DcMotor.RunMode.RESET_ENCODERS);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
distanceDriven = robot.currentRightPosition;
|
||||
|
||||
if (stepsFinished == 5) {
|
||||
// setHasFinished(true);
|
||||
}
|
||||
|
||||
if (robot.odometerR.getCurrentPosition() >= targetDistance && firstDrivePos == false){
|
||||
robot.frontRight.setPower(0);
|
||||
robot.frontLeft.setPower(0);
|
||||
robot.backRight.setPower(0);
|
||||
robot.backLeft.setPower(0);
|
||||
stepsFinished += 1;
|
||||
firstDrivePos = true;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (robot.odometerR.getCurrentPosition() < targetDistance){
|
||||
robot.frontRight.setPower(0.5);
|
||||
robot.frontLeft.setPower(0.5);
|
||||
robot.backRight.setPower(0.5);
|
||||
robot.backLeft.setPower(0.5);
|
||||
}
|
||||
|
||||
if (stepsFinished == 1 && firstDrivePos && armToFIrstPos == false){
|
||||
robot.armPosition = 2;
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) {
|
||||
robot.oldArmPosition = 2;
|
||||
armToFIrstPos = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (firstDrivePos && armToFIrstPos){
|
||||
robot.collector.setPosition(1F);
|
||||
collect = true;
|
||||
}
|
||||
|
||||
// if (stepsFinished == 3 && firstDrivePos && armToFIrstPos && collect && secondDrivePos == false){
|
||||
// if (robot.odometerR.getCurrentPosition() <= targetDistance - 500){
|
||||
// robot.frontRight.setPower(0);
|
||||
// robot.frontLeft.setPower(0);
|
||||
// robot.backRight.setPower(0);
|
||||
// robot.backLeft.setPower(0);
|
||||
// stepsFinished += 1;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// secondDrivePos = true;
|
||||
// } else {
|
||||
// robot.frontRight.setPower(-0.3);
|
||||
// robot.frontLeft.setPower(-0.3);
|
||||
// robot.backRight.setPower(-0.3);
|
||||
// robot.backLeft.setPower(-0.3);
|
||||
// }
|
||||
// }
|
||||
if (firstDrivePos && armToFIrstPos && collect){
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("current pos", robot.odometerR.getCurrentPosition());
|
||||
engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -122,6 +122,7 @@ public class PrototypeRobot extends Robot {
|
||||
backRight = engine.hardwareMap.dcMotor.get("backRight");
|
||||
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
|
||||
lift = engine.hardwareMap.dcMotor.get("Lift");
|
||||
odometerR = engine.hardwareMap.dcMotor.get("odometerR");
|
||||
|
||||
configuration = new TimeCraftersConfiguration("Blue Crab");
|
||||
|
||||
@@ -134,6 +135,8 @@ public class PrototypeRobot extends Robot {
|
||||
lift.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
|
||||
|
||||
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
@@ -159,10 +162,9 @@ public class PrototypeRobot extends Robot {
|
||||
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
|
||||
collector.setPosition(0F);
|
||||
|
||||
|
||||
// depositorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
|
||||
// depositorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
|
||||
}
|
||||
|
||||
@@ -263,10 +265,10 @@ public class PrototypeRobot extends Robot {
|
||||
public void DepositorToggle(){
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (depositorPos == 1F) {
|
||||
if (depositorPos == 0.6F) {
|
||||
depositorPos = 0F;
|
||||
} else {
|
||||
depositorPos = 1F;
|
||||
depositorPos = 0.6F;
|
||||
}
|
||||
}
|
||||
rbsVar2 = rbs2;
|
||||
@@ -277,7 +279,6 @@ public class PrototypeRobot extends Robot {
|
||||
switch (oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
break;
|
||||
case 1:
|
||||
// driving
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
@@ -287,7 +288,6 @@ public class PrototypeRobot extends Robot {
|
||||
oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
@@ -297,7 +297,6 @@ public class PrototypeRobot extends Robot {
|
||||
oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
@@ -313,141 +312,139 @@ public class PrototypeRobot extends Robot {
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
|
||||
switch (oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// drive pos
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2100) {
|
||||
oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
|
||||
oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // drive pos
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2100) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
|
||||
case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
|
||||
switch (oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// driving
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_OUT);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
|
||||
switch (oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// driving
|
||||
collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.States;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
@@ -9,74 +11,299 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
private PrototypeRobot robot;
|
||||
private int maxExtension = 2000;
|
||||
private int minExtension = 0;
|
||||
private PIDController HeadingPidController;
|
||||
private double targetHeading;
|
||||
private final double p = 0, i = 0, d = 0;
|
||||
|
||||
|
||||
|
||||
private long lastCheckedTime;
|
||||
|
||||
public PrototypeRobotDrivetrainState(PrototypeRobot robot) {
|
||||
this.robot = robot;
|
||||
HeadingPidController = new PIDController(p, i, d);
|
||||
|
||||
}
|
||||
|
||||
// }
|
||||
// }
|
||||
// --------------------------------------------------------------------------------------------------------- Slider control function
|
||||
private void sliderTeleOp(){
|
||||
if (engine.gamepad1.right_trigger != 0){
|
||||
if (robot.lift.getCurrentPosition() >= maxExtension){
|
||||
private void SliderTeleOp() {
|
||||
if (engine.gamepad2.right_trigger != 0) {
|
||||
if (robot.lift.getCurrentPosition() >= maxExtension) {
|
||||
robot.lift.setPower(0);
|
||||
} else if (robot.lift.getCurrentPosition() >= maxExtension - 200){
|
||||
} else if (robot.lift.getCurrentPosition() >= maxExtension - 200) {
|
||||
robot.lift.setPower(0.35);
|
||||
}else {
|
||||
robot.lift.setPower(engine.gamepad1.right_trigger);
|
||||
} else {
|
||||
robot.lift.setPower(engine.gamepad2.right_trigger);
|
||||
}
|
||||
} else if (engine.gamepad1.left_trigger != 0){
|
||||
} else if (engine.gamepad2.left_trigger != 0) {
|
||||
|
||||
if (robot.lift.getCurrentPosition() <= minExtension) {
|
||||
robot.lift.setPower(0);
|
||||
} else if (robot.lift.getCurrentPosition() < 350){
|
||||
} else if (robot.lift.getCurrentPosition() < 350) {
|
||||
robot.lift.setPower(-0.3);
|
||||
}else {
|
||||
robot.lift.setPower(-engine.gamepad1.left_trigger);
|
||||
} else {
|
||||
robot.lift.setPower(-engine.gamepad2.left_trigger);
|
||||
}
|
||||
} else {
|
||||
robot.lift.setPower(0);
|
||||
}
|
||||
}
|
||||
public double angleWrap(double radians) {
|
||||
while (radians > Math.PI) {
|
||||
radians -= 2 * Math.PI;
|
||||
}
|
||||
while (radians < -Math.PI){
|
||||
radians += 2 * Math.PI;
|
||||
}
|
||||
return radians;
|
||||
}
|
||||
|
||||
private void HeadingLock(){
|
||||
HeadingPidController.setPID(p, i, d);
|
||||
double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
|
||||
double pid = HeadingPidController.calculate(currentHeading, targetHeading);
|
||||
double r = pid;
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
HeadingPidController = new PIDController(p, i, d);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (engine.gamepad2.a){
|
||||
|
||||
if (engine.gamepad1.right_stick_button) {
|
||||
robot.imu.resetYaw();
|
||||
}
|
||||
|
||||
if (engine.gamepad2.a) {
|
||||
robot.armPosition = 0;
|
||||
} else if (engine.gamepad2.x){
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.x) {
|
||||
robot.armPosition = 1;
|
||||
} else if (engine.gamepad2.b){
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.b) {
|
||||
robot.armPosition = 2;
|
||||
} else if (engine.gamepad2.y){
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.y) {
|
||||
robot.armPosition = 3;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
robot.depositor.setPosition(robot.depositorPos);
|
||||
robot.collector.setPosition(robot.collectorPos);
|
||||
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
|
||||
// arm sequencer
|
||||
robot.ArmSequences();
|
||||
// lift
|
||||
sliderTeleOp();
|
||||
// collector depositor
|
||||
robot.CollectorToggle();
|
||||
// depositor toggle
|
||||
robot.DepositorToggle();
|
||||
// robot.ArmSequences();
|
||||
switch (robot.armPosition) {
|
||||
case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
|
||||
switch (robot.oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
robot.oldArmPosition = 0;
|
||||
break;
|
||||
case 1:
|
||||
// driving
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
robot.oldArmPosition = 0;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position
|
||||
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) {
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) {
|
||||
robot.oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
|
||||
switch (robot.oldArmPosition) {
|
||||
case 0:
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// drive pos
|
||||
robot.oldArmPosition = 1;
|
||||
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position
|
||||
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) {
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
robot.oldArmPosition = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
|
||||
switch (robot.oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) {
|
||||
robot.oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
// driving
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) {
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) {
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) {
|
||||
robot.oldArmPosition = 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
|
||||
switch (robot.oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) {
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){
|
||||
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){
|
||||
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){
|
||||
robot.oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) {
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){
|
||||
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){
|
||||
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){
|
||||
robot.oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
// collect
|
||||
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
robot.oldArmPosition = 3;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
// deposit
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
// lift
|
||||
SliderTeleOp();
|
||||
// collector depositor
|
||||
robot.CollectorToggle();
|
||||
// depositor toggle
|
||||
robot.DepositorToggle();
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition());
|
||||
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("arm Pos", robot.armPosition);
|
||||
engine.telemetry.addData("old arm pos", robot.oldArmPosition);
|
||||
engine.telemetry.addData("depositor pos", robot.depositorPos);
|
||||
engine.telemetry.addData("collector pos", robot.collectorPos);
|
||||
@Override
|
||||
public void telemetry () {
|
||||
engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition());
|
||||
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("arm Pos", robot.armPosition);
|
||||
engine.telemetry.addData("old arm pos", robot.oldArmPosition);
|
||||
engine.telemetry.addData("depositor pos", robot.depositorPos);
|
||||
engine.telemetry.addData("collector pos", robot.collectorPos);
|
||||
engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user