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 + ")";
|
||||
}
|
||||
|
||||
public boolean equals(FieldVector rhs) {
|
||||
return this.x == rhs.x && this.y == rhs.y;
|
||||
}
|
||||
|
||||
public FieldVector add(FieldVector rhs) {
|
||||
return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
|
||||
}
|
||||
|
|
|
@ -40,6 +40,14 @@ public class Poser {
|
|||
this.lastTargetLiftPos = hardware.getLiftEncoder();
|
||||
}
|
||||
|
||||
public double getSpeed() {
|
||||
return this.speed;
|
||||
}
|
||||
|
||||
public void setSpeed(double speed) {
|
||||
this.speed = speed;
|
||||
}
|
||||
|
||||
private Pose getPose() {
|
||||
return new Pose(
|
||||
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) {
|
||||
this.goTo(new Pose(pos, yaw, liftPos));
|
||||
}
|
||||
|
@ -109,7 +125,7 @@ public class Poser {
|
|||
Pose currentPose = this.getPose();
|
||||
|
||||
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;
|
||||
|
||||
if (
|
||||
|
|
Loading…
Reference in a new issue