missing/power-play
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

literally everything ive done over the past >2 months in one commit lol

Browse source 
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting

https://xkcd.com/1296/
This commit is contained in:
missing 2023-01-23 23:52:26 -06:00
parent 217460af6b
commit 8cdd3dd447
40 changed files with 2677 additions and 122 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
.idea/jarRepositories.xml
View file

@ -21,5 +21,10 @@
<option name="name" value="Google" />
<option name="url" value="https://dl.google.com/dl/android/maven2/" />
</remote-repository>
<remote-repository>
<option name="id" value="maven" />
<option name="name" value="maven" />
<option name="url" value="https://maven.brott.dev/" />
</remote-repository>
</component>
</project>

5
TeamCode/build.gradle
View file

@ -26,4 +26,7 @@ android {
dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
}
implementation 'org.openftc:easyopencv:1.5.2'
implementation 'org.openftc:apriltag:1.1.0'
}

177
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement.java Normal file
View file

@ -0,0 +1,177 @@
//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;
// }
// }
// }
// }
//}

119
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement2.java Normal file
View file

@ -0,0 +1,119 @@
//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;
// }
// }
//}

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonMovement3.java Normal file
View file

@ -0,0 +1,68 @@
//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);
// }
//}

228
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java Normal file
View file

@ -0,0 +1,228 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
import org.firstinspires.ftc.teamcode.kinematics.Pose;
public class DashboardTelemetryWrapper implements Telemetry {
private final FtcDashboard dashboard;
private TelemetryPacket packet;
private final TelemetryLog log;
public DashboardTelemetryWrapper(FtcDashboard dashboard) {
this.dashboard = dashboard;
this.packet = new TelemetryPacket();
this.log = this.new TelemetryLog();
}
private void rawDrawRobot(FieldVector pos, double yaw) {
FieldVector topLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
FieldVector topRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
FieldVector bottomLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
FieldVector bottomRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
Canvas canvas = this.packet.fieldOverlay();
canvas.fillPolygon(
new double[]{topLeft.x.valInInches(), topRight.x.valInInches(), bottomRight.x.valInInches(), bottomLeft.x.valInInches()},
new double[]{topLeft.y.valInInches(), topRight.y.valInInches(), bottomRight.y.valInInches(), bottomLeft.y.valInInches()}
);
}
public void drawRobot(FieldVector pos, double yaw) {
Canvas canvas = this.packet.fieldOverlay();
canvas.setStrokeWidth(1);
canvas.setFill("gray");
canvas.setStroke("black");
this.rawDrawRobot(pos, yaw);
}
public void drawTargetOnDashboard(Pose target, Pose current) {
Canvas canvas = this.packet.fieldOverlay();
canvas.setStrokeWidth(1);
canvas.setFill("transparent");
canvas.setStroke("green");
canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 1);
canvas.strokeLine(
current.pos.x.valInInches(),
current.pos.y.valInInches(),
target.pos.x.valInInches(),
target.pos.y.valInInches()
);
this.rawDrawRobot(target.pos, target.yaw);
}
@Override
public Item addData(String caption, String format, Object... args) {
return this.addData(caption, String.format(format, args));
}
@Override
public Item addData(String caption, Object value) {
this.packet.put(caption, value);
return null;
}
@Override
public <T> Item addData(String caption, Func<T> valueProducer) {
return this.addData(caption, valueProducer.value());
}
@Override
public <T> Item addData(String caption, String format, Func<T> valueProducer) {
return this.addData(caption, format, valueProducer.value());
}
@Override
public boolean removeItem(Item item) {
throw new UnsupportedOperationException();
}
@Override
public void clear() {
this.dashboard.clearTelemetry();
this.packet = new TelemetryPacket();
}
@Override
public void clearAll() {
this.clear();
}
@Override
public Object addAction(Runnable action) {
throw new UnsupportedOperationException();
}
@Override
public boolean removeAction(Object token) {
throw new UnsupportedOperationException();
}
@Override
public void speak(String text) {
throw new UnsupportedOperationException();
}
@Override
public void speak(String text, String languageCode, String countryCode) {
throw new UnsupportedOperationException();
}
@Override
public boolean update() {
this.dashboard.sendTelemetryPacket(packet);
this.packet = new TelemetryPacket();
return true;
}
@Override
public Line addLine() {
return null;
}
@Override
public Line addLine(String lineCaption) {
this.packet.addLine(lineCaption);
return null;
}
@Override
public boolean removeLine(Line line) {
throw new UnsupportedOperationException();
}
@Override
public boolean isAutoClear() {
return false;
}
@Override
public void setAutoClear(boolean autoClear) {
throw new UnsupportedOperationException();
}
@Override
public int getMsTransmissionInterval() {
return this.dashboard.getTelemetryTransmissionInterval();
}
@Override
public void setMsTransmissionInterval(int msTransmissionInterval) {
this.dashboard.setTelemetryTransmissionInterval(msTransmissionInterval);
}
@Override
public String getItemSeparator() {
return null;
}
@Override
public void setItemSeparator(String itemSeparator) {
}
@Override
public String getCaptionValueSeparator() {
return null;
}
@Override
public void setCaptionValueSeparator(String captionValueSeparator) {
}
@Override
public void setDisplayFormat(DisplayFormat displayFormat) {
}
@Override
public Log log() {
return log;
}
private class TelemetryLog implements Log {
@Override
public int getCapacity() {
return 0;
}
@Override
public void setCapacity(int capacity) {
}
@Override
public DisplayOrder getDisplayOrder() {
return DisplayOrder.OLDEST_FIRST;
}
@Override
public void setDisplayOrder(DisplayOrder displayOrder) {
}
@Override
public void add(String entry) {
packet.addLine(entry);
}
@Override
public void add(String format, Object... args) {
this.add(String.format(format, args));
}
@Override
public void clear() {
packet.clearLines();
}
}
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
View file

@ -1,46 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@TeleOp(name = "BBBBBBBBB")
public class Gamepad extends OpMode {
Hardware hardware;
MovementAPI movement;
double speed = 0.6;
@Override
public void init() {
hardware = new Hardware(this);
movement = new MovementAPI(hardware);
hardware.claw.setPosition(1);
hardware.wrist.setPosition(0.5);
}
@Override
public void loop() {
if (gamepad1.left_bumper) {
speed -= 0.005;
}
if (gamepad1.right_bumper) {
speed += 0.005;
}
speed = Math.max(0, Math.min(speed, 1));
hardware.spool.setPower(gamepad2.left_stick_y);
hardware.claw.setPosition(1 - gamepad2.left_trigger);
hardware.wrist.setPosition((-gamepad2.right_stick_y + 1) / 2);
movement.move(
gamepad1.left_stick_x,
-gamepad1.left_stick_y,
gamepad1.right_stick_x,
speed
);
telemetry.addLine(speed + "");
}
}

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
View file

