DashboardPoser
This commit is contained in:
parent
6bb0d1796e
commit
139a7b59a9
|
@ -0,0 +1,57 @@
|
||||||
|
package org.firstinspires.ftc.teamcode.auto;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.reflection.FieldProvider;
|
||||||
|
import com.acmerobotics.dashboard.config.variable.BasicVariable;
|
||||||
|
import com.acmerobotics.dashboard.config.variable.CustomVariable;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.Hardware;
|
||||||
|
import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
|
||||||
|
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
|
||||||
|
import org.firstinspires.ftc.teamcode.kinematics.Poser;
|
||||||
|
|
||||||
|
@TeleOp(name = "Dashboard Poser")
|
||||||
|
public class DashboardPoser extends LinearOpMode {
|
||||||
|
private double targetXPos = 0;
|
||||||
|
private double targetYPos = 0;
|
||||||
|
private double targetYaw = 0;
|
||||||
|
private LiftPosition targetLiftPos = LiftPosition.FLOOR;
|
||||||
|
private double speed = 0.2;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
Hardware hardware = new Hardware(this);
|
||||||
|
Poser poser = new Poser(hardware, speed, FieldVector.inTiles(targetXPos, targetYPos), targetYaw);
|
||||||
|
|
||||||
|
DashboardPoser me = this;
|
||||||
|
FtcDashboard.getInstance().withConfigRoot(root -> {
|
||||||
|
CustomVariable subVar = new CustomVariable();
|
||||||
|
me.addFieldVariable(subVar, "Target X Position", "targetXPos");
|
||||||
|
me.addFieldVariable(subVar, "Target Y Position", "targetYPos");
|
||||||
|
me.addFieldVariable(subVar, "Target Yaw", "targetYaw");
|
||||||
|
me.addFieldVariable(subVar, "Target Lift Height", "targetLiftPos");
|
||||||
|
me.addFieldVariable(subVar, "Speed", "speed");
|
||||||
|
|
||||||
|
root.putVariable(me.getClass().getSimpleName(), subVar);
|
||||||
|
});
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
while (!isStopRequested()) {
|
||||||
|
poser.setSpeed(speed);
|
||||||
|
poser.goTo(FieldVector.inTiles(targetXPos, targetYPos), targetYaw, targetLiftPos);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private <T> void addFieldVariable(CustomVariable root, String name, String field) {
|
||||||
|
try {
|
||||||
|
root.putVariable(name, new BasicVariable<T>(
|
||||||
|
new FieldProvider<>(this.getClass().getField(field), this)
|
||||||
|
));
|
||||||
|
} catch (NoSuchFieldException e) {
|
||||||
|
telemetry.log().add("warning: field `" + field + "` not found on " + this.getClass().getSimpleName());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -28,6 +28,10 @@ public class FieldVector {
|
||||||
return "(" + this.x + ", " + this.y + ")";
|
return "(" + this.x + ", " + this.y + ")";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public boolean equals(FieldVector rhs) {
|
||||||
|
return this.x == rhs.x && this.y == rhs.y;
|
||||||
|
}
|
||||||
|
|
||||||
public FieldVector add(FieldVector rhs) {
|
public FieldVector add(FieldVector rhs) {
|
||||||
return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
|
return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
|
||||||
}
|
}
|
||||||
|
|
|
@ -40,6 +40,14 @@ public class Poser {
|
||||||
this.lastTargetLiftPos = hardware.getLiftEncoder();
|
this.lastTargetLiftPos = hardware.getLiftEncoder();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public double getSpeed() {
|
||||||
|
return this.speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setSpeed(double speed) {
|
||||||
|
this.speed = speed;
|
||||||
|
}
|
||||||
|
|
||||||
private Pose getPose() {
|
private Pose getPose() {
|
||||||
return new Pose(
|
return new Pose(
|
||||||
integrator.pos,
|
integrator.pos,
|
||||||
|
@ -56,6 +64,14 @@ public class Poser {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private static double modAngle(double angle) {
|
||||||
|
angle %= 360;
|
||||||
|
if (angle > 180) {
|
||||||
|
angle -= 360;
|
||||||
|
}
|
||||||
|
return angle;
|
||||||
|
}
|
||||||
|
|
||||||
public void goTo(FieldVector pos, double yaw, int liftPos) {
|
public void goTo(FieldVector pos, double yaw, int liftPos) {
|
||||||
this.goTo(new Pose(pos, yaw, liftPos));
|
this.goTo(new Pose(pos, yaw, liftPos));
|
||||||
}
|
}
|
||||||
|
@ -109,7 +125,7 @@ public class Poser {
|
||||||
Pose currentPose = this.getPose();
|
Pose currentPose = this.getPose();
|
||||||
|
|
||||||
FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
|
FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
|
||||||
double yawDiff = targetPose.yaw - currentPose.yaw;
|
double yawDiff = modAngle(targetPose.yaw - currentPose.yaw);
|
||||||
int liftDiff = targetPose.liftPos - currentPose.liftPos;
|
int liftDiff = targetPose.liftPos - currentPose.liftPos;
|
||||||
|
|
||||||
if (
|
if (
|
||||||
|
|
Loading…
Reference in a new issue