Skip to content
This repository has been archived by the owner on Aug 31, 2024. It is now read-only.

Commit

Permalink
Merge pull request #93 from rh-robotics/teo-teleop-color
Browse files Browse the repository at this point in the history 
Auto Close Claw & Stop Intake on Pixel
  • Loading branch information
@DragonDev07
DragonDev07 authored Feb 22, 2024
2 parents 130e4c1 + f581cc3 commit 1a62e5a
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 117 deletions.
120 changes: 7 additions & 113 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

import androidx.annotation.NonNull;

import com.qualcomm.robotcore.hardware.ColorRangeSensor;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
Expand Down Expand Up @@ -37,9 +38,9 @@ public class HWC {
// ------ Declare Motors ------ //
public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightPulley, leftPulley, intakeMotor;
// ------ Declare Servos ------ //
public Servo intakeArm, wrist, clawR, clawL, passoverArmLeft, passoverArmRight;
public Servo wrist, clawR, clawL, passoverArmLeft, passoverArmRight;
// ------ Declare Sensors ------ //
public ColorSensor colorLeft, colorRight;
public ColorRangeSensor colorLeft, colorRight;
// ------ Declare Gamepads ------ //
public Gamepad currentGamepad1 = new Gamepad();
public Gamepad currentGamepad2 = new Gamepad();
Expand Down Expand Up @@ -90,7 +91,6 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor");

// ------ Retrieve Servos ------ //
intakeArm = hardwareMap.get(Servo.class, "intakeArm");
clawL = hardwareMap.get(Servo.class, "clawL");
clawR = hardwareMap.get(Servo.class, "clawR");
wrist = hardwareMap.get(Servo.class, "wrist");
Expand All @@ -101,8 +101,8 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {

// ------ Retrieve Sensors ------ //
webcam = hardwareMap.get(WebcamName.class, "webcam");
colorLeft = hardwareMap.get(ColorSensor.class, "colorL");
colorRight = hardwareMap.get(ColorSensor.class, "colorR");
colorLeft = hardwareMap.get(ColorRangeSensor.class, "colorL");
colorRight = hardwareMap.get(ColorRangeSensor.class, "colorR");

// ------ Set Motor Directions ------ //
leftFront.setDirection(DcMotorEx.Direction.FORWARD);
Expand All @@ -129,26 +129,6 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
rightPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
}

// ------ Function to Run Intake ------ //
// Runs intake at given power until color sensor detects a
// pixel
public void runIntake(double pwr) {
intakeMotor.setPower(pwr);
if (intakeDetection(colorLeft) == 0 && intakeDetection(colorRight) == 0) {
clawL.setPosition(1);
clawR.setPosition(1);
intakeMotor.setPower(0);
} else if (intakeDetection(colorLeft) == 1) {
clawL.setPosition(1);

} else if (intakeDetection(colorRight) == 0) {
clawR.setPosition(1);


}

}

// ------ Function to Toggle Claw (Open/Close) ------ //
public void toggleClaw(char servo) {
switch (servo) {
Expand All @@ -159,39 +139,12 @@ public void toggleClaw(char servo) {
clawR.setPosition(clawR.getPosition() == 0 ? 0.50 : 0);
break;
case 'C':
clawR.setPosition(clawR.getPosition() == 0 ? 0.85 : 0);
clawL.setPosition(clawL.getPosition() == 1 ? 0.15 : 1);
clawR.setPosition(clawR.getPosition() == 0 ? 0.5 : 0);
clawL.setPosition(clawL.getPosition() == 1 ? 0.5 : 1);
break;
}
}


// ------ Function to get Color ------ //
public String getColor(ColorSensor CS) {
int red = CS.red();
int green = CS.green();
int blue = CS.blue();
String color;

if (red > 200 && green > 200 && blue > 200) {
color = "white";
} else if (210 < red && green > 200 & blue < 160) {
color = "yellow";
} else if (green > red + blue) {
color = "green";
} else {
color = "unknown";
}

return color;
}

public int intakeDetection(ColorSensor CS) {
if (CS.argb() == 0) {
return 1;
} else return 0;
}

public void betterSleep(int sleep) {
sleepTime.reset();
while (sleepTime.milliseconds() < sleep) {
Expand Down Expand Up @@ -228,65 +181,6 @@ public void resetEncoders() {
rightRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
}

// ------ Function to Drive Given Distance Using Odometry ------ //
public void odoDrive(int distance) {
leftRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // y axis
rightRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis
leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis
while (leftRear.getCurrentPosition() > distance) {
leftFront.setPower(0.3);
rightFront.setPower(0.3);
leftRear.setPower(0.3);
rightRear.setPower(0.3);
}
leftFront.setPower(0);
rightFront.setPower(0);
leftRear.setPower(0);
rightRear.setPower(0);
}

// ------ Function to Strafe Given Distance Using Odometry ------ //
public void odoStrafeLeft(int distance) {
odoDrive(500);
leftRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // y axis
rightRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // x axis
leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis


while (leftRear.getCurrentPosition() < distance) {
leftFront.setPower(-0.3);
rightFront.setPower(0.3);
leftRear.setPower(0.3);
rightRear.setPower(-0.3);
}
}

public void odoStrafeRight(int distance) {
odoDrive(500);
leftRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // y axis
rightRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // x axis
leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); // x axis


while (Math.abs(leftRear.getCurrentPosition()) < distance) {
leftFront.setPower(0.3);
rightFront.setPower(-0.3);
leftRear.setPower(-0.3);
rightRear.setPower(0.3);
}
}

public void odoTurnRight() {
leftFront.setPower(0.3);
rightFront.setPower(-0.3);
leftRear.setPower(-0.3);
rightRear.setPower(0.3);
}

// ------ Function to Turn Given Degrees Using Odometry ------ //
public void odoTurn(int degrees) {
}

public void initTFOD(String TFOD_MODEL_ASSET) {

// Create the TensorFlow processor by using a builder.
Expand Down
34 changes: 30 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.subsystems.HWC;
import org.firstinspires.ftc.teamcode.teleop.enums.MultiplierSelection;

Expand Down Expand Up @@ -161,14 +162,19 @@ public void loop() {

// ------ (GAMEPAD 1) Intake Toggle Controls ------ //
if (robot.currentGamepad1.right_bumper && !robot.previousGamepad1.right_bumper) {
if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) { intakeState = IntakeState.OFF; }
else if (intakeState == IntakeState.OFF) { intakeState = IntakeState.INTAKE; }
if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) {
intakeState = IntakeState.OFF;
} else if (intakeState == IntakeState.OFF) {
intakeState = IntakeState.INTAKE;
}
}

if (robot.currentGamepad1.left_bumper && !robot.previousGamepad1.left_bumper) {
if (intakeState == IntakeState.INTAKE || intakeState == IntakeState.OUTTAKE) {
intakeState = IntakeState.OFF;
} else if (intakeState == IntakeState.OFF) { intakeState = IntakeState.OUTTAKE; }
} else if (intakeState == IntakeState.OFF) {
intakeState = IntakeState.OUTTAKE;
}
}
// ------ (GAMEPAD 1) MANUAL Wrist Controls ------ //
if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) {
Expand Down Expand Up @@ -208,15 +214,35 @@ public void loop() {
robot.wrist.setPosition(wristPosition);

// -------- Check Intake State & Run Intake ------ //
switch(intakeState) {
switch (intakeState) {
case INTAKE:
// Set Power
robot.intakeMotor.setPower(-1);

// Close Claws when Pixel Detected
if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1) {
robot.clawL.setPosition(0.5);
}

if (robot.colorRight.getDistance(DistanceUnit.CM) <= 1) {
robot.clawR.setPosition(0.5);
}

// If both Pixels are Detected, Stop Intake
if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) {
intakeState = IntakeState.OFF;
}

break;
case OFF:
// Give Manual Control to Gamepad 2
robot.intakeMotor.setPower(robot.currentGamepad2.right_stick_x);

break;
case OUTTAKE:
// Set Power
robot.intakeMotor.setPower(1);

break;
}

Expand Down

0 comments on commit 1a62e5a

Please sign in to comment.