Skip to content

Commit

Permalink
Worked on intake claw code and added button inputs for manipulator.
Browse files Browse the repository at this point in the history
  • Loading branch information
@AlphaX410
AlphaX410 committed Nov 12, 2024
1 parent 7ec3a79 commit af70585
Show file tree
Hide file tree
Showing 7 changed files with 206 additions and 54 deletions.
4 changes: 3 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;


package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
//ARM from RI6W
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
Expand Down
39 changes: 39 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;

import java.util.Arrays;
import java.util.Collections;
import java.util.List;

@Config
public class IntakeArm extends QQMechanism {
//change values later
public static double ARM_START_POSITION = 0.0;
public static double ARM_INTAKE_POSITION = 0.9;
public static double SEARCHING_POSITION = 0.5;
Servo armServo;

@Override
public void init(HardwareMap hwMap) {armServo = hwMap.get(Servo.class, "intakeArm");}
public void goToPos(double position){
armServo.setPosition(position);
}
public void start(){armServo.setPosition(ARM_START_POSITION);}
public void intake(){armServo.setPosition(ARM_INTAKE_POSITION);}
public void searching(){armServo.setPosition(SEARCHING_POSITION);}


@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestServo("arm_pos", ARM_START_POSITION, ARM_INTAKE_POSITION, armServo));
}
}

56 changes: 10 additions & 46 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ public class IntakeClaw extends QQMechanism {
public static double CLAW_OPEN_POSITION = 0;
public static double WRIST_START_POSITION = 0;
public static double WRIST_END_POSITION = 0.67;
public static double WRIST_TRANSFER_POSITION = 0.0;
public static double WRIST_DEPO_POSITION = 1.0;

Servo clawServo;
Servo wristServo;
Expand All @@ -44,71 +46,33 @@ public class IntakeClaw extends QQMechanism {
public void init(HardwareMap hwMap) {
clawServo = hwMap.get(Servo.class, "intake_claw_servo");
wristServo = hwMap.get(Servo.class, "intake_wrist_servo");
webcamName = hwMap.get(WebcamName.class, "Webcam 1");

colorLocator = new ColorBlobLocatorProcessor.Builder()
.setTargetColorRange(ColorRange.RED) // use a predefined color match
.setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs
.setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) // search central 1/4 of camera view
.setDrawContours(true) // Show contours on the Stream Preview
.setBlurSize(5) // Smooth the transitions between different colors in image
.build();
@SuppressWarnings("unused")
VisionPortal portal = new VisionPortal.Builder()
.addProcessor(colorLocator)
.setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT))
.setCamera(webcamName)
.build();

}

@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo),
new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo),
new TestWebcam("Webcam", webcamName)
new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo)
);
}

@Override
public void update() {
ColorBlobLocatorProcessor.Blob blob = blobClosestToCenter();
if (blob != null) {
double angle = blob.getBoxFit().angle;
if (blob.getBoxFit().size.height > blob.getBoxFit().size.width) {
angle = 90 + angle;
}
setWristAngle(angle);
}
}

public ColorBlobLocatorProcessor.Blob blobClosestToCenter() {
double closest = 100000000;
ColorBlobLocatorProcessor.Blob returnBlob = null;

List<ColorBlobLocatorProcessor.Blob> blobs = colorLocator.getBlobs();
for (ColorBlobLocatorProcessor.Blob blob : blobs) {
double distance = getDistanceFromCenter(blob.getBoxFit());
if ((blob.getContourArea() > 50) && (distance < closest)) {
closest = distance;
returnBlob = blob;
}
}
return returnBlob;
}

private double getDistanceFromCenter(RotatedRect boxFit) {
return Math.abs(boxFit.center.x - CENTER_X) + Math.abs(boxFit.center.y - CENTER_Y);
}

public void open() {
clawServo.setPosition(CLAW_OPEN_POSITION);
}

public void close() {
clawServo.setPosition(CLAW_CLOSE_POSITION);
}
public void transfer(){
wristServo.setPosition(WRIST_TRANSFER_POSITION);
}
public void deposit(){wristServo.setPosition(WRIST_DEPO_POSITION);};

public void setWristAngle(double angle){
//convert angle to servo pos
double servoAngle = WRIST_START_POSITION + (WRIST_END_POSITION - WRIST_START_POSITION) * (angle/180);
if(telemetry != null){
telemetry.addData("angle", angle);
Expand Down
119 changes: 119 additions & 0 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/VisionIntakeClaw.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;

import android.util.Size;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestWebcam;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
import org.firstinspires.ftc.vision.opencv.ColorRange;
import org.firstinspires.ftc.vision.opencv.ImageRegion;
import org.opencv.core.RotatedRect;

import java.util.Arrays;
import java.util.List;

@Config
public class VisionIntakeClaw extends QQMechanism {
public static double CLAW_CLOSE_POSITION = 0.3;
public static double CLAW_OPEN_POSITION = 0;
public static double WRIST_START_POSITION = 0;
public static double WRIST_END_POSITION = 0.67;

Servo clawServo;
Servo wristServo;
WebcamName webcamName;
public Telemetry telemetry;

public static final int CAMERA_WIDTH = 640;
public static final int CAMERA_HEIGHT = 480;
int CENTER_X = CAMERA_WIDTH / 2;
int CENTER_Y = CAMERA_HEIGHT / 2;

ColorBlobLocatorProcessor colorLocator;

@Override
public void init(HardwareMap hwMap) {
clawServo = hwMap.get(Servo.class, "intake_claw_servo");
wristServo = hwMap.get(Servo.class, "intake_wrist_servo");
webcamName = hwMap.get(WebcamName.class, "Webcam 1");

colorLocator = new ColorBlobLocatorProcessor.Builder()
.setTargetColorRange(ColorRange.RED) // use a predefined color match
.setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs
.setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) // search central 1/4 of camera view
.setDrawContours(true) // Show contours on the Stream Preview
.setBlurSize(5) // Smooth the transitions between different colors in image
.build();
@SuppressWarnings("unused")
VisionPortal portal = new VisionPortal.Builder()
.addProcessor(colorLocator)
.setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT))
.setCamera(webcamName)
.build();
}

