diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java deleted file mode 100644 index 7cd7000..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java +++ /dev/null @@ -1,177 +0,0 @@ -//package org.firstinspires.ftc.teamcode; -// -//import androidx.annotation.NonNull; -//import androidx.annotation.Nullable; -// -//import com.qualcomm.hardware.bosch.BNO055IMU; -// -//import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; -//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -//import org.firstinspires.ftc.robotcore.external.navigation.NavUtil; -//import org.firstinspires.ftc.robotcore.external.navigation.Position; -//import org.firstinspires.ftc.robotcore.external.navigation.Velocity; -// -//public class AutonMovement { -// final Hardware hardware; -// final MovementAPI movementAPI; -// final CustomAccelerationIntegrator integrator; -//// ExecutorService integratorThread; -//// final Object integratorLock = new Object(); -// -// public AutonMovement(Hardware hardware) { -// this.hardware = hardware; -// this.movementAPI = new MovementAPI(hardware); -// this.integrator = new CustomAccelerationIntegrator(FieldVector.ZERO); -// this.hardware.initIMU(); -// this.hardware.initPosIMU(this.integrator); -// -//// this.integratorThread = ThreadPool.newSingleThreadExecutor("imu acceleration integration"); -//// this.integratorThread.execute(() -> { -//// while (!Thread.currentThread().isInterrupted()) { -//// synchronized (this.integratorLock) { -//// this.integrator.update(); -//// } -//// -//// Thread.yield(); -//// } -//// }); -// } -// -// public void move(FieldVector relativePos) { -// this.moveTo(this.hardware.getIMUPosition().add(relativePos)); -// } -// -// public void moveTo(FieldVector targetPos) { -// final double KP = 1; -// final double KI = 0; -// final double KD = 0; -// -// FieldVector currentPos2 = this.hardware.getIMUPosition(); -// -// double scaleFactor = currentPos2.sub(targetPos).magnitude(); -// -// FieldVector integral = FieldVector.ZERO; -// FieldVector prevPos = currentPos2; -// -// while (true) { -// FieldVector currentPos = this.hardware.getIMUPosition(); -// FieldVector posDiff = targetPos.sub(currentPos); -// -// if (posDiff.sqMagnitude() < 100 * 100) return; -// -// integral = integral.add(posDiff); -// -// FieldVector unscaled = posDiff.mul(KP) -// .add(currentPos.sub(prevPos).mul(KD)) -// .add(integral.mul(KI)); -// -// FieldVector scaled = unscaled.div(scaleFactor * 2); -// -// double yaw = this.hardware.getImuYaw(); -// -// FieldVector rotated = scaled.rot(-yaw); -// -// this.movementAPI.move(rotated.x, rotated.y, 0, 0.7); -// -// prevPos = currentPos; -// } -// } -// -// public void rotTo(double targetAngle) { -// final double KP = 1; -// final double KI = 0; -// final double KD = 0; -// -// double integral = 0; -// double prevYaw = this.hardware.getImuYaw(); -// -// while (true) { -// double currentYaw = this.hardware.getImuYaw(); -// double yawDiff = targetAngle - currentYaw; -// -// if (yawDiff < 2) return; -// -// integral += yawDiff; -// -// double turnSpeed = yawDiff * KP -// + (currentYaw - prevYaw) * KD -// + integral * KI; -// -// // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw -// this.movementAPI.move(0, 0, turnSpeed, 0.7); -// -// prevYaw = currentYaw; -// } -// } -// -// public static class CustomAccelerationIntegrator implements BNO055IMU.AccelerationIntegrator { -// BNO055IMU.Parameters parameters = null; -// Position position = new Position(); -// Velocity velocity = new Velocity(); -// Acceleration acceleration = null; -// -// public Position getPosition() { -// return this.position; -// } -// -// public Velocity getVelocity() { -// return this.velocity; -// } -// -// public Acceleration getAcceleration() { -// return this.acceleration; -// } -// -// public CustomAccelerationIntegrator(FieldVector initialPosition) { -// this.position = new Position(DistanceUnit.MM, initialPosition.x, initialPosition.y, 0, 0); -// } -// -// public void initialize(@NonNull BNO055IMU.Parameters parameters, @Nullable Position initialPosition, @Nullable Velocity initialVelocity) { -// this.parameters = parameters; -// this.position = initialPosition != null ? initialPosition : this.position; -// this.velocity = initialVelocity != null ? initialVelocity : this.velocity; -// this.acceleration = null; -// } -// -// public void update(Acceleration linearAcceleration) { -// if (linearAcceleration.acquisitionTime != 0L) { -// if (Math.sqrt( -// linearAcceleration.xAccel * linearAcceleration.xAccel -// + linearAcceleration.yAccel * linearAcceleration.yAccel -// + linearAcceleration.zAccel * linearAcceleration.zAccel -// ) < 0.05) { -// linearAcceleration.xAccel = 0; -// linearAcceleration.yAccel = 0; -// linearAcceleration.zAccel = 0; -// } -// -// if (this.acceleration != null) { -// Acceleration accelPrev = this.acceleration; -// Velocity velocityPrev = this.velocity; -// this.acceleration = linearAcceleration; -// if (accelPrev.acquisitionTime != 0L) { -// Velocity deltaVelocity = NavUtil.meanIntegrate(this.acceleration, accelPrev); -// this.velocity = NavUtil.plus(this.velocity, deltaVelocity); -// } -// -// if (Math.sqrt( -// this.velocity.xVeloc * this.velocity.xVeloc -// + this.velocity.yVeloc * this.velocity.yVeloc -// + this.velocity.zVeloc * this.velocity.zVeloc -// ) < 0.05) { -// this.velocity.xVeloc = 0; -// this.velocity.yVeloc = 0; -// this.velocity.zVeloc = 0; -// } -// -// if (velocityPrev.acquisitionTime != 0L) { -// Position deltaPosition = NavUtil.meanIntegrate(this.velocity, velocityPrev); -// this.position = NavUtil.plus(this.position, deltaPosition); -// } -// } else { -// this.acceleration = linearAcceleration; -// } -// } -// } -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java deleted file mode 100644 index 200ff5e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java +++ /dev/null @@ -1,119 +0,0 @@ -//package org.firstinspires.ftc.teamcode; -// -//import com.qualcomm.robotcore.eventloop.opmode.OpMode; -// -//public class AutonMovement2 { -// final Hardware hardware; -// final MovementAPI movementAPI; -// final OpMode opMode; -// FieldVector currentPosition; -// -// public AutonMovement2(OpMode opMode, FieldVector initialPosition) { -// this.opMode = opMode; -// this.hardware = new Hardware(opMode); -// this.movementAPI = new MovementAPI(hardware); -// this.hardware.initIMU(); -// this.currentPosition = initialPosition; -// } -// -// public void move(FieldVector relativePos) { -// this.moveTo(currentPosition.add(relativePos)); -// } -// -// public void moveTo(FieldVector targetPos) { -// final double KP = 1; -// final double KI = 0.3; -// final double KD = 0.5; -// -// final double encoderTicksPerRev = 537.7; -// final double mmPerRev = 96 /* diameter of gobilda mecanums */ * Math.PI; -// final double encoderTicksPerMm = encoderTicksPerRev / mmPerRev; -// -// FieldVector posDiffNormalized = targetPos.sub(currentPosition).normalized(); -// MotorPowers powers = MotorPowers.calculateUnscaled(posDiffNormalized.x, posDiffNormalized.y, 0, 1); -// -// final double port0EncoderScale = powers.fl; -// final double port3EncoderScale = powers.fr; -// -// double port0EncoderStart = hardware.fl.getCurrentPosition(); -// double port3EncoderStart = hardware.fr.getCurrentPosition(); -// -// FieldVector integral = FieldVector.ZERO; -// FieldVector prevPos = this.currentPosition; -// -// while (true) { -// double port0Encoder = hardware.fl.getCurrentPosition(); -// double port3Encoder = hardware.fr.getCurrentPosition(); -// -// double port0EncoderScaled = (port0Encoder - port0EncoderStart) / port0EncoderScale; -// double port3EncoderScaled = (port3Encoder - port3EncoderStart) / port3EncoderScale; -// -// double encoderTicksMoved = (port0EncoderScaled + port3EncoderScaled) / 2; -// double mmMoved = encoderTicksMoved / encoderTicksPerMm; -// -// opMode.telemetry.addLine("the port 0 encoder is " + port0Encoder); -// opMode.telemetry.addLine("and it started as " + port0EncoderStart); -// opMode.telemetry.addLine("oh and the scale value is " + port0EncoderScale); -// opMode.telemetry.addLine("the port 3 encoder is " + port3Encoder); -// opMode.telemetry.addLine("and it started as " + port3EncoderStart); -// opMode.telemetry.addLine("oh and the scale value is " + port3EncoderScale); -// opMode.telemetry.addLine("i think we have moved " + encoderTicksMoved + " encoder ticks"); -// opMode.telemetry.addLine("i think we have moved " + mmMoved + "mm"); -// -// FieldVector currentPos = posDiffNormalized.mul(mmMoved).add(this.currentPosition /* actually more like initialPosition */); -// FieldVector posDiff = targetPos.sub(currentPos); -// -// opMode.telemetry.addLine("i think we are at (" + currentPos.x + ", " + currentPos.y + ")"); -// opMode.telemetry.update(); -// -// if (posDiff.sqMagnitude() < 5 * 5) { -// movementAPI.stop(); -// this.currentPosition = currentPos; // finally update `this.currentPosition` -// return; -// } -// -// integral = integral.add(posDiff); -// -// FieldVector unscaled = posDiff.mul(KP) -// .add(currentPos.sub(prevPos).mul(KD)) -// .add(integral.mul(KI)); -// -// FieldVector scaled = unscaled.div(Distance.MM_PER_FOOT); -// -// double yaw = this.hardware.getImuYaw(); -// -// FieldVector rotated = scaled.rot(yaw); -// -// this.movementAPI.move(rotated.x, rotated.y, 0, 0.4); -// -// prevPos = currentPos; -// } -// } -// -// public void rotTo(double targetAngle) { -// final double KP = 1; -// final double KI = 0; -// final double KD = 0; -// -// double integral = 0; -// double prevYaw = this.hardware.getImuYaw(); -// -// while (true) { -// double currentYaw = this.hardware.getImuYaw(); -// double yawDiff = targetAngle - currentYaw; -// -// if (yawDiff < 2) return; -// -// integral += yawDiff; -// -// double turnSpeed = yawDiff * KP -// + (currentYaw - prevYaw) * KD -// + integral * KI; -// -// // note: should be `-turnSpeed` but `MovementAPI.move` has the reverse mapping of +/- to cw/ccw -// this.movementAPI.move(0, 0, turnSpeed / 15, 0.4); -// -// prevYaw = currentYaw; -// } -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java deleted file mode 100644 index 7cb9894..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java +++ /dev/null @@ -1,68 +0,0 @@ -//package org.firstinspires.ftc.teamcode; -// -//public class AutonMovement3 { -// Hardware hardware; -// MovementAPI movement; -// -// double speed; -// -// public AutonMovement3(Hardware hardware, double speed) { -// this.hardware = hardware; -// this.movement = new MovementAPI(hardware); -// this.speed = speed; -// } -// -// private void sleep(double seconds) { -// double startTime = System.nanoTime(); -// double targetTime = startTime + seconds * 1000 * 1000 * 1000; -// -// while (System.nanoTime() < targetTime); -// } -// -// public void straight(double power, double tiles) { -// this.moveFor(0, power, tiles * (23/18.)); -// } -// -// public void strafe(double power, double tiles) { -// this.moveFor(power, 0, tiles * (23/17.)); -// } -// -// public void moveFor(double powerX, double powerY, double seconds) { -// movement.move(powerX, powerY, 0, this.speed); -// -// this.sleep(seconds); -// movement.stop(); -// this.sleep(1); -// } -// -// private double modAngle(double angle) { -// double angle2 = angle % 360; -// if (angle2 >= 180) { -// return angle2 - 360; -// } else { -// return angle2; -// } -// } -// -// public void turn(double degrees) { -// double startYaw = hardware.getImuYaw(); -// double targetYaw = startYaw + (-degrees); -// -// while (true) { -// double yaw = hardware.getImuYaw(); -// double yawDiff = this.modAngle(targetYaw - yaw); -// -// if (Math.abs(yawDiff) < 1) { -// break; -// } -// -// if (Math.abs(yawDiff) > 20) { -// movement.move(0, 0, Math.signum(yawDiff), this.speed); -// } else { -// movement.move(0, 0, Math.signum(yawDiff) * (yawDiff / 40 + 0.5), this.speed); -// } -// } -// movement.stop(); -// this.sleep(0.3); -// } -//}