mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-13 06:22:33 +00:00
autonomous work
This commit is contained in:
SpencerPiha
@@ -18,7 +18,17 @@ public class DriverStateWithOdometer extends CyberarmState {
|
||||
private double CurrentPosition;
|
||||
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
|
||||
public final int COUNTS_PER_REVOLUTION = 8192;
|
||||
public double MINIMUM_POWER = 0.25;
|
||||
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) {
|
||||
this.robot = robot;
|
||||
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;
|
||||
}
|
||||
|
||||
private double drivePower, targetDrivePower;
|
||||
private int traveledDistance;
|
||||
|
||||
@@ -50,163 +61,184 @@ public class DriverStateWithOdometer extends CyberarmState {
|
||||
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
|
||||
public void exec() {
|
||||
|
||||
|
||||
if (stateDisabled) {
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
|
||||
double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
|
||||
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
|
||||
double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
|
||||
double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
|
||||
|
||||
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
|
||||
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
|
||||
// ramping up
|
||||
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) {
|
||||
drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
|
||||
drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + MINIMUM_POWER;
|
||||
} 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);
|
||||
|
||||
// Driving Normal
|
||||
else if (RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight &&
|
||||
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 {
|
||||
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 {
|
||||
// middle ground
|
||||
drivePower = targetDrivePower;
|
||||
}
|
||||
if (targetDrivePower < 0 && drivePower > 0) {
|
||||
drivePower = drivePower * -1;
|
||||
}
|
||||
|
||||
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
|
||||
// This is limiting drive power to the targeted drive power
|
||||
drivePower = targetDrivePower;
|
||||
}
|
||||
if (LeftCurrentPosition - traveledDistance < maximumTolerance || Math.abs(RightCurrentPosition - traveledDistance) < maximumTolerance) {
|
||||
if (targetAchieved) {
|
||||
drivePower = drivePower * 0.15;
|
||||
|
||||
if (targetDrivePower < 0 && drivePower > 0) {
|
||||
drivePower = drivePower * -1;
|
||||
}
|
||||
|
||||
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
|
||||
if (targetAchieved) {
|
||||
drivePower = drivePower * 0.25;
|
||||
|
||||
if (Math.abs(drivePower) < 0.25){
|
||||
if (drivePower < 0) {
|
||||
drivePower = -0.25;
|
||||
} else {
|
||||
drivePower = 0.25;
|
||||
if (Math.abs(drivePower) < 0.15) {
|
||||
if (drivePower < 0) {
|
||||
drivePower = -0.15;
|
||||
} else {
|
||||
drivePower = 0.15;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
}
|
||||
else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
|
||||
targetAchieved = true;
|
||||
} else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
|
||||
targetAchieved = true;
|
||||
|
||||
drivePower = targetDrivePower * -0.25;
|
||||
drivePower = targetDrivePower * -0.15;
|
||||
|
||||
if (Math.abs(drivePower) < 0.25){
|
||||
if (drivePower < 0) {
|
||||
drivePower = -0.25;
|
||||
if (Math.abs(drivePower) < 0.15) {
|
||||
if (drivePower < 0) {
|
||||
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 {
|
||||
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
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
|
||||
engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
|
||||
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
|
||||
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
|
||||
@Override
|
||||
public void telemetry () {
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
|
||||
engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
|
||||
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
|
||||
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("targetDrivePower", targetDrivePower);
|
||||
engine.telemetry.addData("traveledDistance", traveledDistance);
|
||||
engine.telemetry.addData("RampUpDistance", RampUpDistance);
|
||||
engine.telemetry.addData("RampDownDistance", RampDownDistance);
|
||||
|
||||
engine.telemetry.addData("traveledDistance", traveledDistance);
|
||||
engine.telemetry.addData("RampUpDistance", RampUpDistance);
|
||||
engine.telemetry.addData("RampDownDistance", RampDownDistance);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -212,11 +212,10 @@ public class PhoenixBot1 {
|
||||
// HighRiserRight.setDirection(Servo.Direction.FORWARD);
|
||||
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
|
||||
LowRiserRight.setDirection(Servo.Direction.REVERSE);
|
||||
ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
// ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
// ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
// ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
CameraServo.setDirection(Servo.Direction.FORWARD);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user