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 max power function, tuned odometry a bit, added a wait arguement in the arm state so we can have delays and pauses when needed, and wrote out all of blue audience side auto steps.

Browse Source
This commit is contained in:
SpencerPiha
2024-01-06 00:59:34 -06:00
parent c5d5ca7b59
commit 3327860e5d
5 changed files with 139 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "BURNSVILLE blue backdrop")
@Autonomous(name = "BURNSVILLE blue audience")
public class Competition2BlueBackStage extends CyberarmEngine {
@@ -30,34 +30,77 @@ public class Competition2BlueBackStage extends CyberarmEngine {
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville backdrop blue");
this.robot = new CompetitionRobotV1("burnsville audience blue");
this.robot.setup();
addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "0-00-0"));
addState(new ClawArmState(robot,"burnsville backdrop blue", "0-01-0"));
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"burnsville backdrop blue", "0-02-0"));
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"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1"));
// drive to the left, center, or right spike mark
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1"));
// drive to search pos
addState(new ClawArmState(robot,"burnsville audience blue", "0-02-2"));
// close right finger
addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3"));
// drive back and away from the spike mark (x,y) (1050, 1000) H = 0
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0"));
// drive to the middle truss (right version) (1130,980) H = -90
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0"));
// drive to hover mode to clear the middle truss
addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1"));
// drive under the middle truss across the field (right version) (1170,1080) H = -90
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0"));
// drive to deposit mode to place on back drop
addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1"));
// drive to back drop
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0"));
// open claw to deposit gold
addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1"));
// drive back from backdrop
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0"));
// close left claw
addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1"));
// drive to parking spot
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0"));
// arm to collect pos
addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1"));
}

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

