60 lines
1.9 KiB
Java
60 lines
1.9 KiB
Java
|
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;
|
||
|
}
|
||
|
}
|
||
|
}
|