58 lines
2.4 KiB
Java
58 lines
2.4 KiB
Java
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);
|
|
|
|
FtcDashboard.getInstance().withConfigRoot(root -> {
|
|
CustomVariable subVar = new CustomVariable();
|
|
this.addFieldVariable(subVar, "Target X Position", "targetXPos");
|
|
this.addFieldVariable(subVar, "Target Y Position", "targetYPos");
|
|
this.addFieldVariable(subVar, "Target Yaw", "targetYaw");
|
|
this.addFieldVariable(subVar, "Target Lift Height", "targetLiftPos");
|
|
this.addFieldVariable(subVar, "Speed", "speed");
|
|
|
|
root.putVariable(this.getClass().getSimpleName(), subVar);
|
|
});
|
|
FtcDashboard.getInstance().updateConfig();
|
|
|
|
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());
|
|
}
|
|
}
|
|
}
|