@@ -13,9 +13,18 @@ public class ClawArmState extends CyberarmState {
CompetitionRobotV1 robot;
public String armPos;
public long wait;
public long initTime;
public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) {
this.robot = robot;
this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value();
this.wait = robot.configuration.variable(groupName, actionName, "wait").value();
}
@Override
public void start() {
super.start();
initTime = System.currentTimeMillis();
}
@Override
@@ -28,7 +37,7 @@ public class ClawArmState extends CyberarmState {
robot.armPos = armPos;
robot.clawArmControl();
if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) {
if ((robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) && wait < System.currentTimeMillis() - initTime) {
setHasFinished(true);
}

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

@@ -21,7 +21,6 @@ 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;
@@ -38,43 +37,38 @@ public class DriveToCoordinatesState extends CyberarmState {
@Override
public void start() {
super.start();
initTime = System.currentTimeMillis();
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
}
@Override
public void exec() {
if (!posSpecific) {
// enter loop
} else {
if (posSpecific) {
if (objectPos != robot.objectPos) {
// enter the ending loop
// setHasFinished(true);
setHasFinished(true);
}
}
if (posAchieved) {
// setHasFinished(true);
}
if (armDrive) {
robot.clawArmControl();
}
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
robot.hTarget = hTarget;
robot.yTarget = yTarget;
robot.xTarget = xTarget;
robot.OdometryLocalizer();
robot.XDrivePowerModifier();
robot.YDrivePowerModifier();
robot.DriveToCoordinates();
robot.OdometryLocalizer();
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;
if ((Math.abs(robot.backLeftPower) < 0.15 &&
Math.abs(robot.backRightPower) < 0.15 &&
Math.abs(robot.frontLeftPower) < .15 &&
Math.abs(robot.frontRightPower) < 0.15)){
setHasFinished(true);
}
}

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

@@ -47,8 +47,8 @@ public class CompetitionRobotV1 extends Robot {
// ----------------------------------------------------------------------------------------------------------------- odometry variables:
public static double Hp = 0.8, Hi = 0, Hd = 0;
public static double Xp = -0.035, Xi = 0, Xd = 0;
public static double Yp = 0.035, Yi = 0, Yd = 0.0013;
public static double Xp = -0.03, Xi = 0, Xd = 0;
public static double Yp = 0.03, Yi = 0, Yd = 0;
public double xMultiplier = 1;
public double yMultiplier = 1;
public double positionX = 1000;
@@ -175,9 +175,10 @@ public class CompetitionRobotV1 extends Robot {
// ----------------------------------------------------------------------------------------------------------------------------- IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
configuration = new TimeCraftersConfiguration("Blue Crab");
@@ -285,44 +286,43 @@ public class CompetitionRobotV1 extends Robot {
return output;
}
public void YDrivePowerModifier () {
rawPidY = XPIDControl(xTarget, positionX);
if (Math.abs(rawPidY) > yMaxPower) {
if (rawPidY < 0) {
pidY = -yMaxPower;
} else {
pidY = yMaxPower;
}
} else {
pidY = rawPidY;
}
}
public void XDrivePowerModifier () {
rawPidX = YPIDControl(yTarget, positionY);
if (Math.abs(rawPidX) > xMaxPower) {
if (rawPidX < 0) {
pidX = -xMaxPower;
} else {
pidX = xMaxPower;
}
} else {
pidX = rawPidX;
}
}
public void DriveToCoordinates () {
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
// rawPidY = XPIDControl(xTarget, positionX);
// rawPidX = YPIDControl(yTarget, positionY);
if (Math.abs(yTarget - positionY) > 5) {
if (Math.abs(XPIDControl(xTarget, positionX)) >= yMaxPower) {
if (XPIDControl(xTarget, positionX) < 0) {
pidY = yMaxPower * -1;
} else {
pidY = yMaxPower;
}
} else {
pidY = rawPidX;
}
} else {
pidY = rawPidX;
}
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
if (Math.abs(xTarget - positionX) > 5) {
if (Math.abs(YPIDControl(yTarget, positionY)) >= xMaxPower) {
if (YPIDControl(yTarget, positionY) < 0) {
pidX = xMaxPower * -1;
} else {
pidX = xMaxPower;
}
} else {
pidX = rawPidY;
}
} else {
pidX = rawPidY;
}
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
// field oriented math, (rotating the global field to the relative field)
@@ -366,7 +366,7 @@ public class CompetitionRobotV1 extends Robot {
lift.setPower(-0.6);
} else {
shoulder.setPosition(0.38);
target = 100;
target = 120;
}
}

31
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -23,17 +23,18 @@ public class CompetitionTeleOpState extends CyberarmState {
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public int target = 0;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
private double targetHeading;
public double collectLock = Math.toRadians(-90);
public double backDropLock = Math.toRadians(90);
public double collectLock = Math.toRadians(90);
public double backDropLock = Math.toRadians(-90);
public double boost;
public double armPower;
private double currentHeading;
private boolean headingLock = false;
public static double Kp = 1;
public static double Kp = 0.8;
public static double Ki = 0;
public static double Kd = 0;
private double lastError = 0;
@@ -53,7 +54,7 @@ public class CompetitionTeleOpState extends CyberarmState {
//---------------------------------------------------------------------------------------------------------------- Drivetrain Variables:
private boolean lbsVar1;
private double drivePower = 1;
public double rx;
public double rx = engine.gamepad1.right_stick_x / 2;
// ------------------------------------------------------------------------------------------------------------------- Slider Variables:
private int maxExtension = 2000;
@@ -117,8 +118,12 @@ public class CompetitionTeleOpState extends CyberarmState {
}
lbsVar1 = lbs1;
double x = engine.gamepad1.left_stick_x;
double y = -engine.gamepad1.left_stick_y;
if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){
boost = engine.gamepad1.right_trigger + 1;
}
double x = -((engine.gamepad1.left_stick_x * 0.5) * boost);
double y = ((engine.gamepad1.left_stick_y * 0.5) * boost);
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
// angle math to make things field oriented
@@ -134,10 +139,10 @@ public class CompetitionTeleOpState extends CyberarmState {
// setting each power determined previously from the math above
// as well as multiplying it by a drive power that can be changed.
robot.backLeft.setPower(backLeftPower * drivePower);
robot.backRight.setPower(-backRightPower * drivePower);
robot.frontLeft.setPower(frontLeftPower * drivePower);
robot.frontRight.setPower(frontRightPower * drivePower);
robot.backLeft.setPower(backLeftPower);
robot.backRight.setPower(-backRightPower);
robot.frontLeft.setPower(frontLeftPower);
robot.frontRight.setPower(frontRightPower);
}
public double angleWrap(double radians) {
@@ -200,9 +205,6 @@ public class CompetitionTeleOpState extends CyberarmState {
if (engine.gamepad2.a) {
armPos = "collect";
depositMode = false;
// } else if (engine.gamepad2.x) {
// armPos = "passive";
// depositMode = false;
} else if (engine.gamepad2.y) {
armPos = "deposit";
depositMode = true;
@@ -286,6 +288,7 @@ public class CompetitionTeleOpState extends CyberarmState {
public void init() {
super.init();
pidController = new PIDController(p, i, d);
robot.imu.resetYaw();
}