@ -1,34 +1,44 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import java.util.List;
public class Hardware {
public final OpMode opMode;
public final DashboardTelemetryWrapper dashboardTelemetryWrapper;
public final DcMotor fl;
public final DcMotor fr;
public final DcMotor bl;
public final DcMotor br;
public final DcMotor spool;
public final Servo claw;
public final Servo clawLeft;
public final Servo clawRight;
public final Servo wrist;
public final BNO055IMU imu;
public final IMU imu;
public final List<LynxModule> hubs;
public Hardware(OpMode opMode) {
this(opMode.hardwareMap);
}
HardwareMap hm = opMode.hardwareMap;
this.opMode = opMode;
this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper);
public Hardware(HardwareMap hardwareMap) {
this.fl = hardwareMap.get(DcMotor.class, "fl");
this.fr = hardwareMap.get(DcMotor.class, "fr");
this.bl = hardwareMap.get(DcMotor.class, "bl");
this.br = hardwareMap.get(DcMotor.class, "br");
this.fl = hm.get(DcMotor.class, "fl");
this.fr = hm.get(DcMotor.class, "fr");
this.bl = hm.get(DcMotor.class, "bl");
this.br = hm.get(DcMotor.class, "br");
this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
@ -38,22 +48,72 @@ public class Hardware {
this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.spool = hardwareMap.get(DcMotor.class, "spool");
this.claw = hardwareMap.get(Servo.class, "claw");
this.wrist = hardwareMap.get(Servo.class, "wrist");
this.imu = hardwareMap.get(BNO055IMU.class, "imu");
this.spool = hm.get(DcMotor.class, "spool");
this.clawLeft = hm.get(Servo.class, "clawLeft");
this.clawRight = hm.get(Servo.class, "clawRight");
this.wrist = hm.get(Servo.class, "wrist");
this.imu = hm.get(IMU.class, "imu");
// this.posImu = hm.get(BNO055IMU.class, "imu");
this.spool.setDirection(DcMotorSimple.Direction.FORWARD);
this.spool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.clawLeft.scaleRange(0, 1/3.);
this.clawRight.scaleRange(2/3., 1);
this.wrist.scaleRange(0.25, 1);
this.hubs = hm.getAll(LynxModule.class);
for (LynxModule hub : this.hubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
}
public void setClawPosition(double pos) {
this.clawLeft.setPosition(1 - pos);
this.clawRight.setPosition(pos);
}
public void setWristPosition(double pos) {
this.wrist.setPosition((pos + 1) / 2);
}
public void initIMU() {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.calibrationDataFile = "BNO055IMUCalibration.json";
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.FORWARD,
RevHubOrientationOnRobot.UsbFacingDirection.LEFT
)
);
this.imu.initialize(parameters);
this.imu.startAccelerationIntegration(new Position(), new Velocity(), 1);
}
// public void initPosIMU(BNO055IMU.AccelerationIntegrator integrator) {
// BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
// parameters.calibrationDataFile = "AdafruitIMUCalibration.json";
// parameters.accelerationIntegrationAlgorithm = integrator;
//
// this.posImu.initialize(parameters);
// this.posImu.stopAccelerationIntegration();
// }
public double getImuYaw() {
return -this.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
public int getLiftEncoder() {
return -spool.getCurrentPosition();
}
// public FieldVector getIMUPosition() {
// Position tmp = this.posImu.getPosition().toUnit(DistanceUnit.MM);
// return FieldVector.inMM(tmp.x, tmp.y);
// }
}

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorPowers.java Normal file
View file

@ -0,0 +1,43 @@
package org.firstinspires.ftc.teamcode;
public class MotorPowers {
public final double fl;
public final double fr;
public final double bl;
public final double br;
public MotorPowers(double fl, double fr, double bl, double br) {
this.fl = fl;
this.fr = fr;
this.bl = bl;
this.br = br;
}
public static MotorPowers calculateUnscaled(double powerX, double powerY, double turn, double speed) {
double fl = (powerY + powerX + turn) * speed;
double fr = (powerY - powerX - turn) * speed;
double bl = (powerY - powerX + turn) * speed;
double br = (powerY + powerX - turn) * speed;
return new MotorPowers(fl, fr, bl, br);
}
public MotorPowers scaled() {
double scale = 1;
scale = Math.max(scale, Math.abs(this.fl));
scale = Math.max(scale, Math.abs(this.fr));
scale = Math.max(scale, Math.abs(this.bl));
scale = Math.max(scale, Math.abs(this.br));
double fl = this.fl / scale;
double fr = this.fr / scale;
double bl = this.bl / scale;
double br = this.br / scale;
return new MotorPowers(fl, fr, bl, br);
}
public static MotorPowers calculate(double powerX, double powerY, double turn, double speed) {
return MotorPowers.calculateUnscaled(powerX, powerY, turn, speed).scaled();
}
}

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MovementAPI.java
View file

@ -1,58 +1,29 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class MovementAPI {
private final DcMotor fl;
private final DcMotor fr;
private final DcMotor bl;
private final DcMotor br;
public DcMotor getFL() { return this.fl; }
public DcMotor getFR() { return this.fr; }
public DcMotor getBL() { return this.bl; }
public DcMotor getBR() { return this.br; }
private final Hardware hardware;
public MovementAPI(Hardware hardware) {
this(
hardware.fl,
hardware.fr,
hardware.bl,
hardware.br
);
this.hardware = hardware;
}
public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) {
this.fl = fl;
this.fr = fr;
this.bl = bl;
this.br = br;
public void move(MotorPowers powers) {
hardware.fl.setPower(powers.fl);
hardware.fr.setPower(powers.fr);
hardware.bl.setPower(powers.bl);
hardware.br.setPower(powers.br);
}
public void move(double powerX, double powerY, double turn, double speed) {
double flPower = (powerY + powerX + turn) * speed;
double frPower = (powerY - powerX - turn) * speed;
double blPower = (powerY - powerX + turn) * speed;
double brPower = (powerY + powerX - turn) * speed;
double scale = Math.max(1, (Math.abs(powerY) + Math.abs(turn) + Math.abs(powerX)) * Math.abs(speed)); // shortcut for max(abs([fl,fr,bl,br]))
flPower /= scale;
frPower /= scale;
blPower /= scale;
brPower /= scale;
fl.setPower(flPower);
fr.setPower(frPower);
bl.setPower(blPower);
br.setPower(brPower);
this.move(MotorPowers.calculate(powerX, powerY, turn, speed));
}
public void stop() {
fl.setPower(0);
fr.setPower(0);
bl.setPower(0);
br.setPower(0);
hardware.fl.setPower(0);
hardware.fr.setPower(0);
hardware.bl.setPower(0);
hardware.br.setPower(0);
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Strings.java Normal file
View file

@ -0,0 +1,9 @@
package org.firstinspires.ftc.teamcode;
public class Strings {
public static final String dualGamepadOpModeName = "BBBBBBBBB";
public static final String monoGamepadOpModeName = "bbbbbbbbb";
public static final String mainOpModeName = Strings.dualGamepadOpModeName;
public static final String leftSideGroup = "Storming the castle (left side)";
public static final String rightSideGroup = "Face the forest (right side)";
}

210
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java Normal file
View file

@ -0,0 +1,210 @@
package org.firstinspires.ftc.teamcode.auto;
import android.annotation.SuppressLint;
import com.acmerobotics.dashboard.FtcDashboard;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.config.RuntimeConfig;
import org.openftc.apriltag.AprilTagDetection;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
import java.util.ArrayList;
public abstract class AprilTagAutonBase extends LinearOpMode {
OpenCvCamera camera;
AprilTagDetectionPipeline aprilTagDetectionPipeline;
// Lens intrinsics
// UNITS ARE PIXELS
// NOTE: this calibration is for the C920 webcam at 800x448.
// You will need to do your own calibration for other configurations!
double fx = 578.272;
double fy = 578.272;
double cx = 402.145;
double cy = 221.506;
// UNITS ARE METERS
double tagSize = 0.0381; // 1.5 in
// All from the 36h11 family
int[][] ID_TAGS_OF_INTEREST = {
{
584,
449,
275,
},
{
125,
345,
392,
},
{
87,
93,
554
},
{
1,
2,
3
}
};
int VARIANT_COUNT = ID_TAGS_OF_INTEREST.length;
AprilTagDetection[][] tagsOfInterest = new AprilTagDetection[VARIANT_COUNT][3];
abstract void onInit();
abstract void onPlay(int signalZone);
@Override
public void runOpMode() {
if (RuntimeConfig.SIGNAL_SLEEVE_VARIANT == null) {
double endTime = getRuntime() + 5;
while (!isStarted() && getRuntime() < endTime) {
telemetry.addLine("Signal sleeve variant not set!");
telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
telemetry.addLine("Here is a flashing block to get your attention:");
telemetry.addLine("####################################");
telemetry.addLine("####################################");
telemetry.addLine("####################################");
telemetry.update();
telemetry.addLine("Signal sleeve variant not set!");
telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
telemetry.addLine("Here is a flashing block to get your attention:");
telemetry.addLine("");
telemetry.addLine("");
telemetry.addLine("");
telemetry.update();
}
return;
}
onInit();
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagSize, fx, fy, cx, cy);
camera.setPipeline(aprilTagDetectionPipeline);
camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened() {
camera.startStreaming(800,448, OpenCvCameraRotation.SIDEWAYS_RIGHT);
FtcDashboard dashboard = FtcDashboard.getInstance();
dashboard.startCameraStream(camera, 0);
}
@Override
public void onError(int errorCode) {}
});
telemetry.setMsTransmissionInterval(50);
Integer[] mostRecentlySeenPerVariant = new Integer[VARIANT_COUNT];
/*
* The INIT-loop:
* This REPLACES waitForStart!
*/
while (!isStarted() && !isStopRequested()) {
telemetry.addLine("Chosen variant: " + RuntimeConfig.signalSleeveVariantName());
ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
ArrayList<Integer>[] tagsFound = new ArrayList[VARIANT_COUNT];
// fill the array, cause i dont know how to do this better
for (int i = 0; i < tagsFound.length; i++) tagsFound[i] = new ArrayList<>();
for (AprilTagDetection tag : currentDetections) {
for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
for (int id_index = 0; id_index < 3; id_index++) {
if (tag.id == ID_TAGS_OF_INTEREST[variant_index][id_index]) {
tagsFound[variant_index].add(id_index);
tagsOfInterest[variant_index][id_index] = tag;
}
}
}
}
for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
if (tagsFound[variant_index].size() == 1) {
mostRecentlySeenPerVariant[variant_index] = tagsFound[variant_index].get(0);
}
}
if (tagsFound[RuntimeConfig.SIGNAL_SLEEVE_VARIANT].size() == 1) {
int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
telemetry.addLine("Tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " is in sight!\n\nLocation data:");
tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
} else {
telemetry.addLine("Don't currently see any tags from variant " + RuntimeConfig.signalSleeveVariantName() + " :(");
if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
telemetry.addLine("(No tags have ever been seen)");
} else {
int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
telemetry.addLine("\nBut we HAVE seen tags before; tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " last seen at:");
tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
}
}
telemetry.update();
sleep(20);
}
/*
* The START command just came in: now work off the latest snapshot acquired
* during the init loop.
*/
int signalZone;
if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
telemetry.addLine("Uh oh, we never saw variant " + RuntimeConfig.signalSleeveVariantName() + "! Checking other variants...");
Integer id = null;
Integer variant = null;
for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
if (mostRecentlySeenPerVariant[variant_index] != null) {
id = mostRecentlySeenPerVariant[variant_index];
variant = variant_index;
}
}
if (id != null) {
telemetry.addLine("We saw tag " + RuntimeConfig.signalSleeveVariantName(variant) + "-" + (id + 1) + " at some point, using that.");
signalZone = id;
} else {
telemetry.addLine("Nope, nothing, nada, defaulting to signal zone 1.");
signalZone = 1;
}
telemetry.update();
} else {
signalZone = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
}
this.onPlay(signalZone + 1);
}
@SuppressLint("DefaultLocale")
void tagToTelemetry(AprilTagDetection detection) {
telemetry.addLine(String.format("\nDetected tag ID=%d", detection.id));
telemetry.addLine(String.format("Translation X: %.2f metres", detection.pose.x));
telemetry.addLine(String.format("Translation Y: %.2f metres", detection.pose.y));
telemetry.addLine(String.format("Translation Z: %.2f metres", detection.pose.z));
telemetry.addLine(String.format("Rotation Yaw: %.2f degrees", Math.toDegrees(detection.pose.yaw)));
telemetry.addLine(String.format("Rotation Pitch: %.2f degrees", Math.toDegrees(detection.pose.pitch)));
telemetry.addLine(String.format("Rotation Roll: %.2f degrees", Math.toDegrees(detection.pose.roll)));
}
}

