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

Degrees should be radians

Browse source
This commit is contained in:
Yash Karandikar 2022-11-07 16:29:03 -06:00
parent e637302696
commit ddfc36c657
12 changed files with 10 additions and 568 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -1,91 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Disabled
@Autonomous
@Config
public class AutoBase extends EnhancedAutoMode {
public static double START_POS_X = 0;
public static double START_POS_Y = 0;
public static double START_POS_ANGLE = 0;
public static int START_METHOD = 20;
public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0;
public static int SHIPPING_ELEMENT_POS_1_X = 0;
public static int SHIPPING_ELEMENT_POS_2_X = 0;
public static int SHIPPING_ELEMENT_POS_3_X = 0;
FtcDashboard dashboard = FtcDashboard.getInstance();
Telemetry dashboardTelemetry = dashboard.getTelemetry();
public static double[] xCoordArr = {};
private static double[] yCoordArr = {};
private static double[] angleArr = {};
private static int[] methodIdArr = {};
public static double[] xCoordArrCase1 = {START_POS_X};
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 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 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() {
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();
if (pos == 1) {
xCoordArr = xCoordArrCase1;
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;
methodIdArr = methodIdArrCase3;
}
initThings();
setXCoords(xCoordArr);
setYCoords(yCoordArr);
setAngles(angleArr);
setMethodIDS(methodIdArr);
try {
makeActionObjects();
} catch (Exception e) {
e.printStackTrace();
}
waitForStart();
run();
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java
View file

@ -24,7 +24,7 @@ public class AutoBlueColorSensor extends LinearOpMode {
Trajectory firstMove = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, 60), 270),
new Pose2d(new Vector2d(-34, 60), Math.toRadians(270)),
new int[]{-34},
new int[]{40}
);
@ -39,7 +39,7 @@ public class AutoBlueColorSensor extends LinearOpMode {
int largest = api.getLargest(r, g, b);
Trajectory endpos;
if (largest == r) {
if (largest == g) {
// move to position 1
endpos = api.makeTrajectories(
mecanumDrive,
@ -47,7 +47,7 @@ public class AutoBlueColorSensor extends LinearOpMode {
new int[]{-34, -12, -12},
new int[]{60, 60, 12}
);
} else if (largest == g) {
} else if (largest == r) {
// move to position 2
endpos = api.makeTrajectories(
mecanumDrive,

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehavior.java
View file

@ -24,7 +24,7 @@ public class AutoBlueSubstationParkingBehavior extends LinearOpMode {
// init move to default terminal
Trajectory endpos = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, 60), 270),
new Pose2d(new Vector2d(-34, 60), Math.toRadians(270)),
new int[]{-34, 1},
new int[]{60, 60}
);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueTerminalParkingBehavior.java
View file

@ -24,7 +24,7 @@ public class AutoBlueTerminalParkingBehavior extends LinearOpMode {
// init move to default terminal
Trajectory endpos = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, 60), 270),
new Pose2d(new Vector2d(-34, 60), Math.toRadians(270)),
new int[]{-34, -70},
new int[]{60, 60}
);

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java
View file

@ -25,7 +25,7 @@ public class AutoRedColorSensor extends LinearOpMode {
Trajectory firstMove = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, -60), 90),
new Pose2d(new Vector2d(-34, -60), Math.toRadians(90)),
new int[]{-34},
new int[]{-40}
);
@ -40,7 +40,7 @@ public class AutoRedColorSensor extends LinearOpMode {
int largest = api.getLargest(r, g, b);
Trajectory endpos;
if (largest == r) {
if (largest == g) {
// move to position 1
endpos = api.makeTrajectories(
mecanumDrive,
@ -48,7 +48,7 @@ public class AutoRedColorSensor extends LinearOpMode {
new int[]{-34, -12, -12},
new int[]{-60, -60, -12}
);
} else if (largest == g) {
} else if (largest == r) {
// move to position 2
endpos = api.makeTrajectories(
mecanumDrive,

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehavior.java
View file

@ -24,7 +24,7 @@ public class AutoRedSubstationParkingBehavior extends LinearOpMode {
// init move to default terminal
Trajectory endpos = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, -60), 90),
new Pose2d(new Vector2d(-34, -60), Math.toRadians(90)),
new int[]{-34, 1},
new int[]{-60, -60}
);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedTerminalParkingBehavior.java
View file

@ -23,7 +23,7 @@ public class AutoRedTerminalParkingBehavior extends LinearOpMode {
// init move to default terminal
Trajectory endpos = api.makeTrajectories(
mecanumDrive,
new Pose2d(new Vector2d(-34, -60), 90),
new Pose2d(new Vector2d(-34, -60), Math.toRadians(90)),
new int[]{-34, -70},
new int[]{-60, -60}
);

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

@ -1,96 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous(group = "Blue")
@Config
public class BlueNearParkAuto extends EnhancedAutoMode {
public static double START_POS_X = -32;
public static double START_POS_Y = 66;
public static double START_POS_ANGLE = 270;
public static int START_METHOD = 20;
public static int BLOCK_DROP_POINT = -30;
public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 135;
public static int SHIPPING_ELEMENT_POS_1_X = 30;
public static int SHIPPING_ELEMENT_POS_2_X = 165;
public static int SHIPPING_ELEMENT_POS_3_X = 285;
FtcDashboard dashboard = FtcDashboard.getInstance();
Telemetry dashboardTelemetry = dashboard.getTelemetry();
public static double[] xCoordArr = {};
private static final double[] yCoordArr = {};
private static final double[] angleArr = {};
private static final int[] methodIdArr = {};
/*
private static final int[] methodIdArrCase1 = {START_METHOD, 13, 0, 22, 10, 32, 0};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 0, 22, 10, 32, 0};
private static final int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 32, 0};
*/
@Override
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();
/*
if (pos == 1) {
xCoordArr = xCoordArrCase1;
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;
methodIdArr = methodIdArrCase3;
}
*/
initThings();
setXCoords(xCoordArr);
setYCoords(yCoordArr);
setAngles(angleArr);
setMethodIDS(methodIdArr);
try {
makeActionObjects();
} catch (Exception e) {
e.printStackTrace();
}
waitForStart();
run();
}
}

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

@ -1,91 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous(group = "Blue")
@Config
public class BlueWarehouseSideAuto extends EnhancedAutoMode {
public static double START_POS_X = 16;
public static double START_POS_Y = 66;
public static double START_POS_ANGLE = -90;
public static int START_METHOD = 20;
//TODO: Get vals
public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0;
public static int SHIPPING_ELEMENT_POS_1_X = 0;
public static int SHIPPING_ELEMENT_POS_2_X = 0;
public static int SHIPPING_ELEMENT_POS_3_X = 0;
FtcDashboard dashboard = FtcDashboard.getInstance();
Telemetry dashboardTelemetry = dashboard.getTelemetry();
public static double[] xCoordArr = {};
private static double[] yCoordArr = {};
private static double[] angleArr = {};
private static int[] methodIdArr = {};
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 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 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() {
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();
if (pos == 1) {
xCoordArr = xCoordArrCase1;
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;
methodIdArr = methodIdArrCase3;
}
initThings();
setXCoords(xCoordArr);
setYCoords(yCoordArr);
setAngles(angleArr);
setMethodIDS(methodIdArr);
try {
makeActionObjects();
} catch (Exception e) {
e.printStackTrace();
}
waitForStart();
run();
}
}

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

@ -1,94 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.createdcode.driveobjs.ActionObject;
import org.firstinspires.ftc.teamcode.createdcode.driveobjs.ObjectDetector;
import java.util.ArrayList;
import java.util.List;
public abstract class EnhancedAutoMode extends LinearOpMode {
private static List<Double> xCoords = new ArrayList<>(0);
private static List<Double> yCoords = new ArrayList<>(0);
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 EnhancedDriver enhancedDriver;
private final FtcDashboard dashboard = FtcDashboard.getInstance();
private final Telemetry dashboardTelemetry = dashboard.getTelemetry();
public void makeActionObjects() throws Exception {
actionObjects = new ArrayList<>(0);
if (xCoords.get(0) == null || yCoords.get(0) == null || angles.get(0) == null)
throw invalidValues;
if (!(xCoords.size() == yCoords.size() && xCoords.size() == angles.size() && xCoords.size() == methodIDs.size()))
throw invalidValues;
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));
actionObjects.add(newAction);
telemetry.addData("Amount of ActionObjects", actionObjects.size());
telemetry.update();
}
}
public void run() {
int count = 0;
for (ActionObject i : actionObjects) {
//enhancedDriver.act(i);
count++;
telemetry.addData("Completed Action:", count);
telemetry.update();
}
}
public double checkNull(List<Double> toCheck, int i) {
return toCheck.get(i) == null ? checkNull(toCheck, i) : toCheck.get(i);
}
public void initThings() {
//enhancedDriver = new EnhancedDriver(hardwareMap);
}
public void setXCoords(double[] xArr) {
xCoords = new ArrayList<>(0);
for (Double i : xArr)
xCoords.add(i);
}
public void setYCoords(double[] yArr) {
yCoords = new ArrayList<>(0);
for (Double i : yArr)
yCoords.add(i);
}
public void setAngles(double[] angleArr) {
angles = new ArrayList<>(0);
for (Double i : angleArr)
angles.add(i);
}
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) {
ObjectDetector detector = new ObjectDetector(hardwareMap, h, pos1, pos2, pos3);
int pos = detector.getPos();
detector.endStream();
return pos;
}
}

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

