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

autonomous work

Browse Source
This commit is contained in:
SpencerPiha
2023-02-02 20:35:01 -06:00
parent 0fb19f030c
commit 8f9a850087
2 changed files with 155 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -18,7 +18,17 @@ public class DriverStateWithOdometer extends CyberarmState {
private double CurrentPosition; private double CurrentPosition;
public final double WHEEL_CIRCUMFERENCE = 7.42108499; public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192; public final int COUNTS_PER_REVOLUTION = 8192;
public double MINIMUM_POWER = 0.25;
public final double distanceMultiplier; public final double distanceMultiplier;
public double startOfRampUpRight;
public double startOfRampDownRight;
public double startOfRampUpLeft;
public double startOfRampDownLeft;
public double endOfRampUpRight;
public double endOfRampDownRight;
public double endOfRampUpLeft;
public double endOfRampDownLeft;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot; this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -31,6 +41,7 @@ public class DriverStateWithOdometer extends CyberarmState {
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
} }
private double drivePower, targetDrivePower; private double drivePower, targetDrivePower;
private int traveledDistance; private int traveledDistance;
@@ -50,47 +61,73 @@ public class DriverStateWithOdometer extends CyberarmState {
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier); maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
if (drivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
}
} }
@Override @Override
public void exec() { public void exec() {
if (stateDisabled) { if (stateDisabled) {
setHasFinished(true); setHasFinished(true);
return; return;
} }
double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition()); double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition()); double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up // ramping up
// double ratio = (Math.abs(CurrentPosition) / RampUpDistance); if (RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight &&
LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft) {
if (targetDrivePower > 0) { if (targetDrivePower > 0) {
drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25; drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + MINIMUM_POWER;
} else { } else {
drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25; drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - MINIMUM_POWER;
} }
} }
else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
// ramping down
if (targetDrivePower > 0){
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
} else {
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
}
} else { // Driving Normal
// middle ground else if (RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight &&
LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft) {
drivePower = targetDrivePower; drivePower = targetDrivePower;
} }
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ // Ramping down
else if (RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight &&
LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft) {
if (targetDrivePower > 0) {
drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampDownRight - RightCurrentPosition) / RampDownDistance) - MINIMUM_POWER;
} else {
drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampDownRight) / RampDownDistance) + MINIMUM_POWER;
}
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power // This is limiting drive power to the targeted drive power
drivePower = targetDrivePower; drivePower = targetDrivePower;
} }
@@ -99,15 +136,15 @@ public class DriverStateWithOdometer extends CyberarmState {
drivePower = drivePower * -1; drivePower = drivePower * -1;
} }
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){ if (LeftCurrentPosition - traveledDistance < maximumTolerance || Math.abs(RightCurrentPosition - traveledDistance) < maximumTolerance) {
if (targetAchieved) { if (targetAchieved) {
drivePower = drivePower * 0.25; drivePower = drivePower * 0.15;
if (Math.abs(drivePower) < 0.25){ if (Math.abs(drivePower) < 0.15) {
if (drivePower < 0) { if (drivePower < 0) {
drivePower = -0.25; drivePower = -0.15;
} else { } else {
drivePower = 0.25; drivePower = 0.15;
} }
} }
} }
@@ -116,17 +153,16 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} } else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
targetAchieved = true; targetAchieved = true;
drivePower = targetDrivePower * -0.25; drivePower = targetDrivePower * -0.15;
if (Math.abs(drivePower) < 0.25){ if (Math.abs(drivePower) < 0.15) {
if (drivePower < 0) { if (drivePower < 0) {
drivePower = -0.25; drivePower = -0.15;
} else { } else {
drivePower = 0.25; drivePower = 0.15;
} }
} }
@@ -136,16 +172,14 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} } else {
else { if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)) {
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) { if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0; drivePower = 0;
} else { } else {
drivePower = 0.25; drivePower = 0.15;
robot.backLeftDrive.setPower(-drivePower); robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(-drivePower);
@@ -153,20 +187,18 @@ public class DriverStateWithOdometer extends CyberarmState {
} }
} }
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){ if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)) {
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){ if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)) {
drivePower = 0; drivePower = 0;
} else { } else {
drivePower = 0.25; drivePower = 0.15;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
} } else {
else {
robot.backLeftDrive.setPower(0); robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0); robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0); robot.frontLeftDrive.setPower(0);
@@ -179,7 +211,7 @@ public class DriverStateWithOdometer extends CyberarmState {
} }
@Override @Override
public void telemetry() { public void telemetry () {
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
@@ -196,7 +228,6 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("Target Achieved", targetAchieved); engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -209,4 +240,5 @@ public class DriverStateWithOdometer extends CyberarmState {
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle); Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
} }
} }

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

@@ -212,11 +212,10 @@ public class PhoenixBot1 {
// HighRiserRight.setDirection(Servo.Direction.FORWARD); // 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);
ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE); // ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); // ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
CameraServo.setDirection(Servo.Direction.FORWARD); CameraServo.setDirection(Servo.Direction.FORWARD);