312
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagDetectionPipeline.java Normal file
View file

@ -0,0 +1,312 @@
/*
* Copyright (c) 2021 OpenFTC Team
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.firstinspires.ftc.teamcode.auto;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.openftc.apriltag.AprilTagDetection;
import org.openftc.apriltag.AprilTagDetectorJNI;
import org.openftc.easyopencv.OpenCvPipeline;
import java.util.ArrayList;
class AprilTagDetectionPipeline extends OpenCvPipeline
{
private long nativeApriltagPtr;
private Mat grey = new Mat();
private ArrayList<AprilTagDetection> detections = new ArrayList<>();
private ArrayList<AprilTagDetection> detectionsUpdate = new ArrayList<>();
private final Object detectionsUpdateSync = new Object();
Mat cameraMatrix;
Scalar blue = new Scalar(7,197,235,255);
Scalar red = new Scalar(255,0,0,255);
Scalar green = new Scalar(0,255,0,255);
Scalar white = new Scalar(255,255,255,255);
double fx;
double fy;
double cx;
double cy;
// UNITS ARE METERS
double tagsize;
double tagsizeX;
double tagsizeY;
private float decimation;
private boolean needToSetDecimation;
private final Object decimationSync = new Object();
public AprilTagDetectionPipeline(double tagsize, double fx, double fy, double cx, double cy)
{
this.tagsize = tagsize;
this.tagsizeX = tagsize;
this.tagsizeY = tagsize;
this.fx = fx;
this.fy = fy;
this.cx = cx;
this.cy = cy;
constructMatrix();
// Allocate a native context object. See the corresponding deletion in the finalizer
nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3);
}
@Override
public void finalize()
{
// Might be null if createApriltagDetector() threw an exception
if(nativeApriltagPtr != 0)
{
// Delete the native context we created in the constructor
AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr);
nativeApriltagPtr = 0;
}
else
{
System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL");
}
}
@Override
public Mat processFrame(Mat input)
{
// Convert to greyscale
Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
synchronized (decimationSync)
{
if(needToSetDecimation)
{
AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);
needToSetDecimation = false;
}
}
// Run AprilTag
detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);
synchronized (detectionsUpdateSync)
{
detectionsUpdate = detections;
}
// For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using
// OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV.
for(AprilTagDetection detection : detections)
{
Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
}
return input;
}
public void setDecimation(float decimation)
{
synchronized (decimationSync)
{
this.decimation = decimation;
needToSetDecimation = true;
}
}
public ArrayList<AprilTagDetection> getLatestDetections()
{
return detections;
}
public ArrayList<AprilTagDetection> getDetectionsUpdate()
{
synchronized (detectionsUpdateSync)
{
ArrayList<AprilTagDetection> ret = detectionsUpdate;
detectionsUpdate = null;
return ret;
}
}
void constructMatrix()
{
// Construct the camera matrix.
//
// -- --
// | fx 0 cx |
// | 0 fy cy |
// | 0 0 1 |
// -- --
//
cameraMatrix = new Mat(3,3, CvType.CV_32FC1);
cameraMatrix.put(0,0, fx);
cameraMatrix.put(0,1,0);
cameraMatrix.put(0,2, cx);
cameraMatrix.put(1,0,0);
cameraMatrix.put(1,1,fy);
cameraMatrix.put(1,2,cy);
cameraMatrix.put(2, 0, 0);
cameraMatrix.put(2,1,0);
cameraMatrix.put(2,2,1);
}
/**
* Draw a 3D axis marker on a detection. (Similar to what Vuforia does)
*
* @param buf the RGB buffer on which to draw the marker
* @param length the length of each of the marker 'poles'
* @param rvec the rotation vector of the detection
* @param tvec the translation vector of the detection
* @param cameraMatrix the camera matrix used when finding the detection
*/
void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
{
// The points in 3D space we wish to project onto the 2D image plane.
// The origin of the coordinate space is assumed to be in the center of the detection.
MatOfPoint3f axis = new MatOfPoint3f(
new Point3(0,0,0),
new Point3(length,0,0),
new Point3(0,length,0),
new Point3(0,0,-length)
);
// Project those points
MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
Point[] projectedPoints = matProjectedPoints.toArray();
// Draw the marker!
Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);
Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);
Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);
Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);
}
void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
{
//axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
// [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
// The points in 3D space we wish to project onto the 2D image plane.
// The origin of the coordinate space is assumed to be in the center of the detection.
MatOfPoint3f axis = new MatOfPoint3f(
new Point3(-tagWidth/2, tagHeight/2,0),
new Point3( tagWidth/2, tagHeight/2,0),
new Point3( tagWidth/2,-tagHeight/2,0),
new Point3(-tagWidth/2,-tagHeight/2,0),
new Point3(-tagWidth/2, tagHeight/2,-length),
new Point3( tagWidth/2, tagHeight/2,-length),
new Point3( tagWidth/2,-tagHeight/2,-length),
new Point3(-tagWidth/2,-tagHeight/2,-length));
// Project those points
MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
Point[] projectedPoints = matProjectedPoints.toArray();
// Pillars
for(int i = 0; i < 4; i++)
{
Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);
}
// Base lines
//Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);
//Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);
//Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);
//Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);
// Top lines
Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);
Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);
Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);
Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
}
/**
* Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the
* original size of the tag.
*
* @param points the points which form the trapezoid
* @param cameraMatrix the camera intrinsics matrix
* @param tagsizeX the original width of the tag
* @param tagsizeY the original height of the tag
* @return the 6DOF pose of the camera relative to the tag
*/
Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)
{
// The actual 2d points of the tag detected in the image
MatOfPoint2f points2d = new MatOfPoint2f(points);
// The 3d points of the tag in an 'ideal projection'
Point3[] arrayPoints3d = new Point3[4];
arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);
arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);
arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);
arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);
MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);
// Using this information, actually solve for pose
Pose pose = new Pose();
Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);
return pose;
}
/*
* A simple container to hold both rotation and translation
* vectors, which together form a 6DOF pose.
*/
class Pose
{
Mat rvec;
Mat tvec;
public Pose()
{
rvec = new Mat();
tvec = new Mat();
}
public Pose(Mat rvec, Mat tvec)
{
this.rvec = rvec;
this.tvec = tvec;
}
}
}

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonLeft.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonLeft.java
View file

