missing
8cdd3dd447
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting https://xkcd.com/1296/
178 lines
6.7 KiB
Java
178 lines
6.7 KiB
Java
//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;
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|