@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo),
new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo),
new TestWebcam("Webcam", webcamName)
);
}

@Override
public void update() {
ColorBlobLocatorProcessor.Blob blob = blobClosestToCenter();
if (blob != null) {
double angle = blob.getBoxFit().angle;
if (blob.getBoxFit().size.height > blob.getBoxFit().size.width) {
angle = 90 + angle;
}
setWristAngle(angle);
}
}

public ColorBlobLocatorProcessor.Blob blobClosestToCenter() {
double closest = 100000000;
ColorBlobLocatorProcessor.Blob returnBlob = null;

List<ColorBlobLocatorProcessor.Blob> blobs = colorLocator.getBlobs();
for (ColorBlobLocatorProcessor.Blob blob : blobs) {
double distance = getDistanceFromCenter(blob.getBoxFit());
if ((blob.getContourArea() > 50) && (distance < closest)) {
closest = distance;
returnBlob = blob;
}
}
return returnBlob;
}

private double getDistanceFromCenter(RotatedRect boxFit) {
return Math.abs(boxFit.center.x - CENTER_X) + Math.abs(boxFit.center.y - CENTER_Y);
}

public void open() {
clawServo.setPosition(CLAW_OPEN_POSITION);
}

public void close() {
clawServo.setPosition(CLAW_CLOSE_POSITION);
}
public void setWristAngle(double angle){
double servoAngle = WRIST_START_POSITION + (WRIST_END_POSITION - WRIST_START_POSITION) * (angle/180);
if(telemetry != null){
telemetry.addData("angle", angle);
telemetry.addData("ServoAngle", servoAngle);
}
wristServo.setPosition(servoAngle);
}
}

36 changes: 31 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ public class NoTurnTeleop extends QQOpMode{
public static final double TRIGGER_THRESHOLD = 0.5;
public static final int MANUAL_CHANGE = 5;
private boolean isPlacing = false;
private double clawAngle = 90;

public void init(){
isPlacing = false;
Expand Down Expand Up @@ -66,20 +67,45 @@ public void loop(){
}else{
nav.driveFieldRelative(forward, left, rotate);
}
if(gamepad2.x){

if(gamepad2.y){
robot.intakeSlides.fullExtension();
}
if(gamepad2.y){
else if(gamepad2.b){
robot.intakeSlides.halfExtension();
}
if(gamepad2.a){
else if(gamepad2.a){
robot.intakeSlides.startPosition();
}
if(gamepad2.dpad_up){
else if(gamepad2.dpad_right){
robot.intakeSlides.manualPositionChange(5);
}
if(gamepad2.dpad_down){
else if (gamepad2.dpad_left){
robot.intakeSlides.manualPositionChange(-5);
}

if (gamepad2.left_trigger>TRIGGER_THRESHOLD){
robot.intakeClaw.setWristAngle(0);
}else if (gamepad2.left_bumper){
robot.intakeClaw.setWristAngle(90);

}else {
robot.intakeClaw.setWristAngle(clawAngle);
clawAngle += (gamepad2.left_stick_x/25);
}

if (gamepad2.right_trigger>TRIGGER_THRESHOLD){
robot.intakeClaw.close();
}else if (gamepad2.right_bumper){
robot.intakeClaw.open();
}

//Intake arm manual

//if (gamepad2.dpad_up){
//robot.intakeArm.manualPositionChange(changeAmount: 5)}
//}else if (gamepad2.dpad_down){
//robot.intakeArm.manualPositionChange(changeAmount: -5)}

}
}
4 changes: 3 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ public Robot() {
otos = new OpticalTrackingOdometrySensor();
claw = new Claw();
intakeSlides = new IntakeSlides();
intakeClaw = new IntakeClaw();
//doubleReverse4Bar = new DoubleReverse4Bar();
//slides = new Slides();

Expand All @@ -54,7 +55,8 @@ public Robot() {
// slides,
//doubleReverse4Bar,
intakeSlides,
scoreArm);
scoreArm,
intakeClaw);

}
public void init(HardwareMap hwMap) {
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/readme.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,6 @@
* Joshua Smith
* Shreyas Thota
* Rebecca Smith
* Ritika Rajagopali
* Ritika Rajagopal
* Kavya Boinepally
* Mahin Waghray

0 comments on commit af70585

Please sign in to comment.