@ -1,9 +1,13 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "idk something left")
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MovementAPI;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "idk something left side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class AutonLeft extends LinearOpMode {
Hardware hardware;
MovementAPI movement;
@ -15,7 +19,7 @@ public class AutonLeft extends LinearOpMode {
hardware.wrist.setPosition(0.5);
sleep(500);
hardware.claw.setPosition(1);
hardware.setClawPosition(1);
waitForStart();
@ -23,13 +27,13 @@ public class AutonLeft extends LinearOpMode {
sleep(1000);
movement.stop();
sleep(1000);
hardware.claw.setPosition(0);
hardware.setClawPosition(0);
sleep(1000);
movement.move(0, -1, 0, 0.4);
sleep(1000);
movement.stop();
sleep(1000);
movement.move(-1, 0, 0, 0.4);
movement.move(1, 0, 0, 0.4);
sleep(2500);
movement.stop();
}

165
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonMovement.java Normal file
View file

@ -0,0 +1,165 @@
package org.firstinspires.ftc.teamcode.auto;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.MovementAPI;
public class AutonMovement {
Hardware hardware;
MovementAPI movement;
Telemetry telemetry;
double speed;
public final double ENCODER_TICKS_PER_MM = 537.7 / (96 * Math.PI);
public AutonMovement(Hardware hardware, double speed) {
this.hardware = hardware;
this.movement = new MovementAPI(hardware);
this.speed = speed;
this.telemetry = hardware.opMode.telemetry;
}
private void sleep(double seconds) {
double startTime = System.nanoTime();
double targetTime = startTime + seconds * 1000 * 1000 * 1000;
while (System.nanoTime() < targetTime);
}
private void movementHelper(Distance distance, boolean strafe) {
int flStart = hardware.fl.getCurrentPosition();
int frStart = hardware.fr.getCurrentPosition();
int targetDisplacement = (int) (distance.valInMM() * ENCODER_TICKS_PER_MM);
if (strafe) targetDisplacement *= (60/52.);
int loopsCorrect = 0;
while (true) {
int flDisplacement = hardware.fl.getCurrentPosition() - flStart;
int frDisplacement = hardware.fr.getCurrentPosition() - frStart;
if (strafe) frDisplacement *= -1;
int totalDisplacement = (flDisplacement + frDisplacement) / 2;
int displacementDiff = targetDisplacement - totalDisplacement;
if (Math.abs(displacementDiff) < 10) {
loopsCorrect += 1;
} else {
loopsCorrect = 0;
}
if (loopsCorrect >= 80) {
movement.stop();
break;
}
double pow;
if (Math.abs(displacementDiff) > 300) {
pow = 1;
} else {
pow = Math.sqrt(1 - Math.pow((Math.abs(displacementDiff) / 300.) - 1, 2));
}
telemetry.addLine("pow = " + pow);
telemetry.addLine("loopsCorrect = " + loopsCorrect);
telemetry.update();
if (strafe) {
movement.move(Math.signum(displacementDiff) * pow, 0, 0, speed);
} else {
movement.move(0, Math.signum(displacementDiff) * pow, 0, speed);
}
}
}
public void straight(Distance distance) {
this.movementHelper(distance, false);
}
public void strafe(Distance distance) {
this.movementHelper(distance, true);
}
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;
int loopsCorrect = 0;
while (true) {
double yaw = hardware.getImuYaw();
double yawDiff = modAngle(targetYaw - yaw);
if (Math.abs(yawDiff) < 1) {
loopsCorrect += 1;
} else {
loopsCorrect = 0;
}
if (loopsCorrect >= 100) {
movement.stop();
break;
}
double pow;
if (Math.abs(yawDiff) > 60) {
pow = 1;
} else {
pow = Math.sqrt(1 - Math.pow((Math.abs(yawDiff) / 60.) - 1, 2));
}
movement.move(0, 0, Math.signum(yawDiff) * pow, speed);
}
}
public void liftTo(LiftPosition height) {
int loopsCorrect = 0;
while (true) {
int spoolPosition = hardware.getLiftEncoder();
int spoolPosDiff = height.getEncoderVal() - spoolPosition;
if (Math.abs(spoolPosDiff) < 10) {
loopsCorrect += 1;
} else {
loopsCorrect = 0;
}
if (loopsCorrect >= 100) {
hardware.spool.setPower(0);
break;
}
double pow;
if (Math.abs(spoolPosDiff) > 300) {
pow = 1;
} else {
pow = Math.sqrt(1 - Math.pow((Math.abs(spoolPosDiff) / 400.) - 1, 2));
}
telemetry.addLine("spool pos = " + spoolPosition);
telemetry.addLine("pow = " + pow);
telemetry.addLine("loopsCorrect = " + loopsCorrect);
telemetry.update();
hardware.spool.setPower(Math.signum(spoolPosDiff) * pow);
}
}
}

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonRight.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AutonRight.java
View file

