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

color sensor test

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-25 21:39:17 -06:00
parent 3b342d1656
commit bfa8b52ebc
3 changed files with 162 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java Normal file
View File

@@ -0,0 +1,129 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp
@Config
public class ActiveColorSensorTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException{
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
int b1Purple = 1;
int b1Total = 1;
int b2Purple = 1;
int b2Total = 1;
int b3Purple = 1;
int b3Total = 1;
double totalStamp1 = 0.0;
double purpleStamp1 = 0.0;
double totalStamp2 = 0.0;
double purpleStamp2 = 0.0;
double totalStamp3 = 0.0;
double purpleStamp3 = 0.0;
String b1 = "none";
String b2 = "none";
String b3 = "none";
double position = 0.0;
double stamp = getRuntime();
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if ((getRuntime() % 0.3) >0.15) {
position = spindexer_intakePos + 0.02;
} else {
position = spindexer_intakePos - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1-position);
robot.intake.setPower(1);
if (robot.pin1.getState()){
if (robot.pin0.getState()){
b1Purple += 1;
purpleStamp1 = getRuntime();
}
b1Total += 1;
totalStamp1 = getRuntime();
}
if (getRuntime() - totalStamp1 > 1){
b1 = "none";
b1Total = 1;
b1Purple = 1;
}else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){
b1 = "Green";
}else if (b1Purple > 10){
b1 = "Purple";
}
if (robot.pin3.getState()){
if (robot.pin2.getState()){
b2Purple += 1;
purpleStamp2 = getRuntime();
}
b2Total += 1;
totalStamp2 = getRuntime();
}
if (getRuntime() - totalStamp2 > 1){
b2 = "none";
b2Total = 1;
b2Purple = 1;
}else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){
b2 = "Green";
}else if (b2Purple > 10){
b2 = "Purple";
}
if (robot.pin5.getState()){
if (robot.pin4.getState()){
b3Purple += 1;
purpleStamp3 = getRuntime();
}
b3Total += 1;
totalStamp3 = getRuntime();
}
if (getRuntime() - totalStamp3 > 1){
b3 = "none";
b3Total = 1;
b3Purple = 1;
}else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){
b3 = "Green";
}else if (b3Purple > 10){
b3 = "Purple";
}
TELE.addData("Green1:", robot.pin1.getState());
TELE.addData("Purple1:", robot.pin0.getState());
TELE.addData("Green2:", robot.pin3.getState());
TELE.addData("Purple2:", robot.pin2.getState());
TELE.addData("Green3:", robot.pin5.getState());
TELE.addData("Purple3:", robot.pin4.getState());
TELE.addData("1", b1);
TELE.addData("2",b2);
TELE.addData("3",b3);
TELE.update();
}
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -19,10 +19,10 @@ public class ShooterTest extends LinearOpMode {
public static double vel = 0.0;
public static double mcpr = 28.0; // CPR of the motor
public static double ecpr = 1024.0; // CPR of the encoder
public static double pos = 0.0;
public static double hoodPos = 0.5;
public static double posPower = 0.0;
public static double posi = 0.5;
public static double turretPos = 0.9;
public static double p = 0.0003, i = 0, d = 0, f = 0;
@@ -30,34 +30,22 @@ public class ShooterTest extends LinearOpMode {
public static String turrMode = "MANUAL";
public static int posTolerance = 40;
double initPos = 0.0;
public static double servoPosition = 0.501;
double velo1 = 0.0;
public double stamp = 0.0;
double stamp1 = 0.0;
public double initPos = 0.0;
public static double time = 0.1;
public double velo = 0.0;
public double velo1 = 0.0;
public double stamp1 = 0.0;
public double initPos1 = 0.0;
double initPos1 = 0.0;
double powPID = 0.0;
public static double maxVel = 4500;
public static int tolerance = 300;
MultipleTelemetry TELE;
public boolean wait(double time) {
return (getRuntime() - stamp) > time;
}
@Override
public void runOpMode() throws InterruptedException {
@@ -85,31 +73,23 @@ public class ShooterTest extends LinearOpMode {
shooter.setControllerCoefficients(p, i, d, f);
shooter.setTurretMode(turrMode);
shooter.setShooterMode(flyMode);
shooter.setManualPower(pow);
shooter.sethoodPosition(pos);
shooter.setTurretPosition(posi);
shooter.setTolerance(posTolerance);
shooter.setPosPower(posPower);
if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
if (hoodPos != 0.501) {
robot.hood.setPosition(hoodPos);
}
if (turretPos != 0.501) {
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(turretPos);
}
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition();
if (Math.abs(vel - velo1) > 1500){
if (vel - velo1 > 0){
powPID = 0.75;
} else if (vel - velo1 < 0){
powPID = 0.25;
}
if (Math.abs(vel - velo1) > 3 * tolerance) {
powPID = vel / maxVel;
} else if (vel - tolerance > velo1) {
powPID = powPID + 0.001;
} else if (vel + tolerance < velo1) {
@@ -121,13 +101,12 @@ public class ShooterTest extends LinearOpMode {
TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
TELE.addData("Velocity", velo);
TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower());
TELE.addData("Power Fly 2", robot.shooter2.getPower());
TELE.addData("powPID", shooter.getpowPID());
TELE.addData("Ins Velocity", velo1);
TELE.addData("Velocity", velo1);
TELE.update();
}

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

@@ -1,13 +1,20 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
@Config
@TeleOp
public class ConfigureColorRangefinder extends LinearOpMode {
public static int LED_Brightness = 35; //1 - 35, 2 - 50,
public static int lowerGreen = 90;
public static int higherGreen = 160;
@Override
public void runOpMode() throws InterruptedException {
ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
@@ -15,12 +22,12 @@ public class ConfigureColorRangefinder extends LinearOpMode {
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
pin0 --> purple
pin1 --> green */
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 20); // green
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 150 / 360.0 * 255, 360 / 360.0 * 255); // purple
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, 100 / 360.0 *255);
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 10mm or closer requirement
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
crf.setLedBrightness(10);
crf.setLedBrightness(LED_Brightness);
}
}
@@ -135,6 +142,7 @@ class ColorRangefinder {
/**
* Read distance via I2C
*
* @return distance in millimeters
*/
public double readDistance() {