KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

yES

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-30 18:16:08 -06:00
parent e04c5fa830
commit 03ae41c19b
9 changed files with 152 additions and 1019 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -1,10 +1,14 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
@@ -17,11 +21,18 @@ public class TeleopV2 extends LinearOpMode {
boolean intake = false;
boolean reject = false;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
private double vel = 3000;
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
List<Double> s1 = new ArrayList<>();
List<Double> s2 = new ArrayList<>();
List<Double> s3 = new ArrayList<>();
List<Double> s1G = new ArrayList<>();
List<Double> s2G = new ArrayList<>();
List<Double> s3G = new ArrayList<>();
List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
@Override
public void runOpMode() throws InterruptedException {
@@ -64,21 +75,123 @@ public class TeleopV2 extends LinearOpMode {
if (intake) {
robot.intake.setPower(1);
double position;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.015;
} else {
position = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
} else if (reject) {
robot.intake.setPower(-1);
double position = spindexer_intakePos1;
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
} else {
robot.intake.setPower(0);
}
//COLOR:
double s1D = robot.
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s1G.add(gP);
if (gP >= 0.43) {
s1.add(true);
}
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s2G.add(gP);
if (gP >= 0.43) {
s2.add(true);
}
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s3G.add(gP);
if (gP >= 0.43) {
s3.add(true);
}
}
boolean green1 = s1.get(s1.size() - 1);
boolean green2 = s2.get(s2.size() - 1);
boolean green3 = s3.get(s3.size() - 1);
//SHOOTER:
double kF = 1.0 / MAX_RPM; // baseline feedforward
double encoderRevolutions = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
double velPID;
// --- FEEDFORWARD BASE POWER ---
double feed = kF * vel; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION ---
double error = vel - velocity;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
velPID = feed + correction;
// clamp to allowed range
velPID = Math.max(0, Math.min(1, velPID));
robot.shooter1.setPower(velPID);
robot.shooter2.setPower(velPID);
//TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
//MISC:
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
TELE.addData("Spin1Green", s1.get(s1.size() - 1));
TELE.addData("Spin2Green", s2.get(s2.size() - 1));
TELE.addData("Spin3Green", s3.get(s3.size() - 1));
TELE.update();
}

238
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/AprilTag.java
View File

@@ -1,238 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class AprilTag implements Subsystem {
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
private MultipleTelemetry TELE;
private boolean teleOn = false;
private int detections = 0;
List<AprilTagDetection> currentDetections;
ArrayList<ArrayList<Double>> Data = new ArrayList<>();
public AprilTag(Robot robot, MultipleTelemetry tele) {
this.aprilTag = robot.aprilTagProcessor;
this.TELE = tele;
}
@Override
public void update() {
currentDetections = aprilTag.getDetections();
UpdateData();
if(teleOn){
tagTELE();
initTelemetry();
}
}
public void initTelemetry (){
TELE.addData("Camera Preview", "Check Driver Station for stream");
TELE.addData("Status", "Initialized - Press START");
}
public void tagTELE () {
TELE.addData("# AprilTags Detected", detections);
// Display info for each detected tag
for (ArrayList<Double> detection : Data) {
if (detection.get(0) ==1) {
// Known AprilTag with metadata
TELE.addLine(String.format("\n==== (ID %d) %s ====",
detection.get(1).intValue(), ""));
TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)",
detection.get(2),
detection.get(3),
detection.get(4)));
TELE.addData("Distance", getDistance(detection.get(1).intValue()));
TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)",
detection.get(5),
detection.get(6),
detection.get(7)));
TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)",
detection.get(8),
detection.get(9),
detection.get(10)));
}
}
}
public void turnTelemetryOn(boolean bool) {
teleOn = bool;
}
public void UpdateData () {
Data.clear(); // <--- THIS FIXES YOUR ISSUE
detections = currentDetections.size();
for (AprilTagDetection detection : currentDetections) {
ArrayList<Double> detectionData = new ArrayList<Double>();
if (detection.metadata != null) {
detectionData.add(1.0);
// Known AprilTag with metadata
detectionData.add( (double) detection.id);
detectionData.add(detection.ftcPose.x);
detectionData.add(detection.ftcPose.y);
detectionData.add(detection.ftcPose.z);
detectionData.add(detection.ftcPose.pitch);
detectionData.add(detection.ftcPose.roll);
detectionData.add(detection.ftcPose.yaw);
detectionData.add(detection.ftcPose.range);
detectionData.add(detection.ftcPose.bearing);
detectionData.add(detection.ftcPose.elevation);
} else {
detectionData.add(0, 0.0);
}
Data.add(detectionData);
}
}
public int getDetectionCount() {
return detections;
}
public boolean isDetected (int id){
return (!filterID(Data, (double) id ).isEmpty());
}
public double getDistance(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 5) {
double x = d.get(2);
double y = d.get(3);
double z = d.get(4);
return Math.sqrt(x*x + y*y + z*z);
}
return -1; // tag not found
}
// Returns the position as [x, y, z]
public List<Double> getPosition(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 5) {
List<Double> pos = new ArrayList<>();
pos.add(d.get(2));
pos.add(d.get(3));
pos.add(d.get(4));
return pos;
}
return Collections.emptyList();
}
// Returns orientation as [pitch, roll, yaw]
public List<Double> getOrientation(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 8) {
List<Double> ori = new ArrayList<>();
ori.add(d.get(5));
ori.add(d.get(6));
ori.add(d.get(7));
return ori;
}
return Collections.emptyList();
}
// Returns range, bearing, elevation as [range, bearing, elevation]
public List<Double> getRBE(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 11) {
List<Double> rbe = new ArrayList<>();
rbe.add(d.get(8));
rbe.add(d.get(9));
rbe.add(d.get(10));
return rbe;
}
return Collections.emptyList();
}
// Returns full raw data for debugging or custom processing
public ArrayList<Double> getRawData(int id) {
return filterID(Data, (double) id);
}
public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
for (ArrayList<Double> innerList : data) {
// Ensure it has a second element
if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
return innerList; // Return the first match
}
}
// Return an empty ArrayList if no match found
return new ArrayList<>();
}
}

