here you go keshav
This commit is contained in:
@@ -0,0 +1,4 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.constants;
|
||||||
|
|
||||||
|
public class Color {
|
||||||
|
}
|
||||||
@@ -58,58 +58,73 @@ public class ActiveColorSensorTest extends LinearOpMode {
|
|||||||
|
|
||||||
robot.intake.setPower(1);
|
robot.intake.setPower(1);
|
||||||
|
|
||||||
|
// Reset the counters after 1 second of not reading a ball.
|
||||||
|
final double ColorCounterResetDelay = 1.0;
|
||||||
|
// Number of times the loop needs to run before deciding on a color.
|
||||||
|
final int ColorCounterTotalMinCount = 20;
|
||||||
|
// If the color sensor reads a color this percentage of time
|
||||||
|
// out of the total, declare the color.
|
||||||
|
// Usage: (Color Count)/(Total Count) > ColorCounterThreshold
|
||||||
|
final double ColorCounterThreshold = 0.65;
|
||||||
|
|
||||||
if (robot.pin1.getState()){
|
if (robot.pin1.getState()){
|
||||||
if (robot.pin0.getState()){
|
if (robot.pin0.getState()){
|
||||||
b1Purple += 1;
|
b1Purple ++;
|
||||||
purpleStamp1 = getRuntime();
|
|
||||||
}
|
}
|
||||||
b1Total += 1;
|
b1Total++;
|
||||||
totalStamp1 = getRuntime();
|
totalStamp1 = getRuntime();
|
||||||
}
|
}
|
||||||
if (getRuntime() - totalStamp1 > 1){
|
if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
b1 = "none";
|
b1 = "none";
|
||||||
b1Total = 1;
|
b1Total = 1;
|
||||||
b1Purple = 1;
|
b1Purple = 1;
|
||||||
}else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){
|
}else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
|
||||||
b1 = "Green";
|
// Enough Time has passed and we met the threshold
|
||||||
}else if (b1Purple > 10){
|
|
||||||
b1 = "Purple";
|
b1 = "Purple";
|
||||||
|
}else if (b1Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b1 = "Green";
|
||||||
}
|
}
|
||||||
|
|
||||||
if (robot.pin3.getState()){
|
if (robot.pin3.getState()){
|
||||||
if (robot.pin2.getState()){
|
if (robot.pin2.getState()){
|
||||||
b2Purple += 1;
|
b2Purple ++;
|
||||||
purpleStamp2 = getRuntime();
|
|
||||||
}
|
}
|
||||||
b2Total += 1;
|
b2Total++;
|
||||||
totalStamp2 = getRuntime();
|
totalStamp2 = getRuntime();
|
||||||
}
|
}
|
||||||
if (getRuntime() - totalStamp2 > 1){
|
if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
b2 = "none";
|
b2 = "none";
|
||||||
b2Total = 1;
|
b2Total = 1;
|
||||||
b2Purple = 1;
|
b2Purple = 1;
|
||||||
}else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){
|
}else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
|
||||||
b2 = "Green";
|
// Enough Time has passed and we met the threshold
|
||||||
}else if (b2Purple > 10){
|
|
||||||
b2 = "Purple";
|
b2 = "Purple";
|
||||||
|
}else if (b2Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b2 = "Green";
|
||||||
}
|
}
|
||||||
|
|
||||||
if (robot.pin5.getState()){
|
if (robot.pin5.getState()){
|
||||||
if (robot.pin4.getState()){
|
if (robot.pin4.getState()){
|
||||||
b3Purple += 1;
|
b3Purple ++;
|
||||||
purpleStamp3 = getRuntime();
|
|
||||||
}
|
}
|
||||||
b3Total += 1;
|
b3Total++;
|
||||||
totalStamp3 = getRuntime();
|
totalStamp3 = getRuntime();
|
||||||
}
|
}
|
||||||
if (getRuntime() - totalStamp3 > 1){
|
if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
b3 = "none";
|
b3 = "none";
|
||||||
b3Total = 1;
|
b3Total = 1;
|
||||||
b3Purple = 1;
|
b3Purple = 1;
|
||||||
}else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){
|
}else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
|
||||||
b3 = "Green";
|
// Enough Time has passed and we met the threshold
|
||||||
}else if (b3Purple > 10){
|
|
||||||
b3 = "Purple";
|
b3 = "Purple";
|
||||||
|
}else if (b3Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b3 = "Green";
|
||||||
}
|
}
|
||||||
|
|
||||||
TELE.addData("Green1:", robot.pin1.getState());
|
TELE.addData("Green1:", robot.pin1.getState());
|
||||||
|
|||||||
@@ -6,34 +6,58 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
|||||||
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.DistanceUnit;
|
||||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||||
|
|
||||||
@TeleOp
|
@TeleOp
|
||||||
@Config
|
@Config
|
||||||
public class ColorSensorTest extends LinearOpMode{
|
public class ColorSensorTest extends LinearOpMode {
|
||||||
Robot robot;
|
Robot robot;
|
||||||
MultipleTelemetry TELE;
|
MultipleTelemetry TELE;
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException{
|
public void runOpMode() throws InterruptedException {
|
||||||
robot = new Robot(hardwareMap);
|
robot = new Robot(hardwareMap);
|
||||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
waitForStart();
|
waitForStart();
|
||||||
if (isStopRequested()) return;
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
while (opModeIsActive()){
|
while (opModeIsActive()) {
|
||||||
TELE.addData("Green1:", robot.pin1.getState());
|
|
||||||
TELE.addData("Purple1:", robot.pin0.getState());
|
// ----- COLOR 1 -----
|
||||||
TELE.addData("Green2:", robot.pin3.getState());
|
double green1 = robot.color1.getNormalizedColors().green;
|
||||||
TELE.addData("Purple2:", robot.pin2.getState());
|
double blue1 = robot.color1.getNormalizedColors().blue;
|
||||||
TELE.addData("Green3:", robot.pin5.getState());
|
double red1 = robot.color1.getNormalizedColors().red;
|
||||||
TELE.addData("Purple3:", robot.pin4.getState());
|
|
||||||
TELE.addData("Hello Keshav (analog)", robot.analogInput.getVoltage());
|
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
|
||||||
TELE.addData("Hello again (analog2)", robot.analogInput2.getVoltage());
|
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
|
||||||
|
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
|
||||||
|
|
||||||
|
|
||||||
|
// ----- COLOR 2 -----
|
||||||
|
double green2 = robot.color2.getNormalizedColors().green;
|
||||||
|
double blue2 = robot.color2.getNormalizedColors().blue;
|
||||||
|
double red2 = robot.color2.getNormalizedColors().red;
|
||||||
|
|
||||||
|
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
|
||||||
|
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
|
||||||
|
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
|
||||||
|
|
||||||
|
|
||||||
|
// ----- COLOR 3 -----
|
||||||
|
double green3 = robot.color3.getNormalizedColors().green;
|
||||||
|
double blue3 = robot.color3.getNormalizedColors().blue;
|
||||||
|
double red3 = robot.color3.getNormalizedColors().red;
|
||||||
|
|
||||||
|
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
|
||||||
|
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
|
||||||
|
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
|
||||||
|
|
||||||
|
|
||||||
TELE.update();
|
TELE.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode {
|
|||||||
|
|
||||||
public static int LED_Brightness = 50;
|
public static int LED_Brightness = 50;
|
||||||
|
|
||||||
public static int lowerGreen = 120;
|
public static int lowerGreen = 110;
|
||||||
|
|
||||||
public static int higherGreen = 150;
|
public static int higherGreen = 150;
|
||||||
|
|
||||||
@@ -22,10 +22,10 @@ public class ConfigureColorRangefinder extends LinearOpMode {
|
|||||||
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
|
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
|
||||||
pin0 --> purple
|
pin0 --> purple
|
||||||
pin1 --> green */
|
pin1 --> green */
|
||||||
crf.setPin0Analog(ColorRangefinder.AnalogMode.GREEN); // green
|
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
|
||||||
// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
|
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
|
||||||
// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
|
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
|
||||||
// crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
|
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
|
||||||
|
|
||||||
crf.setLedBrightness(LED_Brightness);
|
crf.setLedBrightness(LED_Brightness);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
package org.firstinspires.ftc.teamcode.utils;
|
package org.firstinspires.ftc.teamcode.utils;
|
||||||
|
|
||||||
|
import com.qualcomm.hardware.rev.RevColorSensorV3;
|
||||||
import com.qualcomm.robotcore.hardware.AnalogInput;
|
import com.qualcomm.robotcore.hardware.AnalogInput;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
@@ -7,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorImplEx;
|
|||||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
import com.qualcomm.robotcore.hardware.I2cDevice;
|
||||||
import com.qualcomm.robotcore.hardware.Servo;
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||||
@@ -77,6 +79,12 @@ public class Robot {
|
|||||||
|
|
||||||
public DcMotorEx shooterEncoder;
|
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
|
//Define components w/ hardware map
|
||||||
@@ -150,5 +158,10 @@ public class Robot {
|
|||||||
|
|
||||||
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
|
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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user