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

Merge remote-tracking branch 'origin/daniel' into daniel

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
This commit is contained in:
DanTheMan-byte
2025-11-11 20:47:51 -06:00
parent deda28dd37 ee2208922b
commit 331ec2fa0b
4 changed files with 120 additions and 796 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.subsystems; package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.controller.PIDController;
@@ -20,8 +22,6 @@ import java.util.Objects;
public class Shooter implements Subsystem { public class Shooter implements Subsystem {
private final DcMotorEx fly1; private final DcMotorEx fly1;
private final DcMotorEx fly2; private final DcMotorEx fly2;
private final DcMotorEx encoder;
private final Servo hoodServo; private final Servo hoodServo;
private final Servo turret1; private final Servo turret1;
@@ -50,9 +50,9 @@ public class Shooter implements Subsystem {
private String shooterMode = "AUTO"; private String shooterMode = "MANUAL";
private String turretMode = "AUTO"; private String turretMode = "MANUAL";
public Shooter(Robot robot, MultipleTelemetry TELE) { public Shooter(Robot robot, MultipleTelemetry TELE) {
@@ -78,8 +78,6 @@ public class Shooter implements Subsystem {
this.turret2 = robot.turr2; this.turret2 = robot.turr2;
this.encoder = robot.shooterEncoder;
@@ -172,7 +170,7 @@ public class Shooter implements Subsystem {
public void setTurretMode(String mode){ turretMode = mode;} public void setTurretMode(String mode){ turretMode = mode;}
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){ public void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -182,7 +180,7 @@ public class Shooter implements Subsystem {
Pose2d deltaPose = new Pose2d( Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x, goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y, goalPose.position.y - robotPose.position.y,
goalPose.heading.toDouble() - (robotPose.heading.toDouble()) goalPose.heading.toDouble() - robotPose.heading.toDouble()
); );
double distance = Math.sqrt( double distance = Math.sqrt(
@@ -191,65 +189,42 @@ public class Shooter implements Subsystem {
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight + Poses.relativeGoalHeight * Poses.relativeGoalHeight
); );
telemetry.addData("dst", distance);
double shooterPow = getPowerByDist(distance); double shooterPow = getPowerByDist(distance);
double hoodAngle = getAngleByDist(distance); double hoodAngle = getAngleByDist(distance);
if (shooterOn){
// hoodServo.setPosition(hoodAngle); fly1.setVelocity(shooterPow);
fly2.setPower(fly1.getPower());
moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); } else {
fly1.setPower(0);
fly2.setPower(0);
}
return distance; hoodServo.setPosition(hoodAngle);
//0.9974 * 355
moveTurret(getTurretPosByDeltaPose(deltaPose));
} }
public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ public double getTurretPosByDeltaPose (Pose2d dPose){
double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x)); if (deltaAngle < -180) {
deltaAngle +=360;
telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) {
deltaAngle -=360;
} }
deltaAngle /= (turret_GearRatio*turret_Range);
return (0.5+deltaAngle) ;
// 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){ public double getPowerByDist(double dist){
//TODO: ADD LOGIC //TODO: ADD LOGIC
@@ -258,20 +233,11 @@ public class Shooter implements Subsystem {
public double getAngleByDist(double dist){ public double getAngleByDist(double dist){
//TODO: ADD LOGIC
double newDist = dist - 56.5; return dist;
double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
return pos;
} }
public void setTelemetryOn(boolean state){telemetryOn = state;} public void setTelemetryOn(boolean state){telemetryOn = state;}
public void moveTurret(double pos){ public void moveTurret(double pos){
@@ -296,16 +262,9 @@ public class Shooter implements Subsystem {
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setVelocity(velocity);
fly2.setPower(fly1.getPower());
fly1.setVelocity(velocity, AngleUnit.DEGREES);
fly2.setVelocity(velocity, AngleUnit.DEGREES);
} }
else if (Objects.equals(shooterMode, "POS")){ else if (Objects.equals(shooterMode, "POS")){
@@ -320,7 +279,7 @@ public class Shooter implements Subsystem {
} }
if (Objects.equals(turretMode, "MANUAL")){ if (Objects.equals(turretMode, "MANUAL")){
// hoodServo.setPosition(hoodPos); hoodServo.setPosition(hoodPos);
moveTurret(turretPos); moveTurret(turretPos);

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
View File

@@ -41,16 +41,6 @@ public class Transfer implements Subsystem{
this.motorPow = 0; this.motorPow = 0;
} }
public void transferOut(){
this.setTransferPosition(transferServo_out);
}
public void transferIn(){
this.setTransferPosition(transferServo_in);
}
@Override @Override

743
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -1,20 +1,14 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Rejecter; import org.firstinspires.ftc.teamcode.subsystems.Rejecter;
@@ -43,6 +37,8 @@ public class TeleopV1 extends LinearOpMode {
Transfer transfer; Transfer transfer;
Shooter shooter;
MultipleTelemetry TELE; MultipleTelemetry TELE;
GamepadEx g1; GamepadEx g1;
@@ -53,85 +49,34 @@ public class TeleopV1 extends LinearOpMode {
public static double slowMoSpeed = 0.4; public static double slowMoSpeed = 0.4;
public static double power = 0.0; public static double shooterPower = 0.0;
public static double pos = hoodDefault; public static double turretPosition = 0.501;
public boolean all = false; public static double hoodPosition = 0.501;
public int ticker = 0;
ToggleButtonReader g1RightBumper; ToggleButtonReader g1RightBumper;
ToggleButtonReader g1LeftBumper;
ToggleButtonReader g2Circle; ToggleButtonReader g2Circle;
ToggleButtonReader g2Square; ToggleButtonReader g2Square;
ToggleButtonReader g2Triangle;
ToggleButtonReader g2RightBumper;
ToggleButtonReader g1LeftBumper;
ToggleButtonReader g2LeftBumper; ToggleButtonReader g2LeftBumper;
ToggleButtonReader g2DpadUp; ToggleButtonReader g2Triangle;
ToggleButtonReader g2DpadDown;
ToggleButtonReader g2DpadRight;
ToggleButtonReader g2DpadLeft;
public boolean leftBumper = false;
public double g1RightBumperStamp = 0.0; public double g1RightBumperStamp = 0.0;
public double g1LeftBumperStamp = 0.0;
public double g2LeftBumperStamp = 0.0;
public static int spindexerPos = 0; public static int spindexerPos = 0;
public boolean green = false; public double time = 0.0;
Shooter shooter;
public boolean scoreAll = false;
MecanumDrive drive ;
public boolean autotrack = false;
public int last = 0;
public int second = 0;
public double offset = 0.0;
public static double rIn = 0.59;
public static double rOut = 0;
public boolean notShooting = true;
public boolean circle = false;
public boolean square = false;
public boolean tri = false;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, teleStart);
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry( TELE = new MultipleTelemetry(
@@ -145,12 +90,12 @@ public class TeleopV1 extends LinearOpMode {
g1, GamepadKeys.Button.RIGHT_BUMPER g1, GamepadKeys.Button.RIGHT_BUMPER
); );
g2 = new GamepadEx(gamepad2);
g1LeftBumper = new ToggleButtonReader( g1LeftBumper = new ToggleButtonReader(
g1, GamepadKeys.Button.LEFT_BUMPER g1, GamepadKeys.Button.LEFT_BUMPER
); );
g2 = new GamepadEx(gamepad2);
g2Circle = new ToggleButtonReader( g2Circle = new ToggleButtonReader(
g2, GamepadKeys.Button.B g2, GamepadKeys.Button.B
); );
@@ -163,33 +108,10 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.X g2, GamepadKeys.Button.X
); );
g2RightBumper = new ToggleButtonReader( g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.RIGHT_BUMPER
);
g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.LEFT_BUMPER g2, GamepadKeys.Button.LEFT_BUMPER
); );
g2DpadUp = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_UP
);
g2DpadDown = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_DOWN
);
g2DpadLeft = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_LEFT
);
g2DpadRight = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_RIGHT
);
@@ -207,65 +129,19 @@ public class TeleopV1 extends LinearOpMode {
spindexer = new Spindexer(robot, TELE); spindexer = new Spindexer(robot, TELE);
spindexer.setTelemetryOn(true);
shooter = new Shooter(robot, TELE); shooter = new Shooter(robot, TELE);
shooter.setShooterMode("MANUAL"); spindexer.setTelemetryOn(true);
robot.rejecter.setPosition(rIn);
time = getRuntime();
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, teleStart);
while(opModeIsActive()){ while(opModeIsActive()){
drive.updatePoseEstimate();
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("off", offset);
robot.hood.setPosition(pos);
g1LeftBumper.readValue();
if (g1LeftBumper.wasJustPressed()){
g2LeftBumperStamp = getRuntime();
spindexer.intakeShake(getRuntime());
leftBumper = true;
}
if (leftBumper){
double time = getRuntime() - g2LeftBumperStamp;
if (time < 1.0){
robot.rejecter.setPosition(rOut);
} else {
robot.rejecter.setPosition(rIn);
}
}
intake(); intake();
rejecter.rejecterPos(rpos); rejecter.rejecterPos(rpos);
@@ -274,526 +150,13 @@ public class TeleopV1 extends LinearOpMode {
TELE.update(); TELE.update();
transfer.update(); transfer();
g2RightBumper.readValue(); shooter.setManualPower(shooterPower);
g2LeftBumper.readValue(); shooter.sethoodPosition(hoodPosition);
g2DpadDown.readValue();
g2DpadUp.readValue();
if (!scoreAll){
spindexer.checkForBalls();
}
if(g2DpadUp.wasJustPressed()){
pos -=0.02;
}
if(g2DpadDown.wasJustPressed()){
pos +=0.02;
}
g2DpadLeft.readValue();
g2DpadRight.readValue();
if(g2DpadLeft.wasJustPressed()){
offset -=0.02;
}
if(g2DpadRight.wasJustPressed()){
offset +=0.02;
}
TELE.addData("hood", pos);
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
} else {
autotrack = false;
shooter.moveTurret(0.3+offset);
}
if (gamepad2.right_stick_button){
autotrack = true;
}
if (g2RightBumper.wasJustPressed()){
transfer.setTransferPower(1);
transfer.transferIn();
shooter.setManualPower(1);
notShooting = false;
}
if (g2RightBumper.wasJustReleased()){
transfer.setTransferPower(1);
transfer.transferOut();
}
if (gamepad2.left_stick_y>0.5){
shooter.setManualPower(0);
} else if (gamepad2.left_stick_y<-0.5){
shooter.setManualPower(1);
}
if (g2LeftBumper.wasJustPressed()){
g2LeftBumperStamp = getRuntime();
notShooting = false;
scoreAll = true;
}
if (scoreAll) {
double time = getRuntime() - g2LeftBumperStamp;
shooter.setManualPower(1);
TELE.addData("greenImportant", green);
TELE.addData("last", last);
TELE.addData("2ndLast", second);
int numGreen = spindexer.greens();
if (square) {
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
second = last;
} else {
last = spindexer.outtakeGreen(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else if (tri) {
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
} else {
last = spindexer.outtakeGreen(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else if (circle){
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
} else {
last = spindexer.outtakeGreen(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else{
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
} else {
all = true;
}
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (all) {
spindexer.outtake3();
last = 3;
second = 3;
} else if (green) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
}
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (all) {
spindexer.outtake2();
last = 2;
} else if (green) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
}
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (all) {
spindexer.outtake1();
} else if (green) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
}
}
shooter.update();
shooter.setTurretPosition(turretPosition);
@@ -825,10 +188,6 @@ public class TeleopV1 extends LinearOpMode {
if (g1RightBumper.wasJustPressed()){ if (g1RightBumper.wasJustPressed()){
notShooting = true;
if (getRuntime() - g1RightBumperStamp < 0.3){ if (getRuntime() - g1RightBumperStamp < 0.3){
intake.reverse(); intake.reverse();
@@ -836,65 +195,47 @@ public class TeleopV1 extends LinearOpMode {
intake.toggle(); intake.toggle();
} }
if (intake.getIntakeState()==1){
shooter.setManualPower(0);
}
spindexer.intake(); spindexer.intake();
transfer.transferOut();
g1RightBumperStamp = getRuntime(); g1RightBumperStamp = getRuntime();
} }
if (intake.getIntakeState()==1) {
if (intake.getIntakeState()==1 && notShooting) {
spindexer.intakeShake(getRuntime()); spindexer.intakeShake(getRuntime());
transfer.setTransferPowerOff();
transfer.setTransferPositionOff();
} else { } else {
if (g2Circle.wasJustPressed()){ if (g2Circle.wasJustPressed()){
circle = true; transfer.setTransferPositionOff();
tri = false; intake.intakeMinPower();
square = false; spindexer.outtake3();
transfer.setTransferPowerOn();
} }
if (g2Triangle.wasJustPressed()){ if (g2Triangle.wasJustPressed()){
circle = false; transfer.setTransferPositionOff();
tri = true; intake.intakeMinPower();
square = false; spindexer.outtake2();
transfer.setTransferPowerOn();
} }
if (g2Square.wasJustPressed()){ if (g2Square.wasJustPressed()){
circle = false; transfer.setTransferPositionOff();
tri = false; intake.intakeMinPower();
square = true; spindexer.outtake1();
transfer.setTransferPowerOn();
} }
if (gamepad2.x){
circle = false;
tri = false;
square = false;
}
} }
intake.update(); intake.update();
transfer.update();
spindexer.update(); spindexer.update();
@@ -902,5 +243,17 @@ public class TeleopV1 extends LinearOpMode {
}
public void transfer(){
g1LeftBumper.readValue();
if (g1LeftBumper.wasJustPressed()){
transfer.setTransferPositionOn();
}
transfer.update();
} }
} }

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
View File

@@ -1,34 +1,41 @@
package org.firstinspires.ftc.teamcode.utils; package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
@Config @Config
@TeleOp @Autonomous
public class ConfigureColorRangefinder extends LinearOpMode { public class ConfigureColorRangefinder extends LinearOpMode {
public static double lowerBound = 80;
public static double higherBound = 120;
public static int led = 0;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color")); ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
waitForStart(); /*
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true: Using this example configuration, you can detect all three sample colors based on which pin is reading true:
pin0 --> purple both --> yellow
pin1 --> green */ only pin0 --> blue
crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 3, 20); only pin1 --> red
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green neither --> no object
crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer */
crf.setLedBrightness(led); crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 80 / 360.0 * 255, 140 / 360.0 * 255); // green
crf.setPin1Saturation(175, 255);
crf.setPin1Value(100,200);
crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement
crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple
crf.setLedBrightness(0);
waitForStart();
stop();
} }
} }
@@ -37,9 +44,12 @@ public class ConfigureColorRangefinder extends LinearOpMode {
* Online documentation: <a href="https://docs.brushlandlabs.com">...</a> * Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
*/ */
class ColorRangefinder { class ColorRangefinder {
public static int LED_VALUE = 15;
public final RevColorSensorV3 emulator;
private final I2cDeviceSynchSimple i2c; private final I2cDeviceSynchSimple i2c;
public ColorRangefinder(RevColorSensorV3 emulator) { public ColorRangefinder(RevColorSensorV3 emulator) {
this.emulator = emulator;
this.i2c = emulator.getDeviceClient(); this.i2c = emulator.getDeviceClient();
this.i2c.enableWriteCoalescing(true); this.i2c.enableWriteCoalescing(true);
} }
@@ -62,6 +72,22 @@ class ColorRangefinder {
setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound); setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound);
} }
public void setPin0Saturation(double lowerBound, double higherBound) {
setDigital(PinNum.PIN0, DigitalMode.SATURATION, lowerBound, higherBound);
}
public void setPin1Saturation(double lowerBound, double higherBound) {
setDigital(PinNum.PIN1, DigitalMode.SATURATION, lowerBound, higherBound);
}
// Optional: Easy methods for value/brightness thresholding
public void setPin0Value(double lowerBound, double higherBound) {
setDigital(PinNum.PIN0, DigitalMode.VALUE, lowerBound, higherBound);
}
public void setPin1Value(double lowerBound, double higherBound) {
setDigital(PinNum.PIN1, DigitalMode.VALUE, lowerBound, higherBound);
}
/** /**
* Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger. * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
* This is most useful when we want to know if an object is both close and the correct color. * This is most useful when we want to know if an object is both close and the correct color.
@@ -83,18 +109,13 @@ class ColorRangefinder {
* This is useful if we want to threshold red; instead of having two thresholds we would invert * This is useful if we want to threshold red; instead of having two thresholds we would invert
* the color and look for blue. * the color and look for blue.
*/ */
public void setPin0InvertHue() {
setPin0DigitalMaxDistance(DigitalMode.HSV, 200);
}
/** /**
* Invert the hue value before thresholding it, meaning that the colors become their opposite. * Invert the hue value before thresholding it, meaning that the colors become their opposite.
* This is useful if we want to threshold red; instead of having two thresholds we would invert * This is useful if we want to threshold red; instead of having two thresholds we would invert
* the color and look for blue. * the color and look for blue.
*/ */
public void setPin1InvertHue() {
setPin1DigitalMaxDistance(DigitalMode.HSV, 200);
}
/** /**
* The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog. * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog.
@@ -161,7 +182,7 @@ class ColorRangefinder {
if (lowerBound == higherBound) { if (lowerBound == higherBound) {
lo = (int) lowerBound; lo = (int) lowerBound;
hi = (int) higherBound; hi = (int) higherBound;
} else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255 } else if (digitalMode.value <= DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range
lo = (int) Math.round(lowerBound / 255.0 * 65535); lo = (int) Math.round(lowerBound / 255.0 * 65535);
hi = (int) Math.round(higherBound / 255.0 * 65535); hi = (int) Math.round(higherBound / 255.0 * 65535);
} else { // distance in mm } else { // distance in mm
@@ -214,7 +235,7 @@ class ColorRangefinder {
} }
public enum DigitalMode { public enum DigitalMode {
RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6); RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6), SATURATION(7), VALUE(8);
public final byte value; public final byte value;
DigitalMode(int value) { DigitalMode(int value) {
@@ -231,3 +252,4 @@ class ColorRangefinder {
} }
} }
} }