99
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Drivetrain.java
View File

@@ -1,99 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import java.util.Objects;
public class Drivetrain implements Subsystem {
private final GamepadEx gamepad;
public MultipleTelemetry TELE;
private String Mode = "Default";
private final DcMotorEx fl;
private final DcMotorEx fr;
private final DcMotorEx bl;
private final DcMotorEx br;
private double defaultSpeed = 0.7;
private double slowSpeed = 0.3;
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
this.fl = robot.frontLeft;
this.fr = robot.frontRight;
this.br = robot.backRight;
this.bl = robot.backLeft;
this.gamepad = gamepad1;
this.TELE = tele;
}
public void setMode (String mode){
this.Mode = mode;
}
public void setDefaultSpeed (double speed){
this.defaultSpeed = speed;
}
public void setSlowSpeed (double speed){
this.slowSpeed = speed;
}
public void RobotCentric(double fwd, double strafe, double turn, double turbo){
double y = -fwd; // Remember, Y stick value is reversed
double x = strafe * 1.1; // Counteract imperfect strafing
double rx = turn;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
fl.setPower(frontLeftPower*turbo);
bl.setPower(backLeftPower*turbo);
fr.setPower(frontRightPower*turbo);
br.setPower(backRightPower*turbo);
}
@Override
public void update() {
if (Objects.equals(Mode, "Default")) {
RobotCentric(
gamepad.getRightY(),
gamepad.getRightX(),
gamepad.getLeftX(),
(gamepad.getTrigger(
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed)
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
+ defaultSpeed
)
);
}
}
}

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Intake.java
View File

