missing
8cdd3dd447
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting https://xkcd.com/1296/
90 lines
2.8 KiB
Java
90 lines
2.8 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 = "Multi-Cone Left Side", group = "don't use me", preselectTeleOp = Strings.mainOpModeName)
|
|
public class MultiConeAutonLeft 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.5));
|
|
movement.turn(90);
|
|
|
|
// lift and move forward
|
|
movement.liftTo(LiftPosition.HIGH);
|
|
movement.straight(Distance.inInches(4));
|
|
|
|
// wrist down, drop, wrist up
|
|
hardware.setWristPosition(0);
|
|
sleep(500);
|
|
hardware.setClawPosition(0);
|
|
sleep(500);
|
|
hardware.setClawPosition(1);
|
|
|
|
// move back and lower
|
|
movement.straight(Distance.inInches(-4));
|
|
movement.liftTo(LiftPosition.STACK);
|
|
|
|
movement.turn(180);
|
|
movement.strafe(Distance.inTiles(0.5));
|
|
|
|
hardware.setClawPosition(0);
|
|
|
|
movement.straight(Distance.inTiles(2).add(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
|
|
|
|
hardware.setClawPosition(1);
|
|
|
|
movement.liftTo(LiftPosition.HIGH);
|
|
hardware.setWristPosition(-2/3.);
|
|
|
|
movement.straight(Distance.inTiles(-1.5).sub(Distance.inTiles(1).sub(Distance.ROBOT_LENGTH).div(2)));
|
|
movement.turn(90);
|
|
|
|
// lift and move forward
|
|
movement.liftTo(LiftPosition.HIGH);
|
|
movement.straight(Distance.inInches(4));
|
|
|
|
// 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(-4));
|
|
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;
|
|
// }
|
|
}
|
|
}
|