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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2023-11-09 21:29:45 -06:00
parent d2520f2325 7d3d5da8de
commit 0d35a20e5f
4 changed files with 236 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java
View File

@@ -37,7 +37,7 @@ public class DepositorArmPosState extends CyberarmState {
case 0:
// transfer
break;
case 10:
case 1:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
@@ -48,41 +48,29 @@ public class DepositorArmPosState extends CyberarmState {
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += (long) robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= totalWaitedTime) {
// setHasFinished(true);
setHasFinished(true);
}
}
break;
case 20:
case 2:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
case 3:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
setHasFinished(true);
}
@@ -93,57 +81,41 @@ public class DepositorArmPosState extends CyberarmState {
}
break;
case 10:// ----------------------------------------------------------------------------------------------- drive to driving pos
case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
case 1:
// drive pos
setHasFinished(true);
break;
case 20:
case 2:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2100) {
setHasFinished(true);
}
}
break;
case 30:
case 3:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
setHasFinished(true);
}
@@ -154,65 +126,46 @@ public class DepositorArmPosState extends CyberarmState {
}
break;
case 20:// ----------------------------------------------------------------------------------------------- drive to collect pos
case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
case 1:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 20:
case 2:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
case 3:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
setHasFinished(true);
}
@@ -223,51 +176,39 @@ public class DepositorArmPosState extends CyberarmState {
}
break;
case 30:// ----------------------------------------------------------------------------------------------- drive to deposit pos
case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
case 1:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 20:
case 2:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
case 3:
// deposit
break;
}

186
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -17,8 +17,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class PrototypeRobot extends Robot {
public int armPosition = 0;
public int oldArmPosititon;
public boolean stateFinished;
public long startOfSequencerTime;
public int oldArmPosition = 0;
public long waitTime;
public double servoWaitTime;
public double servoSecPerDeg = 0.14/60;
@@ -203,4 +204,185 @@ public class PrototypeRobot extends Robot {
positionH += dtheta;
}
public void ArmSequences(){
switch (armPosition) {
case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
switch (oldArmPosition) {
case 0:
// transfer
break;
case 1:
// driving
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
oldArmPosition = 0;
}
}
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 = 0;
}
}
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_IN); // drive the shoulder to the transfer position
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
oldArmPosition = 0;
}
}
}
}
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 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;
}
}
}

36
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java
View File

@@ -7,7 +7,8 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class ArmPosTest extends CyberarmState {
private float armPos;
private float armPosE;
private float armPosS;
private long lastMeasuredTime;
private long waitTime;
PrototypeRobot robot;
@@ -17,7 +18,8 @@ public class ArmPosTest extends CyberarmState {
@Override
public void start() {
armPos = 0F;
armPosS = 0F;
armPosE = 0F;
lastMeasuredTime = System.currentTimeMillis();
waitTime = 500;
@@ -27,22 +29,31 @@ public class ArmPosTest extends CyberarmState {
@Override
public void exec(){
robot.collectorElbow.setPosition(armPos);
robot.collectorShoulder.setPosition(armPosS);
robot.collectorElbow.setPosition(armPosE);
if (engine.gamepad1.a){
robot.collectorShoulder.setPosition(0.75);
} else if (engine.gamepad1.y){
robot.collectorShoulder.setPosition(0.65);
}else if (engine.gamepad1.x){
robot.collectorShoulder.setPosition(0.4);
// if (engine.gamepad1.a){
// robot.collectorShoulder.setPosition(0.75);
// } else if (engine.gamepad1.y){
// robot.collectorShoulder.setPosition(0.65);
// }else if (engine.gamepad1.x){
// robot.collectorShoulder.setPosition(0.4);
// }
if (engine.gamepad1.y && System.currentTimeMillis() - lastMeasuredTime > 500){
lastMeasuredTime = System.currentTimeMillis();
armPosS += 0.05;
} else if (engine.gamepad1.a && System.currentTimeMillis() - lastMeasuredTime > 500){
lastMeasuredTime = System.currentTimeMillis();
armPosS -= 0.05;
}
if (engine.gamepad2.y && System.currentTimeMillis() - lastMeasuredTime > 500){
lastMeasuredTime = System.currentTimeMillis();
armPos += 0.05;
armPosE += 0.05;
} else if (engine.gamepad2.a && System.currentTimeMillis() - lastMeasuredTime > 500){
lastMeasuredTime = System.currentTimeMillis();
armPos -= 0.05;
armPosE -= 0.05;
}
}
@@ -50,6 +61,7 @@ public class ArmPosTest extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("arm pos current", armPos);
engine.telemetry.addData("arm pos current Shoulder", armPosS);
engine.telemetry.addData("arm pos current Elbow", armPosE);
}
}

13
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java
View File

@@ -43,9 +43,20 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
@Override
public void exec() {
if (engine.gamepad2.a){
robot.armPosition = 0;
} else if (engine.gamepad2.x){
robot.armPosition = 1;
} else if (engine.gamepad2.b){
robot.armPosition = 2;
} else if (engine.gamepad2.y){
robot.armPosition = 3;
}
// drivetrain
robot.driveTrainTeleOp();
// arm sequencer
robot.ArmSequences();
// lift
sliderTeleOp();