@@ -1,80 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Intake implements Subsystem {
private GamepadEx gamepad;
public MultipleTelemetry TELE;
private DcMotorEx intake;
private double intakePower = 1.0;
private int intakeState = 0;
public Intake (Robot robot){
this.intake = robot.intake;
}
public int getIntakeState() {
return intakeState;
}
public void toggle(){
if (intakeState !=0){
intakeState = 0;
} else {
intakeState = 1;
}
}
public void intakeMinPower(){
intakeState = 2;
}
public void intake(){
intakeState =1;
}
public void reverse(){
intakeState =-1;
}
public void stop(){
intakeState =0;
}
@Override
public void update() {
if (intakeState == 1){
intake.setPower(intakePower);
} else if (intakeState == -1){
intake.setPower(-intakePower);
} else if (intakeState == 2){
intake.setPower(intakePower);
}else {
intake.setPower(0);
}
}
}

249
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Shooter.java
View File

@@ -1,249 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.PIDCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.constants.Poses;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.Objects;
public class Shooter implements Subsystem {
private final DcMotorEx fly1;
private final DcMotorEx fly2;
private final DcMotorEx encoder;
private final Servo hoodServo;
private final Servo turret1;
private final Servo turret2;
private final MultipleTelemetry telemetry;
private boolean telemetryOn = false;
private double manualPower = 0.0;
private double hoodPos = 0.0;
private double turretPos = 0.0;
private double velocity = 0.0;
private double posPower = 0.0;
public double velo = 0.0;
private int targetPosition = 0;
public double powPID = 1.0;
private double p = 0.0003, i = 0, d = 0.00001, f=0;
private PIDFController controller;
private double pow = 0.0;
private String shooterMode = "AUTO";
private String turretMode = "AUTO";
public Shooter(Robot robot, MultipleTelemetry TELE) {
this.fly1 = robot.shooter1;
this.fly2 = robot.shooter2;
this.telemetry = TELE;
this.hoodServo = robot.hood;
// Reset encoders
fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
controller = new PIDFController(p, i, d, f);
controller.setPIDF(p, i, d, f);
this.turret1 = robot.turr1;
this.turret2 = robot.turr2;
this.encoder = robot.shooterEncoder;
}
public double gethoodPosition() {
return (hoodServo.getPosition());
}
public void sethoodPosition(double pos) { hoodPos = pos; }
public double getTurretPosition() {
return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2);
}
public void setTurretPosition(double pos) { turretPos = pos; }
public double getVelocity(double vel) {
return vel;
}
public void setVelocity(double vel) { velocity = vel; }
public void setPosPower(double power) { posPower = power; }
public void setTargetPosition(int pos) {
targetPosition = pos;
}
public void setTolerance(int tolerance) {
controller.setTolerance(tolerance);
}
public void setControllerCoefficients(double kp, double ki, double kd, double kf) {
p = kp;
i = ki;
d = kd;
f = kf;
controller.setPIDF(p, i, d, f);
}
public PIDCoefficients getControllerCoefficients() {
return new PIDCoefficients(p, i, d);
}
public void setManualPower(double power) { manualPower = power; }
public String getShooterMode() { return shooterMode; }
public String getTurretMode() { return turretMode; }
public double getECPRPosition() {
return fly1.getCurrentPosition() / (2 * ecpr);
}
public double getMCPRPosition() {
return (double) fly1.getCurrentPosition() / 4;
}
public void setShooterMode(String mode) { shooterMode = mode; }
public void setTurretMode(String mode) { turretMode = mode; }
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) {
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y,
goalPose.heading.toDouble() - (robotPose.heading.toDouble())
);
double distance = Math.sqrt(
deltaPose.position.x * deltaPose.position.x
+ deltaPose.position.y * deltaPose.position.y
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight
);
telemetry.addData("dst", distance);
double shooterPow = getPowerByDist(distance);
double hoodAngle = getAngleByDist(distance);
// hoodServo.setPosition(hoodAngle);
moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
return distance;
//0.9974 * 355
}
public double getTurretPosByDeltaPose(Pose2d dPose, double offset) {
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x));
telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) {
deltaAngle -= 360;
}
// deltaAngle += aTanAngle;
deltaAngle /= (335);
telemetry.addData("dAngle", deltaAngle);
telemetry.addData("AtanAngle", aTanAngle);
return ((0.30 - deltaAngle) + offset);
}
//62, 0.44
//56.5, 0.5
public double getPowerByDist(double dist) {
//TODO: ADD LOGIC
return dist;
}
public double getAngleByDist(double dist) {
double newDist = dist - 56.5;
double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
return pos;
}
public void setTelemetryOn(boolean state) { telemetryOn = state; }
public void moveTurret(double pos) {
turret1.setPosition(pos);
turret2.setPosition(1 - pos);
}
public double getpowPID() {
return powPID;
}
@Override
public void update() {
if (Objects.equals(shooterMode, "MANUAL")) {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly1.setPower(manualPower);
fly2.setPower(manualPower);
} else if (Objects.equals(shooterMode, "VEL")) {
powPID = velocity;
fly1.setPower(powPID);
fly2.setPower(powPID);
}
}
}