@ -1,95 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous(group = "Red")
@Config
public class RedNearParkAuto extends EnhancedAutoMode {
public static double START_POS_X = -40;
public static double START_POS_Y = -66;
public static double START_POS_ANGLE = 90;
public static int START_METHOD = 20;
public static int BLOCK_DROP_POINT = -30;
//TODO: GET THESE VALUES
public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0;
public static int SHIPPING_ELEMENT_POS_1_X = 0;
public static int SHIPPING_ELEMENT_POS_2_X = 0;
public static int SHIPPING_ELEMENT_POS_3_X = 0;
public static double[] xCoordArr = {};
private static final double[] yCoordArr = {};
private static final double[] angleArr = {};
private static final int[] methodIdArr = {};
/*
private static final int[] methodIdArrCase1 = {START_METHOD, 11, 22, 10, 31, 0};
private static final int[] methodIdArrCase2 = {START_METHOD, 12, 0, 22, 10, 31, 0};
private static final int[] methodIdArrCase3 = {START_METHOD, 13, 0, 22, 10, 31, 0};
*/
FtcDashboard dashboard = FtcDashboard.getInstance();
Telemetry dashboardTelemetry = dashboard.getTelemetry();
@Override
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();
/*
if (pos == 1) {
xCoordArr = xCoordArrCase1;
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;
methodIdArr = methodIdArrCase3;
}
*/
initThings();
setXCoords(xCoordArr);
setYCoords(yCoordArr);
setAngles(angleArr);
setMethodIDS(methodIdArr);
try {
makeActionObjects();
} catch (Exception e) {
e.printStackTrace();
}
waitForStart();
run();
}
}

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

