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