karx/FreeOfCharge2022-23
1
1
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

optimization

Browse source
This commit is contained in:
Khosraw 2022-10-20 21:34:22 -05:00
parent 4983fe23a0
commit 87dbbefe2d
47 changed files with 640 additions and 665 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ActionObject.java
View file

@ -3,25 +3,29 @@ package org.firstinspires.ftc.teamcode.createdcode.driveobjs;
import com.acmerobotics.roadrunner.geometry.Pose2d;
public class ActionObject {
private double x, y;
private double angle;
private final double x;
private final double y;
private final double angle;
private int methodID;
public ActionObject(double x, double y, double angle){
public ActionObject(double x, double y, double angle) {
this.x = x;
this.y = y;
this.angle = angle;
}
public ActionObject(double x, double y, double angle, int methodID){
public ActionObject(double x, double y, double angle, int methodID) {
this.x = x;
this.y = y;
this.angle = angle;
this.methodID = methodID;
}
public Pose2d getPose2d(){
public Pose2d getPose2d() {
return new Pose2d(x, y, Math.toRadians(angle));
}
public int getMethodID(){return methodID;}
public int getMethodID() {
return methodID;
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/InterruptAction.java
View file

@ -12,12 +12,10 @@ public class InterruptAction {
ArrayList<Servo> servos;
ArrayList<DcMotor> dcMotors;
ArrayList<CRServo> crServos;
InterruptAction(HardwareMap hardwareMap){
InterruptAction(HardwareMap hardwareMap) {
}
}

74
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ObjectDetector.java
View file

@ -28,18 +28,20 @@ import java.util.List;
import java.util.concurrent.TimeUnit;
@Config
public class ObjectDetector
{
public class ObjectDetector {
private OpenCvWebcam webcam;
private FtcDashboard dashboard = FtcDashboard.getInstance();
private Telemetry dashboardTelemetry = dashboard.getTelemetry();
private HardwareMap hardwareMap;
private final FtcDashboard dashboard = FtcDashboard.getInstance();
private final Telemetry dashboardTelemetry = dashboard.getTelemetry();
private final HardwareMap hardwareMap;
private int pos;
private int approxHeight, pos1, pos2, pos3;
private final int approxHeight;
private final int pos1;
private final int pos2;
private final int pos3;
public ObjectDetector(HardwareMap hardwareMap, int approxHeight, int pos1, int pos2, int pos3){
public ObjectDetector(HardwareMap hardwareMap, int approxHeight, int pos1, int pos2, int pos3) {
this.hardwareMap = hardwareMap;
startCamera();
this.pos1 = pos1;
@ -49,31 +51,26 @@ public class ObjectDetector
}
public void startCamera()
{
public void startCamera() {
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
webcam.setPipeline(new DetectionPipeline());
webcam.setMillisecondsPermissionTimeout(2500); // Timeout for obtaining permission is configurable. Set before opening.
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
{
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened()
{
public void onOpened() {
webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT);
}
@Override
public void onError(int errorCode)
{
public void onError(int errorCode) {
}
});
for (long stop = System.nanoTime() + TimeUnit.SECONDS.toNanos(4); stop > System.nanoTime();)
{
for (long stop = System.nanoTime() + TimeUnit.SECONDS.toNanos(4); stop > System.nanoTime(); ) {
dashboard.startCameraStream(webcam, 0);
@ -91,22 +88,20 @@ public class ObjectDetector
}
public int getPos(){
public int getPos() {
return pos;
}
public void endStream(){
public void endStream() {
webcam.stopStreaming();
}
class DetectionPipeline extends OpenCvPipeline
{
class DetectionPipeline extends OpenCvPipeline {
boolean viewportPaused;
@Override
public Mat processFrame(Mat input)
{
public Mat processFrame(Mat input) {
Mat blurredImage = new Mat();
Mat hsvImage = new Mat();
Mat mask = new Mat();
@ -137,15 +132,13 @@ public class ObjectDetector
Mat output = input.clone();
List<Rect> rectList= new ArrayList<>();
List<Rect> rectList = new ArrayList<>();
// find contours
Imgproc.findContours(morphOutput, contours, hierarchy, Imgproc.RETR_CCOMP, Imgproc.CHAIN_APPROX_SIMPLE);
if (hierarchy.size().height > 0 && hierarchy.size().width > 0)
{
if (hierarchy.size().height > 0 && hierarchy.size().width > 0) {
// for each contour, display it in blue
for (int idx = 0; idx >= 0; idx = (int) hierarchy.get(0, idx)[0])
{
for (int idx = 0; idx >= 0; idx = (int) hierarchy.get(0, idx)[0]) {
Imgproc.drawContours(output, contours, idx, new Scalar(250, 0, 0));
Rect rect = Imgproc.boundingRect(contours.get(idx));
@ -155,13 +148,12 @@ public class ObjectDetector
}
if (rectList.size() > 0) {
double largestArea = -1;
Rect largestRect = new Rect();
for (int i = 0; i < rectList.size(); i++) {
Rect checkRect = rectList.get(i);
if (checkRect.area() > largestArea && Math.abs(checkRect.y + checkRect.height/2 - approxHeight) < 10) {
if (checkRect.area() > largestArea && Math.abs(checkRect.y + checkRect.height / 2 - approxHeight) < 10) {
largestArea = checkRect.area();
largestRect = checkRect;
}
@ -176,14 +168,14 @@ public class ObjectDetector
largestRect.y + largestRect.height),
new Scalar(0, 255, 0), 4);
int centerX = largestRect.x + largestRect.width/2;
int centerY = largestRect.y + largestRect.height/2;
int centerX = largestRect.x + largestRect.width / 2;
int centerY = largestRect.y + largestRect.height / 2;
if (Math.abs(pos1-centerX) < Math.abs(pos2-centerX) && Math.abs(pos1-centerX) < Math.abs(pos3-centerX))
pos = 1;
else if (Math.abs(pos2-centerX) < Math.abs(pos1-centerX) && Math.abs(pos2-centerX) < Math.abs(pos3-centerX))
if (Math.abs(pos1 - centerX) < Math.abs(pos2 - centerX) && Math.abs(pos1 - centerX) < Math.abs(pos3 - centerX))
pos = 1;
else if (Math.abs(pos2 - centerX) < Math.abs(pos1 - centerX) && Math.abs(pos2 - centerX) < Math.abs(pos3 - centerX))
pos = 2;
else if (Math.abs(pos3-centerX) < Math.abs(pos2-centerX) && Math.abs(pos3-centerX) < Math.abs(pos1-centerX))
else if (Math.abs(pos3 - centerX) < Math.abs(pos2 - centerX) && Math.abs(pos3 - centerX) < Math.abs(pos1 - centerX))
pos = 3;
@ -197,16 +189,12 @@ public class ObjectDetector
}
@Override
public void onViewportTapped()
{
public void onViewportTapped() {
viewportPaused = !viewportPaused;
if(viewportPaused)
{
if (viewportPaused) {
webcam.pauseViewport();
}
else
{
} else {
webcam.resumeViewport();
}
}

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBase.java
View file

@ -34,22 +34,22 @@ public class AutoBase extends EnhancedAutoMode {
private static int[] methodIdArr = {};
public static double[] xCoordArrCase1 = {START_POS_X};
private static double[] yCoordArrCase1 = {START_POS_Y};
private static double[] angleArrCase1 = {START_POS_ANGLE};
private static int[] methodIdArrCase1 = {START_METHOD};
private static final double[] yCoordArrCase1 = {START_POS_Y};
private static final double[] angleArrCase1 = {START_POS_ANGLE};
private static final int[] methodIdArrCase1 = {START_METHOD};
public static double[] xCoordArrCase2 = {START_POS_X};
private static double[] yCoordArrCase2 = {START_POS_Y};
private static double[] angleArrCase2 = {START_POS_ANGLE};
private static int[] methodIdArrCase2 = {START_METHOD};
private static final double[] yCoordArrCase2 = {START_POS_Y};
private static final double[] angleArrCase2 = {START_POS_ANGLE};
private static final int[] methodIdArrCase2 = {START_METHOD};
public static double[] xCoordArrCase3 = {START_POS_X};
private static double[] yCoordArrCase3 = {START_POS_Y};
private static double[] angleArrCase3 = {START_POS_ANGLE};
private static int[] methodIdArrCase3 = {START_METHOD};
private static final double[] yCoordArrCase3 = {START_POS_Y};
private static final double[] angleArrCase3 = {START_POS_ANGLE};
private static final int[] methodIdArrCase3 = {START_METHOD};
@Override
public void runOpMode(){
public void runOpMode() {
int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X);
dashboardTelemetry.addData("Position Detected is", pos);
dashboardTelemetry.update();
@ -59,14 +59,12 @@ public class AutoBase extends EnhancedAutoMode {
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
}
else if (pos == 2){
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
}
else if (pos == 3) {
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueNearParkAuto.java
View file

@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous (group = "Blue")
@Autonomous(group = "Blue")
@Config
public class BlueNearParkAuto extends EnhancedAutoMode {
public static double START_POS_X = -32;
@ -35,22 +35,22 @@ public class BlueNearParkAuto extends EnhancedAutoMode {
private static int[] methodIdArr = {};
public static double[] xCoordArrCase1 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE_X};
private static double[] yCoordArrCase1 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static double[] angleArrCase1 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static int[] methodIdArrCase1 = {START_METHOD, 13, 0, 22, 10, 32, 0};
private static final double[] yCoordArrCase1 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static final double[] angleArrCase1 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static final int[] methodIdArrCase1 = {START_METHOD, 13, 0, 22, 10, 32, 0};
public static double[] xCoordArrCase2 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE_X};
private static double[] yCoordArrCase2 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static double[] angleArrCase2 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static int[] methodIdArrCase2 = {START_METHOD, 12 , 0, 22, 10, 32, 0};
private static final double[] yCoordArrCase2 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static final double[] angleArrCase2 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 0, 22, 10, 32, 0};
public static double[] xCoordArrCase3 = {START_POS_X, BLOCK_DROP_POINT, BLOCK_DROP_POINT, -56, BLUE_CAROUSEL_X, BLUE_STORAGE_X};
private static double[] yCoordArrCase3 = {START_POS_Y, 48, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static double[] angleArrCase3 = {START_POS_ANGLE, 135, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 32, 0};
private static final double[] yCoordArrCase3 = {START_POS_Y, 48, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y};
private static final double[] angleArrCase3 = {START_POS_ANGLE, 135, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE};
private static final int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 32, 0};
@Override
public void runOpMode(){
public void runOpMode() {
int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X);
dashboardTelemetry.addData("Position Detected is", pos);
dashboardTelemetry.update();
@ -60,14 +60,12 @@ public class BlueNearParkAuto extends EnhancedAutoMode {
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
}
else if (pos == 2){
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
}
else if (pos == 3) {
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueWarehouseSideAuto.java
View file

@ -8,7 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous (group = "Blue")
@Autonomous(group = "Blue")
@Config
public class BlueWarehouseSideAuto extends EnhancedAutoMode {
public static double START_POS_X = 16;
@ -32,23 +32,23 @@ public class BlueWarehouseSideAuto extends EnhancedAutoMode {
private static double[] angleArr = {};
private static int[] methodIdArr = {};
public static double[] xCoordArrCase1 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase1 = {START_POS_Y, 64, 36, 68};
private static double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase1 = {START_METHOD, 13, 22, 10, 0};
public static double[] xCoordArrCase1 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase1 = {START_POS_Y, 64, 36, 68};
private static final double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase1 = {START_METHOD, 13, 22, 10, 0};
public static double[] xCoordArrCase2 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase2 = {START_POS_Y, 64, 36, 68};
private static double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0};
public static double[] xCoordArrCase2 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase2 = {START_POS_Y, 64, 36, 68};
private static final double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0};
public static double[] xCoordArrCase3 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase3 = {START_POS_Y, 64, 36, 68};
private static double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 0};
public static double[] xCoordArrCase3 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase3 = {START_POS_Y, 64, 36, 68};
private static final double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 0};
@Override
public void runOpMode(){
public void runOpMode() {
int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X);
dashboardTelemetry.addData("Position Detected is", pos);
dashboardTelemetry.update();
@ -58,14 +58,12 @@ public class BlueWarehouseSideAuto extends EnhancedAutoMode {
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
}
else if (pos == 2){
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
}
else if (pos == 3) {
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/EnhancedAutoMode.java
View file

@ -17,13 +17,13 @@ public abstract class EnhancedAutoMode extends LinearOpMode {
private static List<Double> angles = new ArrayList<>(0);
private static List<Integer> methodIDs = new ArrayList<>(0);
private static final Exception invalidValues = new Exception("List of values is invalid");
private static List<ActionObject> actionObjects = new ArrayList<>(0);
private static List<ActionObject> actionObjects = new ArrayList<>(0);
private EnhancedDriver enhancedDriver;
private final FtcDashboard dashboard = FtcDashboard.getInstance();
private final Telemetry dashboardTelemetry = dashboard.getTelemetry();
public void makeActionObjects() throws Exception{
public void makeActionObjects() throws Exception {
actionObjects = new ArrayList<>(0);
if (xCoords.get(0) == null || yCoords.get(0) == null || angles.get(0) == null)
throw invalidValues;
@ -31,20 +31,21 @@ public abstract class EnhancedAutoMode extends LinearOpMode {
throw invalidValues;
for (int i = 0; i < xCoords.size(); i++){
for (int i = 0; i < xCoords.size(); i++) {
double x = checkNull(xCoords, i);
double y = checkNull(yCoords, i);
double angle = checkNull(angles, i);
ActionObject newAction = new ActionObject(x, y, angle, methodIDs.get(i));
ActionObject newAction = new ActionObject(x, y, angle, methodIDs.get(i));
actionObjects.add(newAction);
telemetry.addData("Amount of ActionObjects", actionObjects.size());
telemetry.update();
}
}
public void run(){
public void run() {
int count = 0;
for (ActionObject i : actionObjects){
for (ActionObject i : actionObjects) {
enhancedDriver.act(i);
count++;
telemetry.addData("Completed Action:", count);
@ -52,36 +53,39 @@ public abstract class EnhancedAutoMode extends LinearOpMode {
}
}
public double checkNull(List<Double> toCheck, int i){
public double checkNull(List<Double> toCheck, int i) {
return toCheck.get(i) == null ? checkNull(toCheck, i) : toCheck.get(i);
}
public void initThings(){
public void initThings() {
enhancedDriver = new EnhancedDriver(hardwareMap);
}
public void setXCoords(double[] xArr){
public void setXCoords(double[] xArr) {
xCoords = new ArrayList<>(0);
for (Double i : xArr)
xCoords.add(i);
}
public void setYCoords(double[] yArr){
public void setYCoords(double[] yArr) {
yCoords = new ArrayList<>(0);
for (Double i : yArr)
yCoords.add(i);
}
public void setAngles(double[] angleArr){
public void setAngles(double[] angleArr) {
angles = new ArrayList<>(0);
for (Double i : angleArr)
angles.add(i);
}
public void setMethodIDS(int[] methodIdsArr){
public void setMethodIDS(int[] methodIdsArr) {
methodIDs = new ArrayList<>(0);
for (int i : methodIdsArr)
methodIDs.add(i);
}
public int getShippingElementPos(int h, int pos1, int pos2, int pos3){
public int getShippingElementPos(int h, int pos1, int pos2, int pos3) {
ObjectDetector detector = new ObjectDetector(hardwareMap, h, pos1, pos2, pos3);
int pos = detector.getPos();
detector.endStream();

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedNearParkAuto.java
View file

@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous (group = "Red")
@Autonomous(group = "Red")
@Config
public class RedNearParkAuto extends EnhancedAutoMode {
public static double START_POS_X = -40;
@ -38,7 +38,7 @@ public class RedNearParkAuto extends EnhancedAutoMode {
public static double[] xCoordArrCase2 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, RED_CAROUSEL_X, RED_STORAGE_X};
private static final double[] yCoordArrCase2 = {START_POS_Y, -48, -24, -24, -24, RED_CAROUSEL_Y, RED_STORAGE_Y};
private static final double[] angleArrCase2 = {START_POS_ANGLE, 0, 0, 0, 0, RED_CAROUSEL_ANGLE, RED_STORAGE_ANGLE};
private static final int[] methodIdArrCase2 = {START_METHOD, 12 , 0, 22, 10, 31, 0};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 0, 22, 10, 31, 0};
public static double[] xCoordArrCase3 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, RED_CAROUSEL_X, RED_STORAGE_X};
private static final double[] yCoordArrCase3 = {START_POS_Y, -48, -24, -24, -24, RED_CAROUSEL_Y, RED_STORAGE_Y};
@ -49,7 +49,7 @@ public class RedNearParkAuto extends EnhancedAutoMode {
Telemetry dashboardTelemetry = dashboard.getTelemetry();
@Override
public void runOpMode(){
public void runOpMode() {
int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X);
dashboardTelemetry.addData("Position Detected is", pos);
dashboardTelemetry.update();
@ -59,14 +59,12 @@ public class RedNearParkAuto extends EnhancedAutoMode {
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
}
else if (pos == 2){
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
}
else if (pos == 3) {
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedWarehouseSideAuto.java
View file

@ -8,7 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous (group = "Red")
@Autonomous(group = "Red")
@Config
public class RedWarehouseSideAuto extends EnhancedAutoMode {
public static double START_POS_X = 8;
@ -32,23 +32,23 @@ public class RedWarehouseSideAuto extends EnhancedAutoMode {
private static double[] angleArr = {};
private static int[] methodIdArr = {};
public static double[] xCoordArrCase1 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase1 = {START_POS_Y, -64, -36, -68};
private static double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase1 = {START_METHOD, 11, 22, 10, 0};
public static double[] xCoordArrCase1 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase1 = {START_POS_Y, -64, -36, -68};
private static final double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase1 = {START_METHOD, 11, 22, 10, 0};
public static double[] xCoordArrCase2 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase2 = {START_POS_Y, -64, -36, -68};
private static double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0};
public static double[] xCoordArrCase2 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase2 = {START_POS_Y, -64, -36, -68};
private static final double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0};
public static double[] xCoordArrCase3 = {START_POS_X, -12. -12, 36};
private static double[] yCoordArrCase3 = {START_POS_Y, -64, -36, -68};
private static double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0};
private static int[] methodIdArrCase3 = {START_METHOD, 13, 22, 10, 0};
public static double[] xCoordArrCase3 = {START_POS_X, -12. - 12, 36};
private static final double[] yCoordArrCase3 = {START_POS_Y, -64, -36, -68};
private static final double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0};
private static final int[] methodIdArrCase3 = {START_METHOD, 13, 22, 10, 0};
@Override
public void runOpMode(){
public void runOpMode() {
int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X);
dashboardTelemetry.addData("Position Detected is", pos);
dashboardTelemetry.update();
@ -58,14 +58,12 @@ public class RedWarehouseSideAuto extends EnhancedAutoMode {
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
}
else if (pos == 2){
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
}
else if (pos == 3) {
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/AutoBotAuto.java
View file

@ -6,6 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@Disabled
@Autonomous
@ -14,7 +15,7 @@ public class AutoBotAuto extends LinearOpMode {
public static int dist = 2000;
public void runOpMode(){
public void runOpMode() {
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
@ -51,8 +52,7 @@ public class AutoBotAuto extends LinearOpMode {
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
while (frontLeft.isBusy() || frontRight.isBusy() || rearLeft.isBusy() || rearRight.isBusy()){
while (frontLeft.isBusy() || frontRight.isBusy() || rearLeft.isBusy() || rearRight.isBusy()) {
double power = 1 - Math.max(Math.min((dist - frontLeft.getCurrentPosition()) / 2000, 1), 0.8);
frontRight.setPower(power);
frontLeft.setPower(power);
@ -65,7 +65,8 @@ public class AutoBotAuto extends LinearOpMode {
telemetry.addData("Ran", "Ran");
telemetry.update();
}
public void motorsPower(double frontRightPow, double frontLeftPow, double rearRightPow, double rearLeftPow){
public void motorsPower(double frontRightPow, double frontLeftPow, double rearRightPow, double rearLeftPow) {
frontRight.setPower(frontRightPow);
frontLeft.setPower(frontLeftPow);
rearRight.setPower(rearRightPow);
@ -74,6 +75,4 @@ public class AutoBotAuto extends LinearOpMode {
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/EncoderValueTester.java
View file

@ -12,7 +12,7 @@ public class EncoderValueTester extends LinearOpMode {
DcMotor testMotor;
public void runOpMode(){
public void runOpMode() {
testMotor = hardwareMap.get(DcMotor.class, "testMotor");
testMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
testMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@ -21,13 +21,11 @@ public class EncoderValueTester extends LinearOpMode {
testMotor.setTargetPosition(testMotor.getCurrentPosition() + 1120);
testMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
testMotor.setPower(0.2);
while (testMotor.isBusy()){
while (testMotor.isBusy()) {
}
testMotor.setPower(0);
sleep(500);
}
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/RoadRunnerAutoTester.java
View file

@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.teamcode.drive.*;
@ -26,7 +27,7 @@ public class RoadRunnerAutoTester extends LinearOpMode {
waitForStart();
if(isStopRequested()) return;
if (isStopRequested()) return;
drive.followTrajectory(traj1);
drive.followTrajectory(traj2);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/SpinTheMotor.java
View file

@ -7,7 +7,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
@Disabled
@Autonomous
public class SpinTheMotor extends LinearOpMode{
public class SpinTheMotor extends LinearOpMode {
private DcMotor testMotor;
@Override

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutoConstants.java
View file

@ -15,44 +15,44 @@ public class AutoConstants {
public static double GRABBER_OPEN = 0.4;
//blue values
//blue1
public static int BLUE1_START_X = -30;
public static int BLUE1_START_Y = 65;
public static int BLUE1_MIDPOS_X = -53;
public static int BLUE1_MIDPOS_Y = 36;
public static int BLUE1_BLOCK_REPOS_X = 2;
public static int BLUE1_BLOCK_MIDPOS_X = 20;
//blue1
public static int BLUE1_START_X = -30;
public static int BLUE1_START_Y = 65;
public static int BLUE1_MIDPOS_X = -53;
public static int BLUE1_MIDPOS_Y = 36;
public static int BLUE1_BLOCK_REPOS_X = 2;
public static int BLUE1_BLOCK_MIDPOS_X = 20;
//blue park
public static int BLUE_PARK_START_X = 18;
public static int BLUE_PARK_START_Y = 65;
public static int BLUE_WAREHOUSE_X = 48;
public static int BLUE_WAREHOUSE_Y = 44;
//blue park
public static int BLUE_PARK_START_X = 18;
public static int BLUE_PARK_START_Y = 65;
public static int BLUE_WAREHOUSE_X = 48;
public static int BLUE_WAREHOUSE_Y = 44;
//blue movements
public static double BLUE_CAROUSEL_X = -50.8;
public static double BLUE_CAROUSEL_Y = 50.8;
public static int BLUE_START_FORWARD = 39;
public static int BLUE_FINAL_STRAFE = 17;
//blue movements
public static double BLUE_CAROUSEL_X = -50.8;
public static double BLUE_CAROUSEL_Y = 50.8;
public static int BLUE_START_FORWARD = 39;
public static int BLUE_FINAL_STRAFE = 17;
//red values
//red1
public static int RED1_START_X = -34;
public static int RED1_START_Y = -65;
public static int RED1_MIDPOS_X = -55;
public static int RED1_MIDPOS_Y = -36;
public static int RED1_BLOCK_REPOS_X = 4;
public static int RED1_BLOCK_MIDPOS_X = 20;
//red1
public static int RED1_START_X = -34;
public static int RED1_START_Y = -65;
public static int RED1_MIDPOS_X = -55;
public static int RED1_MIDPOS_Y = -36;
public static int RED1_BLOCK_REPOS_X = 4;
public static int RED1_BLOCK_MIDPOS_X = 20;
//red park
public static int RED_PARK_START_X = 18;
public static int RED_PARK_START_Y = -65;
public static int RED_WAREHOUSE_X = 48;
public static int RED_WAREHOUSE_Y = -44;
//red park
public static int RED_PARK_START_X = 18;
public static int RED_PARK_START_Y = -65;
public static int RED_WAREHOUSE_X = 48;
public static int RED_WAREHOUSE_Y = -44;
//red movements
public static double RED_CAROUSEL_X = -56;
public static double RED_CAROUSEL_Y = -56.75;
public static int RED_START_FORWARD = 40;
public static int RED_FINAL_STRAFE = 15;
//red movements
public static double RED_CAROUSEL_X = -56;
public static double RED_CAROUSEL_Y = -56.75;
public static int RED_START_FORWARD = 40;
public static int RED_FINAL_STRAFE = 15;
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutonomousTester.java
View file

@ -25,7 +25,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.*;
@Disabled
@Autonomous
public class AutonomousTester extends LinearOpMode {
@ -35,7 +34,7 @@ public class AutonomousTester extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
public void runOpMode() {
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -44,7 +43,7 @@ public class AutonomousTester extends LinearOpMode {
.forward(41)
.build();
Trajectory part2 = drive.trajectoryBuilder(part1.end().plus(new Pose2d(0,0, -1 * Math.toRadians(90))))
Trajectory part2 = drive.trajectoryBuilder(part1.end().plus(new Pose2d(0, 0, -1 * Math.toRadians(90))))
.splineTo(new Vector2d(BLUE_CAROUSEL_X, BLUE_CAROUSEL_Y), Math.toRadians(0))
.build();
@ -68,10 +67,10 @@ public class AutonomousTester extends LinearOpMode {
//telemetry.addData("grabServo", grabServo.getController());
//telemetry.update();
sleep(1000);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(part1);
drive.turn(-1*Math.toRadians(90));
moveArms(ARM_MAX_DIST /4);
drive.turn(-1 * Math.toRadians(90));
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(0.4);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -82,7 +81,8 @@ public class AutonomousTester extends LinearOpMode {
drive.followTrajectory(part5);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setPower(0.5);
@ -90,7 +90,7 @@ public class AutonomousTester extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/blueParkOnly.java
View file

@ -16,6 +16,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Disabled
@Autonomous(group = "Blue")
public class blueParkOnly extends LinearOpMode {
@ -26,7 +27,7 @@ public class blueParkOnly extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
public void runOpMode() {
Pose2d startPos = new Pose2d(BLUE_PARK_START_X, BLUE_PARK_START_Y, Math.toRadians(0));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -56,7 +57,8 @@ public class blueParkOnly extends LinearOpMode {
drive.followTrajectory(moveIntoWarehouse);
drive.followTrajectory(moveToPark);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -66,7 +68,7 @@ public class blueParkOnly extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/blueSpinPark.java
View file

@ -29,7 +29,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.*;
@Disabled
@Autonomous(group = "Blue")
public class blueSpinPark extends LinearOpMode {
@ -39,8 +38,8 @@ public class blueSpinPark extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1*Math.toRadians(90));
public void runOpMode() {
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1 * Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -71,9 +70,9 @@ public class blueSpinPark extends LinearOpMode {
grabServo.setPosition(GRABBER_CLOSE);
sleep(GRAB_TIME);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(moveToBlockDrop);
moveArms(ARM_MAX_DIST /4);
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(GRABBER_OPEN);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -84,7 +83,8 @@ public class blueSpinPark extends LinearOpMode {
drive.followTrajectory(park);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setPower(0.5);
@ -92,7 +92,7 @@ public class blueSpinPark extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/blueSpinParkLongPath.java
View file

@ -44,21 +44,21 @@ public class blueSpinParkLongPath extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1*Math.toRadians(90));
public void runOpMode() {
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1 * Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory moveToBlockDrop = drive.trajectoryBuilder(startPos)
.lineToLinearHeading(new Pose2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y, -1*Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y, -1 * Math.toRadians(90)))
.build();
Trajectory moveToBlockDrop2 = drive.trajectoryBuilder(moveToBlockDrop.end())
.lineToLinearHeading(new Pose2d(BLUE1_START_X + BLUE1_BLOCK_REPOS_X, BLUE1_START_Y - BLUE_START_FORWARD, -1*Math.toRadians(180)))
.lineToLinearHeading(new Pose2d(BLUE1_START_X + BLUE1_BLOCK_REPOS_X, BLUE1_START_Y - BLUE_START_FORWARD, -1 * Math.toRadians(180)))
.build();
Trajectory moveToCarousel = drive.trajectoryBuilder(moveToBlockDrop2.end())
.lineTo(new Vector2d(moveToBlockDrop2.end().getX()-BLUE1_BLOCK_MIDPOS_X, BLUE1_START_Y - BLUE_START_FORWARD))
.lineTo(new Vector2d(moveToBlockDrop2.end().getX() - BLUE1_BLOCK_MIDPOS_X, BLUE1_START_Y - BLUE_START_FORWARD))
.build();
Trajectory moveToCarousel2 = drive.trajectoryBuilder(moveToCarousel.end())
@ -81,12 +81,12 @@ public class blueSpinParkLongPath extends LinearOpMode {
grabServo.setPosition(GRABBER_CLOSE);
sleep(GRAB_TIME);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(moveToBlockDrop);
drive.followTrajectory(moveToBlockDrop2);
moveArms(ARM_MAX_DIST /4);
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(GRABBER_OPEN);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -94,11 +94,12 @@ public class blueSpinParkLongPath extends LinearOpMode {
drive.followTrajectory(moveToCarousel);
drive.followTrajectory(moveToCarousel2);
carouselServo.setPower(-1*CAROUSEL_POWER);
carouselServo.setPower(-1 * CAROUSEL_POWER);
sleep(CAROUSEL_WAIT);
drive.followTrajectory(park);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setPower(0.5);
@ -106,7 +107,7 @@ public class blueSpinParkLongPath extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/blueSpinParkLongPathv2.java
View file

@ -18,7 +18,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.*;
@Disabled
@Autonomous(group = "Blue")
public class blueSpinParkLongPathv2 extends LinearOpMode {
@ -28,21 +27,21 @@ public class blueSpinParkLongPathv2 extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1*Math.toRadians(90));
public void runOpMode() {
Pose2d startPos = new Pose2d(BLUE1_START_X, BLUE1_START_Y, -1 * Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory moveToBlockDrop = drive.trajectoryBuilder(startPos)
.lineToLinearHeading(new Pose2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y+3, -1*Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y + 3, -1 * Math.toRadians(90)))
.build();
Trajectory moveToBlockDrop2 = drive.trajectoryBuilder(moveToBlockDrop.end())
.lineTo(new Vector2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y-15))
.lineTo(new Vector2d(BLUE1_MIDPOS_X, BLUE1_MIDPOS_Y - 15))
.build();
Trajectory moveToBlockDrop3 = drive.trajectoryBuilder(moveToBlockDrop2.end())
.lineToLinearHeading(new Pose2d(BLUE1_START_X + BLUE1_BLOCK_REPOS_X, BLUE1_START_Y - BLUE_START_FORWARD, -1*Math.toRadians(180)))
.lineToLinearHeading(new Pose2d(BLUE1_START_X + BLUE1_BLOCK_REPOS_X, BLUE1_START_Y - BLUE_START_FORWARD, -1 * Math.toRadians(180)))
.build();
Trajectory moveToCarousel = drive.trajectoryBuilder(moveToBlockDrop3.end())
@ -54,7 +53,7 @@ public class blueSpinParkLongPathv2 extends LinearOpMode {
.build();
Trajectory park = drive.trajectoryBuilder(moveToCarousel2.end())
.lineToLinearHeading(new Pose2d(BLUE_CAROUSEL_X-3, BLUE_CAROUSEL_Y - BLUE_FINAL_STRAFE, Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(BLUE_CAROUSEL_X - 3, BLUE_CAROUSEL_Y - BLUE_FINAL_STRAFE, Math.toRadians(0)))
.build();
drive.setPoseEstimate(startPos);
@ -69,13 +68,13 @@ public class blueSpinParkLongPathv2 extends LinearOpMode {
grabServo.setPosition(GRABBER_CLOSE);
sleep(GRAB_TIME);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(moveToBlockDrop);
drive.followTrajectory(moveToBlockDrop2);
drive.followTrajectory(moveToBlockDrop3);
moveArms(ARM_MAX_DIST /4);
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(GRABBER_OPEN);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -83,11 +82,12 @@ public class blueSpinParkLongPathv2 extends LinearOpMode {
drive.followTrajectory(moveToCarousel);
drive.followTrajectory(moveToCarousel2);
carouselServo.setPower(-1*CAROUSEL_POWER);
carouselServo.setPower(-1 * CAROUSEL_POWER);
sleep(CAROUSEL_WAIT);
drive.followTrajectory(park);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -97,7 +97,7 @@ public class blueSpinParkLongPathv2 extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/redParkOnly.java
View file

@ -16,6 +16,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Disabled
@Autonomous(group = "Red")
public class redParkOnly extends LinearOpMode {
@ -26,7 +27,7 @@ public class redParkOnly extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
public void runOpMode() {
Pose2d startPos = new Pose2d(RED_PARK_START_X, RED_PARK_START_Y, Math.toRadians(0));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -56,7 +57,8 @@ public class redParkOnly extends LinearOpMode {
drive.followTrajectory(moveIntoWarehouse);
drive.followTrajectory(moveToPark);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -66,7 +68,7 @@ public class redParkOnly extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/redSpinPark.java
View file

@ -29,7 +29,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.*;
@Disabled
@Autonomous(group = "Red")
public class redSpinPark extends LinearOpMode {
@ -39,7 +38,7 @@ public class redSpinPark extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
public void runOpMode() {
Pose2d startPos = new Pose2d(RED1_START_X, RED1_START_Y, Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -53,7 +52,7 @@ public class redSpinPark extends LinearOpMode {
.build();
Trajectory park = drive.trajectoryBuilder(moveToCarousel.end())
.lineToLinearHeading(new Pose2d(RED_CAROUSEL_X, RED_CAROUSEL_Y+RED_FINAL_STRAFE, Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(RED_CAROUSEL_X, RED_CAROUSEL_Y + RED_FINAL_STRAFE, Math.toRadians(0)))
.build();
drive.setPoseEstimate(startPos);
@ -68,11 +67,11 @@ public class redSpinPark extends LinearOpMode {
grabServo.setPosition(GRABBER_CLOSE);
sleep(GRAB_TIME);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(moveToBlockDrop);;
drive.followTrajectory(moveToBlockDrop);
moveArms(ARM_MAX_DIST /4);
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(GRABBER_OPEN);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -82,7 +81,8 @@ public class redSpinPark extends LinearOpMode {
sleep(CAROUSEL_WAIT);
drive.followTrajectory(park);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -92,7 +92,7 @@ public class redSpinPark extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/redSpinParkLongPath.java
View file

@ -34,7 +34,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.drive.*;
@Disabled
@Autonomous(group = "Red")
public class redSpinParkLongPath extends LinearOpMode {
@ -44,7 +43,7 @@ public class redSpinParkLongPath extends LinearOpMode {
private DcMotor armTwo;
@Override
public void runOpMode(){
public void runOpMode() {
Pose2d startPos = new Pose2d(RED1_START_X, RED1_START_Y, Math.toRadians(90));
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@ -58,7 +57,7 @@ public class redSpinParkLongPath extends LinearOpMode {
.build();
Trajectory moveToCarousel = drive.trajectoryBuilder(moveToBlockDrop2.end())
.splineToSplineHeading(new Pose2d(moveToBlockDrop2.end().getX()-RED1_BLOCK_MIDPOS_X, RED1_START_Y + RED_START_FORWARD, Math.toRadians(270)), Math.toRadians(0))
.splineToSplineHeading(new Pose2d(moveToBlockDrop2.end().getX() - RED1_BLOCK_MIDPOS_X, RED1_START_Y + RED_START_FORWARD, Math.toRadians(270)), Math.toRadians(0))
.build();
Trajectory moveToCarousel2 = drive.trajectoryBuilder(moveToCarousel.end())
@ -66,7 +65,7 @@ public class redSpinParkLongPath extends LinearOpMode {
.build();
Trajectory park = drive.trajectoryBuilder(moveToCarousel2.end())
.lineToLinearHeading(new Pose2d(RED_CAROUSEL_X, RED_CAROUSEL_Y+RED_FINAL_STRAFE, Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(RED_CAROUSEL_X, RED_CAROUSEL_Y + RED_FINAL_STRAFE, Math.toRadians(0)))
.build();
drive.setPoseEstimate(startPos);
@ -81,12 +80,12 @@ public class redSpinParkLongPath extends LinearOpMode {
grabServo.setPosition(GRABBER_CLOSE);
sleep(GRAB_TIME);
moveArms(ARM_MAX_DIST /2);
moveArms(ARM_MAX_DIST / 2);
drive.followTrajectory(moveToBlockDrop);
drive.followTrajectory(moveToBlockDrop2);
moveArms(ARM_MAX_DIST /4);
moveArms(ARM_MAX_DIST / 4);
grabServo.setPosition(GRABBER_OPEN);
sleep(500);
moveArms(-ARM_MAX_DIST);
@ -98,7 +97,8 @@ public class redSpinParkLongPath extends LinearOpMode {
sleep(CAROUSEL_WAIT);
drive.followTrajectory(park);
}
private void moveArms(int encoderVal){
private void moveArms(int encoderVal) {
armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal);
armTwo.setTargetPosition(armTwo.getCurrentPosition() - encoderVal);
armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -108,7 +108,7 @@ public class redSpinParkLongPath extends LinearOpMode {
sleep(1000);
}
private void initStuff(){
private void initStuff() {
armOne.setDirection(DcMotorSimple.Direction.FORWARD);
armTwo.setDirection(DcMotorSimple.Direction.REVERSE);
armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

88
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/autoparking/BlueSideParkingAuto.java
View file

@ -1,4 +1,3 @@
package org.firstinspires.ftc.teamcode.createdcode.oldthings.autoparking;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
@ -11,22 +10,23 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@Disabled
@Autonomous (group = "parking")
public class BlueSideParkingAuto extends LinearOpMode{
@Autonomous(group = "parking")
public class BlueSideParkingAuto extends LinearOpMode {
//change values
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
Servo bucketServo;
public void runOpMode() throws InterruptedException{
public void runOpMode() throws InterruptedException {
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
spool = hardwareMap.get(DcMotor.class, "spool");
@ -54,10 +54,10 @@ public class BlueSideParkingAuto extends LinearOpMode{
waitForStart();
frontRight.setTargetPosition(frontRight.getCurrentPosition()+500);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+500);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + 500);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + 500);
setSpeed(0.2);
while (rearRight.isBusy()){
while (rearRight.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -72,7 +72,7 @@ public class BlueSideParkingAuto extends LinearOpMode{
sleep(4000);
driveForward(200,.8);
driveForward(200, .8);
setSpeed(0);
@ -98,18 +98,18 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
public void driveForward(int distance, double speed){
public void driveForward(int distance, double speed) {
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + encodersToTurn);
setSpeed(speed);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -118,36 +118,37 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
}
public void driveBackward(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
public void driveBackward(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeLeft(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
public void strafeLeft(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() - distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeRight(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
public void strafeRight(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() - distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + distance);
setSpeed(speed);
}
/*
@ -162,11 +163,12 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
*/
public void turnCarousel(){
public void turnCarousel() {
}
public void setSpeed(double speed){
public void setSpeed(double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
@ -181,8 +183,7 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
public void rotate90(){
public void rotate90() {
frontLeft.setPower(-1);
rearLeft.setPower(-1);
frontRight.setPower(1);
@ -190,7 +191,8 @@ public class BlueSideParkingAuto extends LinearOpMode{
setSpeed(0);
}
public void turnRight(int degrees, double speed){
public void turnRight(int degrees, double speed) {
double turnDist = 10;
@ -207,7 +209,7 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
public void turnLeft(int degrees, double speed){
public void turnLeft(int degrees, double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
frontRight.setPower(speed);
@ -217,7 +219,7 @@ public class BlueSideParkingAuto extends LinearOpMode{
}
public void resetMotorDirections(){
public void resetMotorDirections() {
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);

88
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/autoparking/BlueSideParkingAutoDelayed.java
View file

@ -1,4 +1,3 @@
package org.firstinspires.ftc.teamcode.createdcode.oldthings.autoparking;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
@ -11,22 +10,23 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@Disabled
@Autonomous (group = "parking")
public class BlueSideParkingAutoDelayed extends LinearOpMode{
@Autonomous(group = "parking")
public class BlueSideParkingAutoDelayed extends LinearOpMode {
//change values
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
Servo bucketServo;
public void runOpMode() throws InterruptedException{
public void runOpMode() throws InterruptedException {
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
spool = hardwareMap.get(DcMotor.class, "spool");
@ -56,10 +56,10 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
sleep(4000);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+530);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+530);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + 530);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + 530);
setSpeed(0.2);
while (rearRight.isBusy()){
while (rearRight.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -74,7 +74,7 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
sleep(4000);
driveForward(200,.5);
driveForward(200, .5);
setSpeed(0);
@ -100,18 +100,18 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
public void driveForward(int distance, double speed){
public void driveForward(int distance, double speed) {
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + encodersToTurn);
setSpeed(speed);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -120,36 +120,37 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
}
public void driveBackward(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
public void driveBackward(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeLeft(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
public void strafeLeft(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() - distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeRight(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
public void strafeRight(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() - distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + distance);
setSpeed(speed);
}
/*
@ -164,11 +165,12 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
*/
public void turnCarousel(){
public void turnCarousel() {
}
public void setSpeed(double speed){
public void setSpeed(double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
@ -183,8 +185,7 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
public void rotate90(){
public void rotate90() {
frontLeft.setPower(-1);
rearLeft.setPower(-1);
frontRight.setPower(1);
@ -192,7 +193,8 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
setSpeed(0);
}
public void turnRight(int degrees, double speed){
public void turnRight(int degrees, double speed) {
double turnDist = 10;
@ -209,7 +211,7 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
public void turnLeft(int degrees, double speed){
public void turnLeft(int degrees, double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
frontRight.setPower(speed);
@ -219,7 +221,7 @@ public class BlueSideParkingAutoDelayed extends LinearOpMode{
}
public void resetMotorDirections(){
public void resetMotorDirections() {
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/autoparking/RedSideParkingAuto.java
View file

@ -1,5 +1,5 @@
package org.firstinspires.ftc.teamcode.createdcode.oldthings.autoparking;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@ -10,23 +10,24 @@ import com.qualcomm.robotcore.hardware.Servo;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TICKS_PER_REV;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.WHEEL_RADIUS;
@Disabled
@Autonomous (group = "parking")
public class RedSideParkingAuto extends LinearOpMode{
@Autonomous(group = "parking")
public class RedSideParkingAuto extends LinearOpMode {
//change values
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
Servo bucketServo;
public void runOpMode() throws InterruptedException{
public void runOpMode() throws InterruptedException {
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
spool = hardwareMap.get(DcMotor.class, "spool");
@ -54,10 +55,10 @@ public class RedSideParkingAuto extends LinearOpMode{
waitForStart();
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+350);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+350);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + 350);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + 350);
setSpeed(0.2);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -73,7 +74,7 @@ public class RedSideParkingAuto extends LinearOpMode{
sleep(4000);
driveForward(1000,.5);
driveForward(1000, .5);
setSpeed(0);
@ -99,18 +100,18 @@ public class RedSideParkingAuto extends LinearOpMode{
}
public void driveForward(int distance, double speed){
public void driveForward(int distance, double speed) {
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + encodersToTurn);
setSpeed(speed);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -119,36 +120,37 @@ public class RedSideParkingAuto extends LinearOpMode{
}
}
public void driveBackward(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
public void driveBackward(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeLeft(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
public void strafeLeft(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() - distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeRight(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
public void strafeRight(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() - distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + distance);
setSpeed(speed);
}
/*
@ -163,11 +165,12 @@ public class RedSideParkingAuto extends LinearOpMode{
}
*/
public void turnCarousel(){
public void turnCarousel() {
}
public void setSpeed(double speed){
public void setSpeed(double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
@ -182,8 +185,7 @@ public class RedSideParkingAuto extends LinearOpMode{
}
public void rotate90(){
public void rotate90() {
frontLeft.setPower(-1);
rearLeft.setPower(-1);
frontRight.setPower(1);
@ -191,7 +193,8 @@ public class RedSideParkingAuto extends LinearOpMode{
setSpeed(0);
}
public void turnRight(int degrees, double speed){
public void turnRight(int degrees, double speed) {
double turnDist = 10;
@ -208,7 +211,7 @@ public class RedSideParkingAuto extends LinearOpMode{
}
public void turnLeft(int degrees, double speed){
public void turnLeft(int degrees, double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
frontRight.setPower(speed);
@ -218,7 +221,7 @@ public class RedSideParkingAuto extends LinearOpMode{
}
public void resetMotorDirections(){
public void resetMotorDirections() {
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);

90
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/autoparking/RedSideParkingAutoDelayed.java
View file

@ -1,4 +1,3 @@
package org.firstinspires.ftc.teamcode.createdcode.oldthings.autoparking;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.GEAR_RATIO;
@ -11,22 +10,23 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@Disabled
@Autonomous (group = "parking")
public class RedSideParkingAutoDelayed extends LinearOpMode{
@Autonomous(group = "parking")
public class RedSideParkingAutoDelayed extends LinearOpMode {
//change values
private static final double TICKS_PER_INCH = (TICKS_PER_REV / GEAR_RATIO) / (WHEEL_RADIUS * 2 * Math.PI);// counts per revolution
DcMotor frontLeft, rearLeft, rearRight, frontRight, spool, intakeMotor, spinnyBoy;
Servo bucketServo;
public void runOpMode() throws InterruptedException{
public void runOpMode() throws InterruptedException {
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
frontRight = hardwareMap.get(DcMotor.class,"frontRight");
rearLeft = hardwareMap.get(DcMotor.class,"rearLeft");
rearRight = hardwareMap.get(DcMotor.class,"rearRight");
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor");
spool = hardwareMap.get(DcMotor.class, "spool");
@ -53,12 +53,12 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
waitForStart();
sleep (4000);
sleep(4000);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+350);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+350);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + 350);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + 350);
setSpeed(0.2);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -74,7 +74,7 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
sleep(4000);
driveForward(1000,.5);
driveForward(1000, .5);
setSpeed(0);
@ -100,18 +100,18 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
public void driveForward(int distance, double speed){
public void driveForward(int distance, double speed) {
int encodersToTurn = (int) Math.round(distance * TICKS_PER_INCH * 0.7);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+encodersToTurn);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + encodersToTurn);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+encodersToTurn);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + encodersToTurn);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + encodersToTurn);
setSpeed(speed);
while (frontLeft.isBusy()){
while (frontLeft.isBusy()) {
telemetry.addData("frontRightEncoder", frontRight.getCurrentPosition());
telemetry.addData("frontLeftEncoder", frontLeft.getCurrentPosition());
telemetry.addData("rearRightEncoder", rearRight.getCurrentPosition());
@ -120,36 +120,37 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
}
public void driveBackward(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
public void driveBackward(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
//motor 0 - FL, motor 1 - BL, motor 2 - FR, motor 3 - BR
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeLeft(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()-distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()+distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()+distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()-distance);
public void strafeLeft(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() - distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() + distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() + distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() - distance);
setSpeed(speed);
}
public void strafeRight(int distance, double speed){
distance = (int)(Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition()+distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition()-distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition()-distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition()+distance);
public void strafeRight(int distance, double speed) {
distance = (int) (Math.round(distance * TICKS_PER_INCH * 0.7));
frontLeft.setTargetPosition(frontLeft.getCurrentPosition() + distance);
frontRight.setTargetPosition(frontRight.getCurrentPosition() - distance);
rearLeft.setTargetPosition(rearLeft.getCurrentPosition() - distance);
rearRight.setTargetPosition(rearRight.getCurrentPosition() + distance);
setSpeed(speed);
}
/*
@ -164,11 +165,12 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
*/
public void turnCarousel(){
public void turnCarousel() {
}
public void setSpeed(double speed){
public void setSpeed(double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
@ -183,8 +185,7 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
public void rotate90(){
public void rotate90() {
frontLeft.setPower(-1);
rearLeft.setPower(-1);
frontRight.setPower(1);
@ -192,7 +193,8 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
setSpeed(0);
}
public void turnRight(int degrees, double speed){
public void turnRight(int degrees, double speed) {
double turnDist = 10;
@ -209,7 +211,7 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
public void turnLeft(int degrees, double speed){
public void turnLeft(int degrees, double speed) {
frontLeft.setPower(speed);
rearLeft.setPower(speed);
frontRight.setPower(speed);
@ -219,7 +221,7 @@ public class RedSideParkingAutoDelayed extends LinearOpMode{
}
public void resetMotorDirections(){
public void resetMotorDirections() {
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
rearLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/getServoPosition.java
View file

@ -11,6 +11,7 @@ import com.qualcomm.robotcore.hardware.Servo;
public class getServoPosition extends LinearOpMode {
private Servo armBaseServo;
private DcMotor armTurnMotor;
@Override
public void runOpMode() {
armBaseServo = hardwareMap.get(Servo.class, "armBaseServo");

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/teleops/ChassisDrive.java
View file

@ -11,8 +11,9 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
public class ChassisDrive extends OpMode {
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
private double frontRightPower, frontLeftPower, rearRightPower, rearLeftPower,
xDrive, yDrive, turn, speed;
public void loop(){
xDrive, yDrive, turn, speed;
public void loop() {
yDrive = gamepad1.left_stick_y;
xDrive = gamepad1.left_stick_x;
turn = gamepad1.right_stick_x;
@ -25,11 +26,12 @@ public class ChassisDrive extends OpMode {
rearRight.setPower(rearRightPower);
rearLeft.setPower(rearLeftPower);
telemetry.addData("Speed", "Current Speed = " + Math.round(speed*100));
telemetry.addData("Speed", "Current Speed = " + Math.round(speed * 100));
telemetry.update();
}
public void init(){
frontRight = hardwareMap.get(DcMotor.class, "frontRight") ;
public void init() {
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
@ -54,20 +56,21 @@ public class ChassisDrive extends OpMode {
speed = 1;
}
public void detectSpeedChange(){
if (gamepad1.dpad_up){
public void detectSpeedChange() {
if (gamepad1.dpad_up) {
if (speed <= 1) {
speed += 0.0005;
}
}
if (gamepad1.dpad_down){
if (gamepad1.dpad_down) {
if (speed >= 0) {
speed -= 0.0005;
}
}
}
public void calcDrive(){
public void calcDrive() {
frontRightPower = yDrive + xDrive;
frontLeftPower = yDrive - xDrive;
rearRightPower = yDrive - xDrive;

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/teleops/DriveTeleOpv1.java
View file

@ -16,23 +16,25 @@ public class DriveTeleOpv1 extends OpMode {
private DcMotor motorFR, motorFL, motorRR, motorRL;
private double powerFR, powerFL, powerRR, powerRL;
private double drive = 0, strafe = 0, turn = 0;
private double speed = 1;
private final double speed = 1;
//carousel servo things
private CRServo carouselServo;
private boolean turnCarouselRight = false, turnCarouselLeft = false;;
private double carouselPower = 0.5;
private boolean turnCarouselRight = false, turnCarouselLeft = false;
private final double carouselPower = 0.5;
//Arm Variables
private DcMotor armOne, armTwo;
private double armOnePow = 0.5, armTwoPow = 0.5;
private double armTwoPos = 0;
private double armSpeedMod = 0.5;
private final double armTwoPos = 0;
private final double armSpeedMod = 0.5;
private Servo grabServo, holdServo;
private double grabServoPow, holdServoPow, servoSpeedMod = 0.05;
private double grabServoPow;
private double holdServoPow;
private final double servoSpeedMod = 0.05;
private boolean raiseArm;
private boolean wasPressingA, wasPressingB, wasPressingX, wasPressingY;
@Override
public void loop() {
takeControllerInput();
@ -44,8 +46,8 @@ public class DriveTeleOpv1 extends OpMode {
}
private void takeControllerInput(){
drive = -1*gamepad1.left_stick_y;
private void takeControllerInput() {
drive = -1 * gamepad1.left_stick_y;
strafe = gamepad1.left_stick_x;
turn = gamepad1.right_stick_x;
@ -54,14 +56,12 @@ public class DriveTeleOpv1 extends OpMode {
turnCarouselLeft = gamepad2.x;
if (gamepad2.a) {
if (!wasPressingA) {
raiseArm = !raiseArm;
}
wasPressingA = true;
}
else wasPressingA = false;
} else wasPressingA = false;
armTwoPow = raiseArm ? 0.25 : gamepad2.right_stick_y * 0.25;
@ -71,13 +71,13 @@ public class DriveTeleOpv1 extends OpMode {
}
private void drive(){
private void drive() {
powerFR = drive - strafe;
powerFL = drive + strafe;
powerRR = drive + strafe;
powerRL = drive - strafe;
addTurn(turn);
// multiplies by speed
@ -96,7 +96,7 @@ public class DriveTeleOpv1 extends OpMode {
telemetry.update();
}
private void addTurn(double turn){
private void addTurn(double turn) {
powerFR -= turn;
powerRR -= turn;
powerFL += turn;
@ -104,22 +104,22 @@ public class DriveTeleOpv1 extends OpMode {
}
private void moveArm(){
private void moveArm() {
armOne.setPower(armOnePow * armSpeedMod);
armTwo.setPower(armTwoPow);
}
private void armGrab(){
grabServo.setPosition(grabServo.getPosition() + -1*grabServoPow*servoSpeedMod);
holdServo.setPosition(holdServo.getPosition() + -1*holdServoPow*servoSpeedMod);
private void armGrab() {
grabServo.setPosition(grabServo.getPosition() + -1 * grabServoPow * servoSpeedMod);
holdServo.setPosition(holdServo.getPosition() + -1 * holdServoPow * servoSpeedMod);
}
private void checkCarousel() {
if (turnCarouselRight)
carouselServo.setPower(carouselPower);
else if (turnCarouselLeft)
carouselServo.setPower(-1*carouselPower);
carouselServo.setPower(-1 * carouselPower);
else
carouselServo.setPower(0);
}
@ -131,7 +131,7 @@ public class DriveTeleOpv1 extends OpMode {
motorFL = hardwareMap.get(DcMotor.class, "frontLeft");
motorRR = hardwareMap.get(DcMotor.class, "rearRight");
motorRL = hardwareMap.get(DcMotor.class, "rearLeft");
motorFL.setDirection(DcMotorSimple.Direction.REVERSE);
motorRL.setDirection(DcMotorSimple.Direction.REVERSE);

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/teleops/DriveTeleOpv2.java
View file

@ -17,21 +17,23 @@ public class DriveTeleOpv2 extends OpMode {
private double speed = 1;
private boolean lockSpeed = true;
private double constantSpeedMult = 0.5;
private double constantSpeedMultChangeMult = 0.25;
private final double constantSpeedMultChangeMult = 0.25;
private boolean wasPressingDpadUp = false, wasPressingDpadDown = false;
//carousel servo things
private DcMotor carouselServo1, carouselServo2;
private boolean turnCarouselRight = false, turnCarouselLeft = false;;
private double carouselPower = 0.5;
private boolean turnCarouselRight = false, turnCarouselLeft = false;
private final double carouselPower = 0.5;
//Arm Variables
private DcMotor armOne, armTwo;
private double armPow = 0.5;
private double armTwoPos = 0;
private double armSpeedMod = 0.5;
private final double armTwoPos = 0;
private final double armSpeedMod = 0.5;
private Servo grabServo;
private double grabServoPos, holdServoPow, servoSpeedMod = 0.01;
private double grabServoPos;
private double holdServoPow;
private final double servoSpeedMod = 0.01;
private boolean raiseArm;
private boolean wasPressingA, wasPressingB, wasPressingX, wasPressingY;
@ -47,8 +49,8 @@ public class DriveTeleOpv2 extends OpMode {
telemetry.update();
}
private void takeControllerInput(){
drive = -1*gamepad1.left_stick_y;
private void takeControllerInput() {
drive = -1 * gamepad1.left_stick_y;
strafe = gamepad1.left_stick_x;
turn = gamepad1.right_stick_x;
@ -57,25 +59,22 @@ public class DriveTeleOpv2 extends OpMode {
lockSpeed = !lockSpeed;
}
wasPressingA = true;
}
else wasPressingA = false;
} else wasPressingA = false;
if (gamepad1.dpad_up) {
if (!wasPressingDpadUp) {
constantSpeedMult = Math.min(constantSpeedMult+constantSpeedMultChangeMult, 1);
constantSpeedMult = Math.min(constantSpeedMult + constantSpeedMultChangeMult, 1);
}
wasPressingDpadUp = true;
}
else wasPressingDpadUp = false;
} else wasPressingDpadUp = false;
if (gamepad1.dpad_down) {
if (!wasPressingDpadDown) {
constantSpeedMult = Math.max(constantSpeedMult-constantSpeedMultChangeMult, 0);
constantSpeedMult = Math.max(constantSpeedMult - constantSpeedMultChangeMult, 0);
}
wasPressingDpadDown = true;
}
else wasPressingDpadDown = false;
} else wasPressingDpadDown = false;
telemetry.addData("Constant Speed Mult", constantSpeedMult);
@ -86,13 +85,13 @@ public class DriveTeleOpv2 extends OpMode {
armPow = gamepad2.right_stick_y;
grabServoPos = Math.min(Math.max(grabServoPos - gamepad2.left_stick_y * servoSpeedMod, 0.33),0.6);
grabServoPos = Math.min(Math.max(grabServoPos - gamepad2.left_stick_y * servoSpeedMod, 0.33), 0.6);
}
private void drive(){
private void drive() {
powerFR = drive - strafe;
powerFL = drive + strafe;
@ -117,7 +116,7 @@ public class DriveTeleOpv2 extends OpMode {
}
private void addTurn(double turn){
private void addTurn(double turn) {
powerFR -= turn;
powerRR -= turn;
powerFL += turn;
@ -125,31 +124,26 @@ public class DriveTeleOpv2 extends OpMode {
}
private void moveArm(){
private void moveArm() {
armOne.setPower(armPow * armSpeedMod);
armTwo.setPower(armPow * armSpeedMod);
telemetry.addData("Arm One Position", armOne.getCurrentPosition());
}
private void armGrab(){
private void armGrab() {
grabServo.setPosition(grabServoPos);
telemetry.addData("Servo Position", grabServoPos);
}
private void checkCarousel() {
if (turnCarouselRight)
{
if (turnCarouselRight) {
carouselServo1.setPower(carouselPower);
carouselServo2.setPower(carouselPower);
}
else if (turnCarouselLeft)
{
} else if (turnCarouselLeft) {
carouselServo1.setPower(carouselPower);
carouselServo2.setPower(carouselPower);
}
else
{
} else {
carouselServo1.setPower(0);
carouselServo2.setPower(0);
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/teleops/MonoControllerDriveTeleOp.java
View file

@ -14,21 +14,23 @@ public class MonoControllerDriveTeleOp extends OpMode {
private DcMotor motorFR, motorFL, motorRR, motorRL;
private double powerFR, powerFL, powerRR, powerRL;
private double drive = 0, strafe = 0, turn = 0;
private double speed = 1;
private boolean lockSpeed = true;
private final double speed = 1;
private final boolean lockSpeed = true;
//carousel servo things
private CRServo carouselServo;
private boolean turnCarouselRight = false, turnCarouselLeft = false;;
private double carouselPower = 1;
private boolean turnCarouselRight = false, turnCarouselLeft = false;
private final double carouselPower = 1;
//Arm Variables
private DcMotor armOne, armTwo;
private double armPow = 0.5;
private double armTwoPos = 0;
private double armSpeedMod = 0.5;
private final double armTwoPos = 0;
private final double armSpeedMod = 0.5;
private Servo grabServo;
private double grabServoPos, holdServoPow, servoSpeedMod = 0.05;
private double grabServoPos;
private double holdServoPow;
private final double servoSpeedMod = 0.05;
private boolean raiseArm;
private boolean wasPressingA, wasPressingB, wasPressingX, wasPressingY;
@ -43,8 +45,8 @@ public class MonoControllerDriveTeleOp extends OpMode {
}
private void takeControllerInput(){
drive = -1*gamepad1.left_stick_y;
private void takeControllerInput() {
drive = -1 * gamepad1.left_stick_y;
strafe = gamepad1.left_stick_x;
turn = gamepad1.right_trigger - gamepad1.left_trigger;
@ -60,7 +62,7 @@ public class MonoControllerDriveTeleOp extends OpMode {
}
private void drive(){
private void drive() {
powerFR = drive - strafe;
powerFL = drive + strafe;
@ -85,7 +87,7 @@ public class MonoControllerDriveTeleOp extends OpMode {
telemetry.update();
}
private void addTurn(double turn){
private void addTurn(double turn) {
powerFR -= turn;
powerRR -= turn;
powerFL += turn;
@ -93,13 +95,13 @@ public class MonoControllerDriveTeleOp extends OpMode {
}
private void moveArm(){
private void moveArm() {
armOne.setPower(armPow * armSpeedMod);
armTwo.setPower(armPow * armSpeedMod);
}
private void armGrab(){
private void armGrab() {
grabServo.setPosition(grabServoPos);
}
@ -107,7 +109,7 @@ public class MonoControllerDriveTeleOp extends OpMode {
if (turnCarouselRight)
carouselServo.setPower(carouselPower);
else if (turnCarouselLeft)
carouselServo.setPower(-1*carouselPower);
carouselServo.setPower(-1 * carouselPower);
else
carouselServo.setPower(0);
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/utils/CameraTester.java
View file

@ -5,12 +5,12 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
public class CameraTester extends OpMode {
@Override
public void init(){
public void init() {
}
@Override
public void loop(){
public void loop() {
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/utils/FlexAuto.java
View file

@ -31,7 +31,7 @@ public class FlexAuto extends LinearOpMode {
waitForStart();
for(int i = 0; i < 10; i++){
for (int i = 0; i < 10; i++) {
drive.followTrajectory(traj1);
drive.followTrajectory(traj2);
drive.followTrajectory(traj3);

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/utils/ObjectDetectionTest.java
View file

@ -25,8 +25,7 @@ import java.util.List;
@Config
@TeleOp
public class ObjectDetectionTest extends LinearOpMode
{
public class ObjectDetectionTest extends LinearOpMode {
public static int minH = 100;
public static int minS = 100;
public static int minV = 100;
@ -38,8 +37,7 @@ public class ObjectDetectionTest extends LinearOpMode
FtcDashboard dashboard = FtcDashboard.getInstance();
@Override
public void runOpMode()
{
public void runOpMode() {
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
@ -51,17 +49,14 @@ public class ObjectDetectionTest extends LinearOpMode
webcam.setMillisecondsPermissionTimeout(2500); // Timeout for obtaining permission is configurable. Set before opening.
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
{
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened()
{
public void onOpened() {
webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT);
}
@Override
public void onError(int errorCode)
{
public void onError(int errorCode) {
}
});
@ -72,8 +67,7 @@ public class ObjectDetectionTest extends LinearOpMode
waitForStart();
while (opModeIsActive())
{
while (opModeIsActive()) {
dashboard.startCameraStream(webcam, 0);
telemetry.addData("Frame Count", webcam.getFrameCount());
@ -85,8 +79,7 @@ public class ObjectDetectionTest extends LinearOpMode
telemetry.update();
if(gamepad1.a)
{
if (gamepad1.a) {
webcam.stopStreaming();
@ -97,13 +90,11 @@ public class ObjectDetectionTest extends LinearOpMode
}
class SamplePipeline extends OpenCvPipeline
{
class SamplePipeline extends OpenCvPipeline {
boolean viewportPaused;
@Override
public Mat processFrame(Mat input)
{
public Mat processFrame(Mat input) {
Mat blurredImage = new Mat();
Mat hsvImage = new Mat();
Mat mask = new Mat();
@ -134,15 +125,13 @@ public class ObjectDetectionTest extends LinearOpMode
Mat output = input.clone();
List<Rect> rectList= new ArrayList<>();
List<Rect> rectList = new ArrayList<>();
// find contours
Imgproc.findContours(morphOutput, contours, hierarchy, Imgproc.RETR_CCOMP, Imgproc.CHAIN_APPROX_SIMPLE);
if (hierarchy.size().height > 0 && hierarchy.size().width > 0)
{
if (hierarchy.size().height > 0 && hierarchy.size().width > 0) {
// for each contour, display it in blue
for (int idx = 0; idx >= 0; idx = (int) hierarchy.get(0, idx)[0])
{
for (int idx = 0; idx >= 0; idx = (int) hierarchy.get(0, idx)[0]) {
Imgproc.drawContours(output, contours, idx, new Scalar(250, 0, 0));
Rect rect = Imgproc.boundingRect(contours.get(idx));
@ -174,13 +163,13 @@ public class ObjectDetectionTest extends LinearOpMode
largestRect.y + largestRect.height),
new Scalar(0, 255, 0), 4);
int centerX = largestRect.x + largestRect.width/2;
int centerY = largestRect.y + largestRect.height/2;
int centerX = largestRect.x + largestRect.width / 2;
int centerY = largestRect.y + largestRect.height / 2;
telemetry.addData("Center X", centerX);
telemetry.addData("Center Y", centerY);
if (centerX <= 75)
pos = 1;
if (Math.abs(centerX-150) < 75)
pos = 1;
if (Math.abs(centerX - 150) < 75)
pos = 2;
if (centerX >= 225)
pos = 3;
@ -189,9 +178,6 @@ public class ObjectDetectionTest extends LinearOpMode
}
if (gamepad1.y)
return mask;
if (gamepad1.b)
@ -211,16 +197,12 @@ public class ObjectDetectionTest extends LinearOpMode
}
@Override
public void onViewportTapped()
{
public void onViewportTapped() {
viewportPaused = !viewportPaused;
if(viewportPaused)
{
if (viewportPaused) {
webcam.pauseViewport();
}
else
{
} else {
webcam.resumeViewport();
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/utils/WebcamExample.java
View file

@ -37,14 +37,12 @@ import org.openftc.easyopencv.OpenCvPipeline;
import org.openftc.easyopencv.OpenCvWebcam;
@TeleOp
public class WebcamExample extends LinearOpMode
{
public class WebcamExample extends LinearOpMode {
OpenCvWebcam webcam;
FtcDashboard dashboard = FtcDashboard.getInstance();
@Override
public void runOpMode()
{
public void runOpMode() {
/*
* Instantiate an OpenCvCamera object for the camera we'll be using.
* In this sample, we're using a webcam. Note that you will need to
@ -78,11 +76,9 @@ public class WebcamExample extends LinearOpMode
* If you really want to open synchronously, the old method is still available.
*/
webcam.setMillisecondsPermissionTimeout(2500); // Timeout for obtaining permission is configurable. Set before opening.
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
{
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened()
{
public void onOpened() {
/*
* Tell the webcam to start streaming images to us! Note that you must make sure
* the resolution you specify is supported by the camera. If it is not, an exception
@ -103,8 +99,7 @@ public class WebcamExample extends LinearOpMode
}
@Override
public void onError(int errorCode)
{
public void onError(int errorCode) {
/*
* This will be called if the camera could not be opened
*/
@ -119,8 +114,7 @@ public class WebcamExample extends LinearOpMode
*/
waitForStart();
while (opModeIsActive())
{
while (opModeIsActive()) {
dashboard.startCameraStream(webcam, 0);
/*
* Send some stats to the telemetry
@ -138,8 +132,7 @@ public class WebcamExample extends LinearOpMode
* when it will be automatically stopped for you) *IS* supported. The "if" statement
* below will stop streaming from the camera when the "A" button on gamepad 1 is pressed.
*/
if(gamepad1.a)
{
if (gamepad1.a) {
/*
* IMPORTANT NOTE: calling stopStreaming() will indeed stop the stream of images
* from the camera (and, by extension, stop calling your vision pipeline). HOWEVER,
@ -187,8 +180,7 @@ public class WebcamExample extends LinearOpMode
* if you're doing something weird where you do need it synchronized with your OpMode thread,
* then you will need to account for that accordingly.
*/
class SamplePipeline extends OpenCvPipeline
{
class SamplePipeline extends OpenCvPipeline {
boolean viewportPaused;
/*
@ -201,8 +193,7 @@ public class WebcamExample extends LinearOpMode
*/
@Override
public Mat processFrame(Mat input)
{
public Mat processFrame(Mat input) {
/*
* IMPORTANT NOTE: the input Mat that is passed in as a parameter to this method
* will only dereference to the same image for the duration of this particular
@ -217,11 +208,11 @@ public class WebcamExample extends LinearOpMode
Imgproc.rectangle(
input,
new Point(
input.cols()/4,
input.rows()/4),
input.cols() / 4,
input.rows() / 4),
new Point(
input.cols()*(3f/4f),
input.rows()*(3f/4f)),
input.cols() * (3f / 4f),
input.rows() * (3f / 4f)),
new Scalar(0, 255, 0), 4);
/**
@ -234,8 +225,7 @@ public class WebcamExample extends LinearOpMode
}
@Override
public void onViewportTapped()
{
public void onViewportTapped() {
/*
* The viewport (if one was specified in the constructor) can also be dynamically "paused"
* and "resumed". The primary use case of this is to reduce CPU, memory, and power load
@ -250,12 +240,9 @@ public class WebcamExample extends LinearOpMode
viewportPaused = !viewportPaused;
if(viewportPaused)
{
if (viewportPaused) {
webcam.pauseViewport();
}
else
{
} else {
webcam.resumeViewport();
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/utils/encoderTester.java
View file

@ -21,7 +21,7 @@ public class encoderTester extends OpMode {
}
@Override
public void init(){
public void init() {
arm1 = hardwareMap.get(DcMotor.class, "armOne");
arm2 = hardwareMap.get(DcMotor.class, "armTwo");
arm2.setDirection(DcMotorSimple.Direction.REVERSE);

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
View file

@ -62,18 +62,21 @@ public class SampleMecanumDrive extends MecanumDrive {
public static double VY_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
private TrajectorySequenceRunner trajectorySequenceRunner;
private final TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
private TrajectoryFollower follower;
private final TrajectoryFollower follower;
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
private List<DcMotorEx> motors;
private final DcMotorEx leftFront;
private final DcMotorEx leftRear;
private final DcMotorEx rightRear;
private final DcMotorEx rightFront;
private final List<DcMotorEx> motors;
private BNO055IMU imu;
private VoltageSensor batteryVoltageSensor;
private final BNO055IMU imu;
private final VoltageSensor batteryVoltageSensor;
public SampleMecanumDrive(HardwareMap hardwareMap) {
super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
View file

@ -58,17 +58,19 @@ public class SampleTankDrive extends TankDrive {
public static double VX_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
private TrajectorySequenceRunner trajectorySequenceRunner;
private final TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
private TrajectoryFollower follower;
private final TrajectoryFollower follower;
private List<DcMotorEx> motors, leftMotors, rightMotors;
private BNO055IMU imu;
private final List<DcMotorEx> motors;
private final List<DcMotorEx> leftMotors;
private final List<DcMotorEx> rightMotors;
private final BNO055IMU imu;
private VoltageSensor batteryVoltageSensor;
private final VoltageSensor batteryVoltageSensor;
public SampleTankDrive(HardwareMap hardwareMap) {
super(kV, kA, kStatic, TRACK_WIDTH);

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
View file

@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import java.util.Arrays;
@ -34,7 +35,9 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
private Encoder leftEncoder, rightEncoder, frontEncoder;
private final Encoder leftEncoder;
private final Encoder rightEncoder;
private final Encoder frontEncoder;
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
super(Arrays.asList(

129
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
View file

@ -2,65 +2,63 @@ file## TeamCode Module
Welcome!
This module, TeamCode, is the place where you will write/paste the code for your team's
robot controller App. This module is currently empty (a clean slate) but the
process for adding OpModes is straightforward.
This module, TeamCode, is the place where you will write/paste the code for your team's robot
controller App. This module is currently empty (a clean slate) but the process for adding OpModes is
straightforward.
## Creating your own OpModes
The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
Sample opmodes exist in the FtcRobotController module.
To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
Sample opmodes exist in the FtcRobotController module. To locate these samples, find the
FtcRobotController module in the "Project/Android" tab.
Expand the following tree elements:
FtcRobotController / java / org.firstinspires.ftc.robotcontroller / external / samples
FtcRobotController / java / org.firstinspires.ftc.robotcontroller / external / samples
A range of different samples classes can be seen in this folder.
The class names follow a naming convention which indicates the purpose of each class.
The full description of this convention is found in the samples/sample_convention.md file.
A range of different samples classes can be seen in this folder. The class names follow a naming
convention which indicates the purpose of each class. The full description of this convention is
found in the samples/sample_convention.md file.
A brief synopsis of the naming convention is given here:
The prefix of the name will be one of the following:
* Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
of a particular style of OpMode. These are bare bones examples.
* Sensor: This is a Sample OpMode that shows how to use a specific sensor.
It is not intended as a functioning robot, it is simply showing the minimal code
required to read and display the sensor values.
* Hardware: This is not an actual OpMode, but a helper class that is used to describe
one particular robot's hardware devices: eg: for a Pushbot. Look at any
Pushbot sample to see how this can be used in an OpMode.
Teams can copy one of these to create their own robot definition.
* Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure of a
particular style of OpMode. These are bare bones examples.
* Sensor: This is a Sample OpMode that shows how to use a specific sensor. It is not intended as a
functioning robot, it is simply showing the minimal code required to read and display the sensor
values.
* Hardware: This is not an actual OpMode, but a helper class that is used to describe one particular
robot's hardware devices: eg: for a Pushbot. Look at any Pushbot sample to see how this can be
used in an OpMode. Teams can copy one of these to create their own robot definition.
* Pushbot: This is a Sample OpMode that uses the Pushbot robot structure as a base.
* Concept: This is a sample OpMode that illustrates performing a specific function or concept.
These may be complex, but their operation should be explained clearly in the comments,
or the header should reference an external doc, guide or tutorial.
* Library: This is a class, or set of classes used to implement some strategy.
These will typically NOT implement a full OpMode. Instead they will be included
by an OpMode to provide some stand-alone capability.
* Concept: This is a sample OpMode that illustrates performing a specific function or concept.
These may be complex, but their operation should be explained clearly in the comments, or the
header should reference an external doc, guide or tutorial.
* Library: This is a class, or set of classes used to implement some strategy. These will typically
NOT implement a full OpMode. Instead they will be included by an OpMode to provide some
stand-alone capability.
Once you are familiar with the range of samples available, you can choose one to be the
basis for your own robot. In all cases, the desired sample(s) needs to be copied into
your TeamCode module to be used.
Once you are familiar with the range of samples available, you can choose one to be the basis for
your own robot. In all cases, the desired sample(s) needs to be copied into your TeamCode module to
be used.
This is done inside Android Studio directly, using the following steps:
1) Locate the desired sample class in the Project/Android tree.
1) Locate the desired sample class in the Project/Android tree.
2) Right click on the sample class and select "Copy"
2) Right click on the sample class and select "Copy"
3) Expand the TeamCode / java folder
3) Expand the TeamCode / java folder
4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
5) You will be prompted for a class name for the copy.
Choose something meaningful based on the purpose of this class.
Start with a capital letter, and remember that there may be more similar classes later.
5) You will be prompted for a class name for the copy. Choose something meaningful based on the
purpose of this class. Start with a capital letter, and remember that there may be more similar
classes later.
Once your copy has been created, you should prepare it for use on your robot.
This is done by adjusting the OpMode's name, and enabling it to be displayed on the
Driver Station's OpMode list.
Once your copy has been created, you should prepare it for use on your robot. This is done by
adjusting the OpMode's name, and enabling it to be displayed on the Driver Station's OpMode list.
Each OpMode sample class begins with several lines of code like the ones shown below:
@ -70,52 +68,49 @@ Each OpMode sample class begins with several lines of code like the ones shown b
```
The name that will appear on the driver station's "opmode list" is defined by the code:
``name="Template: Linear OpMode"``
You can change what appears between the quotes to better describe your opmode.
The "group=" portion of the code can be used to help organize your list of OpModes.
``name="Template: Linear OpMode"``
You can change what appears between the quotes to better describe your opmode. The "group=" portion
of the code can be used to help organize your list of OpModes.
As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
``@Disabled`` annotation which has been included.
This line can simply be deleted , or commented out, to make the OpMode visible.
``@Disabled`` annotation which has been included. This line can simply be deleted , or commented
out, to make the OpMode visible.
## ADVANCED Multi-Team App management: Cloning the TeamCode Module
In some situations, you have multiple teams in your club and you want them to all share
a common code organization, with each being able to *see* the others code but each having
their own team module with their own code that they maintain themselves.
In some situations, you have multiple teams in your club and you want them to all share a common
code organization, with each being able to *see* the others code but each having their own team
module with their own code that they maintain themselves.
In this situation, you might wish to clone the TeamCode module, once for each of these teams.
Each of the clones would then appear along side each other in the Android Studio module list,
together with the FtcRobotController module (and the original TeamCode module).
In this situation, you might wish to clone the TeamCode module, once for each of these teams. Each
of the clones would then appear along side each other in the Android Studio module list, together
with the FtcRobotController module (and the original TeamCode module).
Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
prior to clicking to the green Run arrow.
Warning: This is not for the inexperienced Software developer.
You will need to be comfortable with File manipulations and managing Android Studio Modules.
These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
Warning: This is not for the inexperienced Software developer. You will need to be comfortable with
File manipulations and managing Android Studio Modules. These changes are performed OUTSIDE of
Android Studios, so close Android Studios before you do this.
Also.. Make a full project backup before you start this :)
To clone TeamCode, do the following:
Note: Some names start with "Team" and others start with "team". This is intentional.
Note: Some names start with "Team" and others start with "team". This is intentional.
1) Using your operating system file management tools, copy the whole "TeamCode"
folder to a sibling folder with a corresponding new name, eg: "Team0417".
1) Using your operating system file management tools, copy the whole "TeamCode"
folder to a sibling folder with a corresponding new name, eg: "Team0417".
2) In the new Team0417 folder, delete the TeamCode.iml file.
2) In the new Team0417 folder, delete the TeamCode.iml file.
3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
to a matching name with a lowercase 'team' eg: "team0417".
3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder to a
matching name with a lowercase 'team' eg: "team0417".
4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
package="org.firstinspires.ftc.teamcode"
to be
package="org.firstinspires.ftc.team0417"
4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that
contains package="org.firstinspires.ftc.teamcode"
to be package="org.firstinspires.ftc.team0417"
5) Add: include ':Team0417' to the "/settings.gradle" file.
6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
5) Add: include ':Team0417' to the "/settings.gradle" file.
6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
View file

@ -1,4 +1,5 @@
package org.firstinspires.ftc.teamcode.trajectorysequence;
public class EmptySequenceException extends RuntimeException { }
public class EmptySequenceException extends RuntimeException {
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
View file

@ -35,7 +35,8 @@ public class AssetsTrajectoryManager {
/**
* Loads a trajectory config with the given name.
*/
public static @Nullable TrajectoryConfig loadConfig(String name) {
public static @Nullable
TrajectoryConfig loadConfig(String name) {
try {
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
"trajectory/" + name + ".yaml");
@ -48,7 +49,8 @@ public class AssetsTrajectoryManager {
/**
* Loads a trajectory builder with the given name.
*/
public static @Nullable TrajectoryBuilder loadBuilder(String name) {
public static @Nullable
TrajectoryBuilder loadBuilder(String name) {
TrajectoryGroupConfig groupConfig = loadGroupConfig();
TrajectoryConfig config = loadConfig(name);
if (groupConfig == null || config == null) {
@ -60,7 +62,8 @@ public class AssetsTrajectoryManager {
/**
* Loads a trajectory with the given name.
*/
public static @Nullable Trajectory load(String name) {
public static @Nullable
Trajectory load(String name) {
TrajectoryBuilder builder = loadBuilder(name);
if (builder == null) {
return null;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055IMUUtil.java
View file

@ -21,13 +21,13 @@ public class BNO055IMUUtil {
* Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
* Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
* mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
*
* <p>
* Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
* Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
*
* <p>
* NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
*
* @param imu IMU
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/
@ -82,7 +82,7 @@ public class BNO055IMUUtil {
* Remaps the IMU coordinate system so that the remapped +Z faces the provided
* {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
*
* @param imu IMU
* @param imu IMU
* @param direction axis direction
*/
public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
@ -112,7 +112,7 @@ public class BNO055IMUUtil {
* Now deprecated due to unintuitive parameter order.
* Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
*
* @param imu IMU
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
View file

@ -23,7 +23,7 @@ public class Encoder {
FORWARD(1),
REVERSE(-1);
private int multiplier;
private final int multiplier;
Direction(int multiplier) {
this.multiplier = multiplier;
@ -34,8 +34,8 @@ public class Encoder {
}
}
private DcMotorEx motor;
private NanoClock clock;
private final DcMotorEx motor;
private final NanoClock clock;
private Direction direction;
@ -68,6 +68,7 @@ public class Encoder {
/**
* Allows you to set the direction of the counts and velocity without modifying the motor's direction state
*
* @param direction either reverse or forward depending on if encoder counts should be negated
*/
public void setDirection(Direction direction) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
View file

@ -33,7 +33,7 @@ public class LoggingUtil {
Long.compare(lhs.lastModified(), rhs.lastModified()));
long dirSize = 0;
for (File file: logFiles) {
for (File file : logFiles) {
dirSize += file.length();
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
View file

@ -63,6 +63,7 @@ public class LynxModuleUtil {
/**
* Retrieve and parse Lynx module firmware version.
*
* @param module Lynx module
* @return parsed firmware version
*/
@ -96,6 +97,7 @@ public class LynxModuleUtil {
/**
* Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
*
* @param hardwareMap hardware map containing Lynx modules
*/
public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
View file

@ -55,7 +55,7 @@ public class RegressionUtil {
for (int i = 1; i < x.size() - 1; i++) {
deriv.add(
(y.get(i + 1) - y.get(i - 1)) /
(x.get(i + 1) - x.get(i - 1))
(x.get(i + 1) - x.get(i - 1))
);
}
// copy endpoints to pad output
@ -66,17 +66,17 @@ public class RegressionUtil {
/**
* Run regression to compute velocity and static feedforward from ramp test data.
*
* <p>
* Here's the general procedure for gathering the requisite data:
* 1. Slowly ramp the motor power/voltage and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
* (kV) and an optional intercept (kStatic).
* 1. Slowly ramp the motor power/voltage and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
* (kV) and an optional intercept (kStatic).
*
* @param timeSamples time samples
* @param timeSamples time samples
* @param positionSamples position samples
* @param powerSamples power samples
* @param fitStatic fit kStatic
* @param file log file
* @param powerSamples power samples
* @param fitStatic fit kStatic
* @param file log file
*/
public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
List<Double> powerSamples, boolean fitStatic,
@ -106,17 +106,17 @@ public class RegressionUtil {
}
return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
rampReg.getRSquare());
rampReg.getRSquare());
}
/**
* Run regression to compute acceleration feedforward.
*
* @param timeSamples time samples
* @param timeSamples time samples
* @param positionSamples position samples
* @param powerSamples power samples
* @param rampResult ramp result
* @param file log file
* @param powerSamples power samples
* @param rampResult ramp result
* @param file log file
*/
public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
List<Double> powerSamples, RampResult rampResult,