@ -1,9 +1,13 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "idk something right")
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MovementAPI;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "idk something right side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class AutonRight extends LinearOpMode {
Hardware hardware;
MovementAPI movement;
@ -15,7 +19,7 @@ public class AutonRight extends LinearOpMode {
hardware.wrist.setPosition(0.5);
sleep(500);
hardware.claw.setPosition(1);
hardware.setClawPosition(1);
waitForStart();
@ -23,13 +27,13 @@ public class AutonRight extends LinearOpMode {
sleep(1000);
movement.stop();
sleep(1000);
hardware.claw.setPosition(0);
hardware.setClawPosition(0);
sleep(1000);
movement.move(0, -1, 0, 0.4);
sleep(1000);
movement.stop();
sleep(1000);
movement.move(1, 0, 0, 0.4);
movement.move(-1, 0, 0, 0.4);
sleep(2500);
movement.stop();
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/MultiConeAutonLeft.java Normal file
View file

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "Multi-Cone Left Side", group = "don't use me", preselectTeleOp = Strings.mainOpModeName)
public class MultiConeAutonLeft extends AprilTagAutonBase {
Hardware hardware;
AutonMovement movement;
@Override
void onInit() {
hardware = new Hardware(this);
movement = new AutonMovement(hardware, 0.6);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
}
@Override
void onPlay(int signalZone) {
// move to the junction
movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
movement.straight(Distance.inTiles(1.5));
movement.turn(90);
// lift and move forward
movement.liftTo(LiftPosition.HIGH);
movement.straight(Distance.inInches(4));
// wrist down, drop, wrist up
hardware.setWristPosition(0);
sleep(500);
hardware.setClawPosition(0);
sleep(500);
hardware.setClawPosition(1);
// move back and lower
movement.straight(Distance.inInches(-4));
movement.liftTo(LiftPosition.STACK);
movement.turn(180);
movement.strafe(Distance.inTiles(0.5));
hardware.setClawPosition(0);
movement.straight(Distance.inTiles(2).add(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
hardware.setClawPosition(1);
movement.liftTo(LiftPosition.HIGH);
hardware.setWristPosition(-2/3.);
movement.straight(Distance.inTiles(-1.5).sub(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
movement.turn(90);
// lift and move forward
movement.liftTo(LiftPosition.HIGH);
movement.straight(Distance.inInches(4));
// wrist down, drop, wrist up
hardware.setWristPosition(0);
sleep(500);
hardware.setClawPosition(0);
sleep(500);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
// move back and lower
movement.straight(Distance.inInches(-4));
movement.liftTo(LiftPosition.FLOOR);
// switch (signalZone) {
// case 1:
// movement.strafe(Distance.inTiles(-2.5));
// break;
// case 2:
// movement.strafe(Distance.inTiles(-1.5));
// break;
// case 3:
// movement.strafe(Distance.inTiles(-0.5));
// break;
// }
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonLeft.java Normal file
View file

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "1-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class OneConeAutonLeft extends AprilTagAutonBase {
Hardware hardware;
AutonMovement movement;
@Override
void onInit() {
hardware = new Hardware(this);
movement = new AutonMovement(hardware, 0.6);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
}
@Override
void onPlay(int signalZone) {
// move to the junction
movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
movement.straight(Distance.inTiles(1));
movement.strafe(Distance.inTiles(0.5));
// lift and move forward
movement.liftTo(LiftPosition.HIGH);
movement.straight(Distance.inInches(2));
// wrist down, drop, wrist up
hardware.setWristPosition(0);
sleep(500);
hardware.setClawPosition(0);
sleep(500);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
// move back and lower
movement.straight(Distance.inInches(-2));
movement.liftTo(LiftPosition.FLOOR);
switch (signalZone) {
case 1:
movement.strafe(Distance.inTiles(-2.5));
break;
case 2:
movement.strafe(Distance.inTiles(-1.5));
break;
case 3:
movement.strafe(Distance.inTiles(-0.5));
break;
}
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/OneConeAutonRight.java Normal file
View file

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "1-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class OneConeAutonRight extends AprilTagAutonBase {
Hardware hardware;
AutonMovement movement;
@Override
void onInit() {
hardware = new Hardware(this);
movement = new AutonMovement(hardware, 0.6);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
}
@Override
void onPlay(int signalZone) {
// move to the junction
movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
movement.straight(Distance.inTiles(1));
movement.strafe(Distance.inTiles(-0.5));
// lift and move forward
movement.liftTo(LiftPosition.HIGH);
movement.straight(Distance.inInches(2));
// wrist down, drop, wrist up
hardware.setWristPosition(0);
sleep(500);
hardware.setClawPosition(0);
sleep(500);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
// move back and lower
movement.straight(Distance.inInches(-2));
movement.liftTo(LiftPosition.FLOOR);
switch (signalZone) {
case 1:
movement.strafe(Distance.inTiles(0.5));
break;
case 2:
movement.strafe(Distance.inTiles(1.5));
break;
case 3:
movement.strafe(Distance.inTiles(2.5));
break;
}
}
}

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ResetEncoders.java Normal file
View file

@ -0,0 +1,21 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.Hardware;
@Autonomous(name = "Reset encoders + imu yaw")
public class ResetEncoders extends LinearOpMode {
@Override
public void runOpMode() {
Hardware hardware = new Hardware(this);
hardware.fl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hardware.fr.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hardware.bl.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hardware.br.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hardware.spool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hardware.imu.resetYaw();
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java Normal file
View file

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.kinematics.NullablePose;
import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
import org.firstinspires.ftc.teamcode.kinematics.Poser;
@Autonomous(name = "test")
public class Test extends LinearOpMode {
Hardware hardware;
Poser poser;
@Override
public void runOpMode() {
hardware = new Hardware(this);
hardware.setWristPosition(-0.6);
poser = new Poser(hardware, 0.3, FieldVector.ZERO, 0);
poser.goToNullablePose(new NullablePose());
waitForStart();
while (!isStopRequested()) {
poser.goToNullablePose(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR));
sleep(1000);
poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH));
sleep(1000);
poser.goToNullablePose(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM));
sleep(1000);
}
}
}

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonLeft.java Normal file
View file

@ -0,0 +1,41 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "0-Cone Left Side", group = Strings.leftSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class ZeroConeAutonLeft extends AprilTagAutonBase {
Hardware hardware;
AutonMovement movement;
@Override
void onInit() {
hardware = new Hardware(this);
movement = new AutonMovement(hardware, 0.6);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
}
@Override
void onPlay(int signalZone) {
// move to the junction
movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
movement.strafe(Distance.inTiles(1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
movement.straight(Distance.inTiles(1));
switch (signalZone) {
case 1:
movement.strafe(Distance.inTiles(-2));
break;
case 2:
movement.strafe(Distance.inTiles(-1));
break;
case 3:
// do nothing
break;
}
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/ZeroConeAutonRight.java Normal file
View file

@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.auto;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.kinematics.Distance;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.Strings;
@Autonomous(name = "0-Cone Right Side", group = Strings.rightSideGroup, preselectTeleOp = Strings.mainOpModeName)
public class ZeroConeAutonRight extends AprilTagAutonBase {
Hardware hardware;
AutonMovement movement;
@Override
void onInit() {
hardware = new Hardware(this);
movement = new AutonMovement(hardware, 0.6);
hardware.setClawPosition(1);
hardware.setWristPosition(-2/3.);
}
@Override
void onPlay(int signalZone) {
movement.straight(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2));
movement.strafe(Distance.inTiles(-1).sub(Distance.ONE_TILE_WITHOUT_BORDER.sub(Distance.ROBOT_WIDTH).div(2)));
movement.straight(Distance.inTiles(1));
switch (signalZone) {
case 1:
// do nothing
break;
case 2:
movement.strafe(Distance.inTiles(1));
break;
case 3:
movement.strafe(Distance.inTiles(2));
break;
}
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeve123.java Normal file
View file

@ -0,0 +1,12 @@
package org.firstinspires.ftc.teamcode.config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "Signal Sleeve With Tags 1, 2, 3", group = "Config")
public class ConfigSleeve123 extends LinearOpMode {
@Override
public void runOpMode() {
RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 3;
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantA.java Normal file
View file

@ -0,0 +1,12 @@
package org.firstinspires.ftc.teamcode.config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "Signal Sleeve Variant A", group = "Config")
public class ConfigSleeveVariantA extends LinearOpMode {
@Override
public void runOpMode() {
RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 0;
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantB.java Normal file
View file

@ -0,0 +1,12 @@
package org.firstinspires.ftc.teamcode.config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "Signal Sleeve Variant B", group = "Config")
public class ConfigSleeveVariantB extends LinearOpMode {
@Override
public void runOpMode() {
RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 1;
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/ConfigSleeveVariantC.java Normal file
View file

@ -0,0 +1,12 @@
package org.firstinspires.ftc.teamcode.config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Autonomous(name = "Signal Sleeve Variant C", group = "Config")
public class ConfigSleeveVariantC extends LinearOpMode {
@Override
public void runOpMode() {
RuntimeConfig.SIGNAL_SLEEVE_VARIANT = 2;
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/RuntimeConfig.java Normal file
View file

@ -0,0 +1,24 @@
package org.firstinspires.ftc.teamcode.config;
public class RuntimeConfig {
public static Integer SIGNAL_SLEEVE_VARIANT = null;
public static String signalSleeveVariantName() {
return RuntimeConfig.signalSleeveVariantName(SIGNAL_SLEEVE_VARIANT);
}
public static String signalSleeveVariantName(int variant) {
switch (variant) {
case 0:
return "A";
case 1:
return "B";
case 2:
return "C";
case 3:
return "123";
default:
return SIGNAL_SLEEVE_VARIANT + "";
}
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Distance.java Normal file
View file

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.kinematics;
public class Distance {
private final double val; // in millimeters
public static final double IN_PER_TILE = 23.625;
public static final double MM_PER_IN = 25.4;
public static final double MM_PER_FOOT = 304.8; // 25.4 * 12 but without floating point problems
public static final Distance ZERO = new Distance(0);
public static final Distance ROBOT_LENGTH = Distance.inInches(18);
public static final Distance ROBOT_WIDTH = Distance.inInches(15.5);
public static final Distance TILE_BORDER = Distance.inInches(0.75);
public static final Distance ONE_TILE_WITHOUT_BORDER = Distance.inTiles(1).sub(Distance.TILE_BORDER);
public static final Distance ONE_TILE_WITH_BORDER = Distance.inTiles(1).add(Distance.TILE_BORDER);
public static Distance inDefaultUnits(double val) {
return new Distance(val);
}
public static Distance inTiles(double val) {
return Distance.inInches(val * IN_PER_TILE);
}
public static Distance inFeet(double val) {
return Distance.inMM(val * MM_PER_FOOT);
}
public static Distance inInches(double val) {
return Distance.inMM(val * MM_PER_IN);
}
public static Distance inMM(double val) {
return new Distance(val);
}
public double valInDefaultUnits() {
return this.val;
}
public double valInTiles() {
return this.valInInches() / IN_PER_TILE;
}
public double valInFeet() {
return this.valInMM() / MM_PER_FOOT;
}
public double valInInches() {
return this.valInMM() / MM_PER_IN;
}
public double valInMM() {
return this.val;
}
private Distance(double val) {
this.val = val;
}
public String toString() {
return Double.toString(this.val);
}
public boolean isZero() {
return this.val == 0;
}
public Distance add(Distance rhs) {
return new Distance(this.val + rhs.val);
}
public Distance sub(Distance rhs) {
return new Distance(this.val - rhs.val);
}
public Distance mul(double rhs) {
return new Distance(this.val * rhs);
}
public Distance div(double rhs) {
return new Distance(this.val / rhs);
}
public Distance neg() {
return new Distance(-this.val);
}
}

117
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java Normal file
View file

@ -0,0 +1,117 @@
package org.firstinspires.ftc.teamcode.kinematics;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.Hardware;
public class EncoderIntegrator {
Hardware hardware;
FieldVector pos;
// `yaw` is the robot angle. `imuYaw` is the last measured value from the imu. usually just
// offset by a constant but that could be different in the future, i.e. radians or smth.
double yaw;
int fl;
int fr;
int bl;
int br;
double imuYaw;
Telemetry telemetry;
public final double MM_PER_ENCODER_TICK = (96 * Math.PI) / 537.7;
public final double WHEELBASE_LENGTH = 337;
public final double WHEELBASE_WIDTH = 355;
public final double WHEELBASE_DIAGONAL = Math.hypot(WHEELBASE_LENGTH, WHEELBASE_WIDTH );
public final double DEGREES_PER_MM = 360 / (WHEELBASE_DIAGONAL * Math.PI);
public EncoderIntegrator(Hardware hardware, FieldVector initialPos, double initialYaw, Telemetry telemetry) {
this.hardware = hardware;
this.pos = initialPos;
this.yaw = initialYaw;
this.telemetry = telemetry;
this.fl = hardware.fl.getCurrentPosition();
this.fr = hardware.fr.getCurrentPosition();
this.bl = hardware.bl.getCurrentPosition();
this.br = hardware.br.getCurrentPosition();
this.imuYaw = hardware.getImuYaw();
}
private static double sinc(double x) {
if (x == 0) {
return 1;
} else {
return Math.sin(x) / x;
}
}
// this function has no generally well-known name, but its similar to `sinc` so im calling it `cosc`
private static double cosc(double x) {
if (x == 0) {
return 0;
} else {
return (1 - Math.cos(x)) / x;
}
}
public void update() {
int newFl = hardware.fl.getCurrentPosition();
int newFr = hardware.fr.getCurrentPosition();
int newBl = hardware.bl.getCurrentPosition();
int newBr = hardware.br.getCurrentPosition();
double newImuYaw = hardware.getImuYaw();
double flDiff = newFl - this.fl;
double frDiff = newFr - this.fr;
double blDiff = newBl - this.bl;
double brDiff = newBr - this.br;
double imuYawDiff = newImuYaw - this.imuYaw;
// telemetry.addData("flDiff", flDiff);
// telemetry.addData("frDiff", frDiff);
// telemetry.addData("blDiff", blDiff);
// telemetry.addData("brDiff", brDiff);
// telemetry.addData("imuYawDiff", imuYawDiff);
// fl = powerY + powerX + turn + noop
// fr = powerY - powerX - turn + noop
// bl = powerY - powerX + turn - noop
// br = powerY + powerX - turn - noop
double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;
// aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
// meaningful we can do with these values to help correct the other values. the noop value
// is ignored for obvious reasons. the turn value is ignored since we get the turn from the
// imu and using encoders for turn has proven to work poorly.
// telemetry.addData("relativeYDiff", relativeYDiff);
// telemetry.addData("relativeXDiff", relativeXDiff);
double imuYawDiffRadians = Math.toRadians(imuYawDiff);
double poseExponentiationX = cosc(imuYawDiffRadians);
double poseExponentiationY = sinc(imuYawDiffRadians);
FieldVector relativePosDiff = FieldVector.inMM(
relativeYDiff * poseExponentiationX + relativeXDiff * poseExponentiationY,
relativeYDiff * poseExponentiationY - relativeXDiff * poseExponentiationX
);
FieldVector posDiff = relativePosDiff.rot(this.imuYaw);
double yawDiff = imuYawDiff;
this.pos = this.pos.add(posDiff);
this.yaw += yawDiff;
this.fl = newFl;
this.fr = newFr;
this.bl = newBl;
this.br = newBr;
this.imuYaw = newImuYaw;
this.imuYaw %= 360;
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java Normal file
View file

@ -0,0 +1,79 @@
package org.firstinspires.ftc.teamcode.kinematics;
public class FieldVector {
public final Distance x;
public final Distance y;
public static final FieldVector ZERO = new FieldVector(Distance.ZERO, Distance.ZERO);
// public static final FieldVector RED_LEFT_SIDE_START = FieldVector.inTiles(-1.5, -2.5);
public static FieldVector inTiles(double x, double y) {
return new FieldVector(Distance.inTiles(x), Distance.inTiles(y));
}
public static FieldVector inFeet(double x, double y) {
return new FieldVector(Distance.inFeet(x), Distance.inFeet(y));
}
public static FieldVector inMM(double x, double y) {
return new FieldVector(Distance.inMM(x), Distance.inMM(y));
}
public FieldVector(Distance x, Distance y) {
this.x = x;
this.y = y;
}
public String toString() {
return "(" + this.x + ", " + this.y + ")";
}
public FieldVector add(FieldVector rhs) {
return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
}
public FieldVector sub(FieldVector rhs) {
return new FieldVector(this.x.sub(rhs.x), this.y.sub(rhs.y));
}
public FieldVector mul(double rhs) {
return new FieldVector(this.x.mul(rhs), this.y.mul(rhs));
}
public FieldVector div(double rhs) {
return new FieldVector(this.x.div(rhs), this.y.div(rhs));
}
public FieldVector rot(double rhs) {
double sin = Math.sin(Math.toRadians(rhs));
double cos = Math.cos(Math.toRadians(rhs));
return new FieldVector(this.y.mul(sin).add(this.x.mul(cos)), this.y.mul(cos).sub(this.x.mul(sin)));
}
public FieldVector neg() {
return new FieldVector(this.x.neg(), this.y.neg());
}
public Distance magnitude() {
return Distance.inDefaultUnits(Math.sqrt(this.sqMagnitude().valInDefaultUnits()));
}
public Distance sqMagnitude() {
return Distance.inDefaultUnits(
this.x.valInDefaultUnits() * this.x.valInDefaultUnits()
+ this.y.valInDefaultUnits() * this.y.valInDefaultUnits()
);
}
public FieldVector normalized() {
if (this.magnitude().isZero()) {
return FieldVector.ZERO;
} else {
return this.div(this.magnitude().valInDefaultUnits());
}
}
public double angle() {
return 90 - Math.toDegrees(Math.atan2(this.y.valInDefaultUnits(), this.x.valInDefaultUnits()));
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java Normal file
View file

@ -0,0 +1,57 @@
package org.firstinspires.ftc.teamcode.kinematics;
import com.acmerobotics.dashboard.config.Config;
@Config
public enum LiftPosition {
FLOOR, STACK, GROUND, LOW, MEDIUM, HIGH;
public static int FLOOR_ENCODER_VAL = 10;
public static int STACK_ENCODER_VAL = 600;
public static int GROUND_ENCODER_VAL = 300;
public static int LOW_ENCODER_VAL = 1000;
public static int MEDIUM_ENCODER_VAL = 2000;
public static int HIGH_ENCODER_VAL = 2900;
LiftPosition() {
}
public int getEncoderVal() {
switch (this) {
case FLOOR:
return FLOOR_ENCODER_VAL;
case STACK:
return STACK_ENCODER_VAL;
case GROUND:
return GROUND_ENCODER_VAL;
case LOW:
return LOW_ENCODER_VAL;
case MEDIUM:
return MEDIUM_ENCODER_VAL;
case HIGH:
return HIGH_ENCODER_VAL;
default:
return 0;
}
}
public String toString() {
switch (this) {
case FLOOR:
return "LiftPosition.FLOOR";
case STACK:
return "LiftPosition.STACK";
case GROUND:
return "LiftPosition.GROUND";
case LOW:
return "LiftPosition.LOW";
case MEDIUM:
return "LiftPosition.MEDIUM";
case HIGH:
return "LiftPosition.HIGH";
default:
return "uh... we dont know?";
}
}
}

96
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/NullablePose.java Normal file
View file

@ -0,0 +1,96 @@
package org.firstinspires.ftc.teamcode.kinematics;
import androidx.annotation.Nullable;
public class NullablePose {
@Nullable
public final FieldVector pos;
@Nullable
public final Double yaw;
@Nullable
public final Integer liftPos;
public NullablePose(FieldVector pos, double yaw, int liftPos) {
this.pos = pos;
this.yaw = yaw;
this.liftPos = liftPos;
}
public NullablePose(FieldVector pos, double yaw, LiftPosition liftPos) {
this.pos = pos;
this.yaw = yaw;
this.liftPos = liftPos.getEncoderVal();
}
public NullablePose(FieldVector pos, double yaw) {
this.pos = pos;
this.yaw = yaw;
this.liftPos = null;
}
public NullablePose(double yaw, int liftPos) {
this.pos = null;
this.yaw = yaw;
this.liftPos = liftPos;
}
public NullablePose(double yaw, LiftPosition liftPos) {
this.pos = null;
this.yaw = yaw;
this.liftPos = liftPos.getEncoderVal();
}
public NullablePose(FieldVector pos, int liftPos) {
this.pos = pos;
this.yaw = null;
this.liftPos = liftPos;
}
public NullablePose(FieldVector pos, LiftPosition liftPos) {
this.pos = pos;
this.yaw = null;
this.liftPos = liftPos.getEncoderVal();
}
public NullablePose(FieldVector pos) {
this.pos = pos;
this.yaw = null;
this.liftPos = null;
}
public NullablePose(double yaw) {
this.pos = null;
this.yaw = yaw;
this.liftPos = null;
}
public NullablePose(int liftPos) {
this.pos = null;
this.yaw = null;
this.liftPos = liftPos;
}
public NullablePose(LiftPosition liftPos) {
this.pos = null;
this.yaw = null;
this.liftPos = liftPos.getEncoderVal();
}
public NullablePose() {
this.pos = null;
this.yaw = null;
this.liftPos = null;
}
public NullablePose or(NullablePose other) {
return new NullablePose(
this.pos != null ? this.pos : other.pos,
this.yaw != null ? this.yaw : other.yaw,
this.liftPos != null ? this.liftPos : other.liftPos
);
}
public NullablePose and(NullablePose other) {
return other.or(this);
}
}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Pose.java Normal file
View file

@ -0,0 +1,19 @@
package org.firstinspires.ftc.teamcode.kinematics;
public class Pose {
public final FieldVector pos;
public final double yaw;
public final int liftPos;
public final static Pose ZERO = new Pose(
FieldVector.ZERO,
0,
0
);
public Pose(FieldVector pos, double yaw, int liftPos) {
this.pos = pos;
this.yaw = yaw;
this.liftPos = liftPos;
}
}

118
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java Normal file
View file

@ -0,0 +1,118 @@
package org.firstinspires.ftc.teamcode.kinematics;
import com.acmerobotics.dashboard.config.Config;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MovementAPI;
@Config
public class Poser {
Hardware hardware;
MovementAPI movement;
Telemetry telemetry;
EncoderIntegrator integrator;
double speed;
public static double POS_STOP_THRESHOLD = 5;
public static double YAW_STOP_THRESHOLD = 0.5;
public static int LIFT_STOP_THRESHOLD = 7;
public static int SLOWDOWN_THRESHOLD_FACTOR = 40;
public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
this.hardware = hardware;
this.movement = new MovementAPI(hardware);
this.speed = speed;
this.telemetry = hardware.opMode.telemetry;
this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
}
private Pose getPose() {
return new Pose(
integrator.pos,
integrator.imuYaw,
hardware.getLiftEncoder()
);
}
public static double ellipticalCurve(double x, double slowdownThreshold) {
if (Math.abs(x) > slowdownThreshold) {
return Math.signum(x);
} else {
return Math.signum(x) * Math.sqrt(1 - Math.pow((Math.abs(x) / slowdownThreshold) - 1, 2));
}
}
public void goToNullablePose(NullablePose targetPose) {
Pose currentPose = this.getPose();
this.goToPose(new Pose(
targetPose.pos != null ? targetPose.pos : currentPose.pos,
targetPose.yaw != null ? targetPose.yaw : currentPose.yaw,
targetPose.liftPos != null ? targetPose.liftPos : currentPose.liftPos
));
}
public void goToPose(Pose targetPose) {
int loopsCorrect = 0;
while (true) {
this.integrator.update();
Pose currentPose = this.getPose();
FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
double yawDiff = targetPose.yaw - currentPose.yaw;
int liftDiff = targetPose.liftPos - currentPose.liftPos;
if (
Math.abs(posDiff.magnitude().valInMM()) < POS_STOP_THRESHOLD
&& Math.abs(yawDiff) < YAW_STOP_THRESHOLD
&& Math.abs(liftDiff) < LIFT_STOP_THRESHOLD
) {
loopsCorrect += 1;
} else {
loopsCorrect = 0;
}
if (loopsCorrect >= 120) {
movement.stop();
hardware.spool.setPower(0);
break;
}
telemetry.addData("posX", currentPose.pos.x);
telemetry.addData("posY", currentPose.pos.y);
telemetry.addData("posDiff", posDiff);
telemetry.addData("posXDiff", posDiff.x);
telemetry.addData("posYDiff", posDiff.y);
telemetry.addData("yawDiff", yawDiff);
telemetry.addData("liftDiff", liftDiff);
FieldVector posPower = posDiff.normalized().mul(ellipticalCurve(posDiff.magnitude().valInMM(), POS_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR));
double yawPower = ellipticalCurve(yawDiff, YAW_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
double liftPower = ellipticalCurve(liftDiff, LIFT_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR);
telemetry.addData("posPower", posPower);
telemetry.addData("posXPower", posPower.x);
telemetry.addData("posYPower", posPower.y);
telemetry.addData("yawPower", yawPower);
telemetry.addData("liftPower", liftPower);
telemetry.addData("loopsCorrect", loopsCorrect);
hardware.dashboardTelemetryWrapper.drawTargetOnDashboard(targetPose, currentPose);
hardware.dashboardTelemetryWrapper.drawRobot(currentPose.pos, currentPose.yaw);
telemetry.update();
posPower = posPower.rot(-currentPose.yaw);
movement.move(
posPower.x.valInDefaultUnits(),
posPower.y.valInDefaultUnits(),
yawPower,
speed
);
hardware.spool.setPower(liftPower);
}
}
}

115
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java Normal file
View file

@ -0,0 +1,115 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MotorPowers;
import org.firstinspires.ftc.teamcode.MovementAPI;
import org.firstinspires.ftc.teamcode.Strings;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.kinematics.Poser;
@TeleOp(name = Strings.dualGamepadOpModeName)
public class Gamepad extends OpMode {
Hardware hardware;
MovementAPI movement;
double speed = 0.6;
LiftPosition targetLiftPos = null;
@Override
public void init() {
hardware = new Hardware(this);
movement = new MovementAPI(hardware);
hardware.setClawPosition(1);
hardware.wrist.setPosition(0.5);
}
@Override
public void loop() {
if (gamepad1.left_bumper) {
speed -= 0.005;
}
if (gamepad1.right_bumper) {
speed += 0.005;
}
speed = Math.max(0, Math.min(speed, 1));
if (gamepad2.left_stick_y != 0 || gamepad2.left_stick_button) {
targetLiftPos = null;
} else if (gamepad2.a) {
targetLiftPos = LiftPosition.HIGH;
} else if (gamepad2.b) {
targetLiftPos = LiftPosition.FLOOR;
}
int spoolPos = hardware.getLiftEncoder();
double pow;
if (targetLiftPos != null) {
int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
} else {
pow = -gamepad2.left_stick_y;
}
if (gamepad2.y) {
telemetry.addLine("lift status: !!! override set !!!");
} else {
if (spoolPos < -50) {
telemetry.addLine("lift status: too far below min, moving up");
pow = Math.max(pow, 0.2);
} else if (spoolPos < 0) {
telemetry.addLine("lift status: below min, stopping downward motion");
pow = Math.max(pow, 0);
} else if (spoolPos < 100) {
telemetry.addLine("lift status: close to min, slowing downward motion");
pow = Math.max(pow, -0.5);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
telemetry.addLine("lift status: too far above max, moving down");
pow = Math.min(pow, -0.2);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
telemetry.addLine("lift status: above max, stopping upward motion");
pow = Math.min(pow, 0);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
telemetry.addLine("lift status: close to max, slowing upward motion");
pow = Math.min(pow, 0.5);
} else {
telemetry.addLine("lift status: ok");
}
}
telemetry.addLine("lift power: " + pow);
hardware.spool.setPower(pow);
hardware.setClawPosition(1 - gamepad2.left_trigger);
telemetry.addLine("wrist pos: " + gamepad2.right_stick_y);
hardware.setWristPosition(gamepad2.right_stick_y);
MotorPowers powers = MotorPowers.calculate(
gamepad1.left_stick_x,
-gamepad1.left_stick_y,
gamepad1.right_stick_x,
speed
);
telemetry.addLine("fl = " + powers.fl);
telemetry.addLine("fr = " + powers.fr);
telemetry.addLine("bl = " + powers.bl);
telemetry.addLine("br = " + powers.br);
movement.move(powers);
telemetry.addLine("speed = " + speed);
telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
telemetry.addLine("lift encoder = " + spoolPos);
telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
}
}

114
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MonoGamepad.java Normal file
View file

@ -0,0 +1,114 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MotorPowers;
import org.firstinspires.ftc.teamcode.MovementAPI;
import org.firstinspires.ftc.teamcode.Strings;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.kinematics.Poser;
@TeleOp(name = Strings.monoGamepadOpModeName)
public class MonoGamepad extends OpMode {
Hardware hardware;
MovementAPI movement;
double speed = 0.6;
LiftPosition targetLiftPos = null;
@Override
public void init() {
hardware = new Hardware(this);
movement = new MovementAPI(hardware);
hardware.setClawPosition(1);
hardware.wrist.setPosition(0.5);
}
@Override
public void loop() {
if (gamepad1.left_bumper) {
speed -= 0.005;
}
if (gamepad1.right_bumper) {
speed += 0.005;
}
speed = Math.max(0, Math.min(speed, 1));
if (gamepad1.dpad_up || gamepad1.dpad_down) {
targetLiftPos = null;
} else if (gamepad1.a) {
targetLiftPos = LiftPosition.HIGH;
} else if (gamepad1.b) {
targetLiftPos = LiftPosition.FLOOR;
}
int spoolPos = hardware.getLiftEncoder();
double pow;
if (targetLiftPos != null) {
int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
} else {
pow = (gamepad1.dpad_up ? 1 : 0) + (gamepad1.dpad_down ? -1 : 0);
}
if (gamepad1.y) {
telemetry.addLine("lift status: !!! override set !!!");
} else {
if (spoolPos < -50) {
telemetry.addLine("lift status: too far below min, moving up");
pow = Math.max(pow, 0.2);
} else if (spoolPos < 0) {
telemetry.addLine("lift status: below min, stopping downward motion");
pow = Math.max(pow, 0);
} else if (spoolPos < 100) {
telemetry.addLine("lift status: close to min, slowing downward motion");
pow = Math.max(pow, -0.5);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
telemetry.addLine("lift status: too far above max, moving down");
pow = Math.min(pow, -0.2);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
telemetry.addLine("lift status: above max, stopping upward motion");
pow = Math.min(pow, 0);
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
telemetry.addLine("lift status: close to max, slowing upward motion");
pow = Math.min(pow, 0.5);
} else {
telemetry.addLine("lift status: ok");
}
}
telemetry.addLine("foobarbaz: " + pow);
hardware.spool.setPower(pow);
hardware.setClawPosition(1 - gamepad1.left_trigger);
hardware.setWristPosition((gamepad1.dpad_left ? 1 : 0) + (gamepad1.dpad_right ? -1 : 0));
MotorPowers powers = MotorPowers.calculate(
gamepad1.left_stick_x,
-gamepad1.left_stick_y,
gamepad1.right_stick_x,
speed
);
telemetry.addLine("fl = " + powers.fl);
telemetry.addLine("fr = " + powers.fr);
telemetry.addLine("bl = " + powers.bl);
telemetry.addLine("br = " + powers.br);
movement.move(powers);
telemetry.addLine("speed = " + speed);
telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
telemetry.addLine("lift encoder = " + spoolPos);
telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
}
}

4
build.common.gradle
View file

@ -94,14 +94,14 @@ android {
signingConfig signingConfigs.release
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
debug {
debuggable true
jniDebuggable true
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
}

2
build.dependencies.gradle
View file

@ -4,6 +4,7 @@ repositories {
flatDir {
dirs rootProject.file('libs')
}
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
@ -18,5 +19,6 @@ dependencies {
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.6'
}

2
build.gradle
View file

@ -29,4 +29,4 @@ repositories {
flatDir {
dirs '../libs'
}
}
}