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

fixed arm control, attempted to start autonomous

Browse Source
This commit is contained in:
SpencerPiha
2023-12-30 11:25:12 -06:00
parent 4e9e646947
commit 362ab0791d
4 changed files with 55 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java
View File

@@ -39,6 +39,12 @@ public class Competition2BlueBackStage extends CyberarmEngine {
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0"));
addState(new ClawFingerState(robot,"burnsville backdrop blue", "0-04-0"));
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-05-0"));
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-06-0"));

35
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
View File

@@ -25,41 +25,10 @@ public class ClawArmState extends CyberarmState {
robot.OdometryLocalizer();
// driving arm to pos
if (armPos == "collect") {
if (robot.lift.getCurrentPosition() >= 20) {
robot.lift.setPower(-0.6);
} else {
robot.lift.setPower(0);
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.target = 30;
}
}
if (armPos == "deposit") {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
robot.target = 400;
}
if (armPos == "hover") {
if (robot.lift.getCurrentPosition() >= 20) {
robot.lift.setPower(-0.6);
} else {
robot.shoulder.setPosition(0.38);
robot.target = 200;
}
}
if (armPos.equals("search")) {
robot.shoulder.setPosition(0.15);
robot.target = 570;
}
robot.armPos = armPos;
robot.clawArmControl();
if (robot.power < 0.1) {
if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) {
setHasFinished(true);
}

15
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -21,6 +21,7 @@ public class DriveToCoordinatesState extends CyberarmState {
public boolean posSpecific;
public double maxXPower;
public double maxYPower;
public long initTime;
public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
this.robot = robot;
@@ -34,6 +35,12 @@ public class DriveToCoordinatesState extends CyberarmState {
this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
}
@Override
public void start() {
super.start();
initTime = System.currentTimeMillis();
}
@Override
public void exec() {
if (!posSpecific) {
@@ -63,10 +70,10 @@ public class DriveToCoordinatesState extends CyberarmState {
robot.OdometryLocalizer();
if (Math.abs(robot.backLeftPower) < 0.15 &&
Math.abs(robot.backRightPower) < 0.15 &&
Math.abs(robot.frontLeftPower) < 0.15 &&
Math.abs(robot.frontRightPower) < 0.15) {
if ((Math.abs(robot.backLeftPower) < 0.18 &&
Math.abs(robot.backRightPower) < 0.18 &&
Math.abs(robot.frontLeftPower) < 0.18 &&
Math.abs(robot.frontRightPower) < 0.18) || (System.currentTimeMillis() - initTime > 5000)) {
posAchieved = true;
}
}

36
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -22,6 +22,8 @@ import org.openftc.easyopencv.OpenCvWebcam;
import org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import java.util.Objects;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Config
@@ -94,6 +96,9 @@ public class CompetitionRobotV1 extends Robot {
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
PIDController pidController;
public String armPos;
public int target;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 0.38;
@@ -324,6 +329,37 @@ public class CompetitionRobotV1 extends Robot {
}
public void clawArmControl(){
if (armPos.equals("collect")) {
if (lift.getCurrentPosition() >= 20) {
lift.setPower(-0.6);
} else {
lift.setPower(0);
shoulder.setPosition(shoulderCollect);
elbow.setPosition(elbowCollect);
target = 30;
}
}
if (Objects.equals(armPos, "deposit")) {
shoulder.setPosition(shoulderDeposit);
elbow.setPosition(elbowDeposit);
target = 400;
}
if (Objects.equals(armPos, "hover")) {
if (lift.getCurrentPosition() >= 20) {
lift.setPower(-0.6);
} else {
shoulder.setPosition(0.38);
target = 100;
}
}
if (armPos.equals("search")) {
shoulder.setPosition(0.15);
target = 570;
}
pidController.setPID(p, i, d);
int armPos = clawArm.getCurrentPosition();