TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 15:32: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

270
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,163 +61,184 @@ 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; // ramping up
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition; if (RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight &&
LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft) {
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up
// double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
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 // Driving Normal
if (targetDrivePower > 0){ else if (RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight &&
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25); LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft) {
drivePower = 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 { } else {
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25); 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
drivePower = targetDrivePower;
} }
} else { if (targetDrivePower < 0 && drivePower > 0) {
// middle ground drivePower = drivePower * -1;
drivePower = targetDrivePower; }
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ if (LeftCurrentPosition - traveledDistance < maximumTolerance || Math.abs(RightCurrentPosition - traveledDistance) < maximumTolerance) {
// This is limiting drive power to the targeted drive power if (targetAchieved) {
drivePower = targetDrivePower; drivePower = drivePower * 0.15;
}
if (targetDrivePower < 0 && drivePower > 0) { if (Math.abs(drivePower) < 0.15) {
drivePower = drivePower * -1; if (drivePower < 0) {
} drivePower = -0.15;
} else {
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){ drivePower = 0.15;
if (targetAchieved) { }
drivePower = drivePower * 0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
} }
} }
} robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); robot.backRightDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); 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 {
drivePower = 0.15;
}
}
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
} else {
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)) {
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0;
} else {
drivePower = 0.15;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
}
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)) {
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)) {
drivePower = 0;
} else {
drivePower = 0.15;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
} else { } else {
drivePower = 0.25; robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
} }
} }
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
else {
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
}
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}
// //
} }
@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());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower()); engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower()); engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition()); engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle); engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
engine.telemetry.addData("Target Achieved", targetAchieved); engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("targetDrivePower", targetDrivePower); engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addData("traveledDistance", traveledDistance); Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("RampUpDistance", RampUpDistance); Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("RampDownDistance", RampDownDistance); Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
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);