remove legacy code now that its in the history
This commit is contained in:
parent
8cdd3dd447
commit
c4e2385d0a
|
|
@ -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;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
|
@ -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;
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
|
@ -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);
|
||||
// }
|
||||
//}
|
||||
Loading…
Reference in a new issue