41 lines
1.3 KiB
Java
41 lines
1.3 KiB
Java
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.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.15, FieldVector.ZERO, 0);
|
|
|
|
poser.goTo(new NullablePose());
|
|
|
|
waitForStart();
|
|
|
|
// while (!isStopRequested()) {
|
|
// poser.goTo(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR));
|
|
// sleep(1000);
|
|
// poser.goTo(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH));
|
|
// sleep(1000);
|
|
// poser.goTo(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM));
|
|
// sleep(1000);
|
|
// }
|
|
|
|
while (!isStopRequested()) {
|
|
poser.goTo(new NullablePose(FieldVector.inTiles(1, 0)));
|
|
poser.goTo(new NullablePose(FieldVector.inTiles(0, 0)));
|
|
}
|
|
}
|
|
}
|