@ -1,91 +0,0 @@
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Autonomous(group = "Red")
@Config
public class RedWarehouseSideAuto extends EnhancedAutoMode {
public static double START_POS_X = 8;
public static double START_POS_Y = -66;
public static double START_POS_ANGLE = -90;
public static int START_METHOD = 20;
//TODO: Get vals
public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0;
public static int SHIPPING_ELEMENT_POS_1_X = 0;
public static int SHIPPING_ELEMENT_POS_2_X = 0;
public static int SHIPPING_ELEMENT_POS_3_X = 0;
FtcDashboard dashboard = FtcDashboard.getInstance();
Telemetry dashboardTelemetry = dashboard.getTelemetry();
public static double[] xCoordArr = {};
private static double[] yCoordArr = {};
private static double[] angleArr = {};
private static int[] methodIdArr = {};
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 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 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() {
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();
if (pos == 1) {
xCoordArr = xCoordArrCase1;
yCoordArr = yCoordArrCase1;
angleArr = angleArrCase1;
methodIdArr = methodIdArrCase1;
} else if (pos == 2) {
xCoordArr = xCoordArrCase2;
yCoordArr = yCoordArrCase2;
angleArr = angleArrCase2;
methodIdArr = methodIdArrCase2;
} else if (pos == 3) {
xCoordArr = xCoordArrCase3;
yCoordArr = yCoordArrCase3;
angleArr = angleArrCase3;
methodIdArr = methodIdArrCase3;
}
initThings();
setXCoords(xCoordArr);
setYCoords(yCoordArr);
setAngles(angleArr);
setMethodIDS(methodIdArr);
try {
makeActionObjects();
} catch (Exception e) {
e.printStackTrace();
}
waitForStart();
run();
}
}