256
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Spindexer.java
View File

@@ -1,256 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Spindexer implements Subsystem {
private Servo s1;
private Servo s2;
private DigitalChannel p0;
private DigitalChannel p1;
private DigitalChannel p2;
private DigitalChannel p3;
private DigitalChannel p4;
private DigitalChannel p5;
private AnalogInput input;
private AnalogInput input2;
private MultipleTelemetry TELE;
private double position = 0.501;
private boolean telemetryOn = false;
private boolean ball0 = false;
private boolean ball1 = false;
private boolean ball2 = false;
private boolean green0 = false;
private boolean green1 = false;
private boolean green2 = false;
public Spindexer (Robot robot, MultipleTelemetry tele){
this.s1 = robot.spin1;
this.s2 = robot.spin2;
this.p0 = robot.pin0;
this.p1 = robot.pin1;
this.p2 = robot.pin2;
this.p3 = robot.pin3;
this.p4 = robot.pin4;
this.p5 = robot.pin5;
this.input = robot.analogInput;
this.input2 = robot.analogInput2;
this.TELE = tele;
}
public void setTelemetryOn(boolean state){
telemetryOn = state;
}
public void colorSensorTelemetry() {
TELE.addData("ball0", ball0);
TELE.addData("ball1", ball1);
TELE.addData("ball2", ball2);
TELE.addData("green0", green0);
TELE.addData("green1", green1);
TELE.addData("green2", green2);
}
public void checkForBalls() {
if (p0.getState()){
ball0 = true;
green0 = p1.getState();
} else {
ball0 = false;
}
if (p2.getState()){
ball1 = true;
green1 = p3.getState();
} else {
ball1 = false;
}
if (p4.getState()){
ball2 = true;
green2 = p5.getState();
} else {
ball2 = false;
}
}
public void setPosition (double pos) {
position = pos;
}
public void intake () {
position = spindexer_intakePos1;
}
public void intakeShake(double runtime) {
if ((runtime % 0.25) >0.125) {
position = spindexer_intakePos1 + 0.04;
} else {
position = spindexer_intakePos1 - 0.04;
}
}
public void outtake3Shake(double runtime) {
if ((runtime % 0.25) >0.125) {
position = spindexer_outtakeBall3 + 0.04;
} else {
position = spindexer_outtakeBall3 - 0.04;
}
}
public void outtake3 () {
position = spindexer_outtakeBall3;
}
public void outtake2 () {
position = spindexer_outtakeBall2;
}
public void outtake1 () {
position = spindexer_outtakeBall1;
}
public int outtakeGreen(int secLast, int Last) {
if (green2 && (secLast!=3) && (Last!=3)) {
outtake3();
return 3;
} else if (green1 && (secLast!=2) && (Last!=2)){
outtake2();
return 2;
} else if (green0 && (secLast!=1) && (Last!=1)) {
outtake1();
return 1;
} else {
if (secLast!=1 && Last!= 1){
outtake1();
return 1;
} else if (secLast!=2 && Last!=2){
outtake2();
return 2;
} else {
outtake3();
return 3;
}
}
}
public void outtakeGreenFs() {
if (green0 && ball0) {
outtake1();
} else if (green1 && ball1){
outtake2();
} else if (green2 && ball2) {
outtake3();
}
}
public int greens() {
int num = 0;
if (green0){num++;}
if (green1){num++;}
if (green2){num++;}
return num;
}
public int outtakePurple(int secLast, int Last) {
if (!green2 && (secLast!=3) && (Last!=3)) {
outtake3();
return 3;
} else if (!green1 && (secLast!=2) && (Last!=2)){
outtake2();
return 2;
} else if (!green0 && (secLast!=1) && (Last!=1)) {
outtake1();
return 1;
} else {
if (secLast!=1 && Last!= 1){
outtake1();
return 1;
} else if (secLast!=2 && Last!=2){
outtake2();
return 2;
} else {
outtake3();
return 3;
}
}
}
@Override
public void update() {
if (position !=0.501) {
s1.setPosition(position);
s2.setPosition(1 - position);
}
if (telemetryOn) {
colorSensorTelemetry();
}
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Subsystem.java
View File

@@ -1,6 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
public interface Subsystem {
public void update();
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old/subsystems/Transfer.java
View File

@@ -1,59 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Transfer implements Subsystem {
private final Servo servo;
private final DcMotorEx transfer;
private double motorPow = 0.0;
private double servoPos = 0.501;
public Transfer (Robot robot){
this.servo = robot.transferServo;
this.transfer = robot.transfer;
}
public void setTransferPosition(double pos){
this.servoPos = pos;
}
public void setTransferPower (double pow){
this.motorPow = pow;
}
public void transferOut(){
this.setTransferPosition(transferServo_out);
}
public void transferIn(){
this.setTransferPosition(transferServo_in);
}
@Override
public void update() {
if (servoPos!=0.501){
servo.setPosition(servoPos);
}
transfer.setPower(motorPow);
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -1,19 +1,16 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorImplEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.openftc.easyopencv.OpenCvWebcam;
public class Robot {
@@ -30,8 +27,6 @@ public class Robot {
public DcMotorEx transfer;
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;
@@ -40,7 +35,6 @@ public class Robot {
public Servo rejecter;
public Servo turr1;
public Servo turr2;
@@ -62,27 +56,31 @@ public class Robot {
public AnalogInput analogInput2;
public AprilTagProcessor aprilTagProcessor;
public AnalogInput spin1Pos;
public AnalogInput spin2Pos;
public AnalogInput hoodPos;
public AnalogInput turr1Pos;
public AnalogInput turr2Pos;
public AnalogInput transferServoPos;
public AprilTagProcessor aprilTagProcessor;
public WebcamName webcam;
public DcMotorEx shooterEncoder;
public RevColorSensorV3 color1;
public RevColorSensorV3 color2;
public RevColorSensorV3 color3;
public Robot (HardwareMap hardwareMap) {
public Robot(HardwareMap hardwareMap) {
//Define components w/ hardware map
@@ -109,14 +107,24 @@ public class Robot {
hood = hardwareMap.get(Servo.class, "hood");
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
turr1 = hardwareMap.get(Servo.class, "t1");
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
turr2 = hardwareMap.get(Servo.class, "t2");
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
spin1 = hardwareMap.get(Servo.class, "spin1");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
spin2 = hardwareMap.get(Servo.class, "spin2");
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -129,27 +137,26 @@ public class Robot {
pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
analogInput = hardwareMap.get(AnalogInput.class, "analog");
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
transfer = hardwareMap.get(DcMotorEx.class, "transfer");
transferServo = hardwareMap.get(Servo.class, "transferServo");
transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
transfer.setDirection(DcMotorSimple.Direction.REVERSE);
aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
}
}