Skip to content

Commit

Permalink
Merge pull request #8 from KookyBotz/dual
Browse files Browse the repository at this point in the history 
haha funny relocalization go brrrrr
  • Loading branch information
@ProDCG
ProDCG authored Jan 23, 2024
2 parents 54a85b6 + 4610b38 commit 259d0f8
Show file tree
Hide file tree
Showing 17 changed files with 181 additions and 78 deletions.
4 changes: 2 additions & 2 deletions ...g/firstinspires/ftc/teamcode/common/commandbase/cycleautocommand/FirstDepositCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ public FirstDepositCommand() {
new WaitCommand(250),
new ClawCommand(IntakeSubsystem.ClawState.OPEN, ClawSide.BOTH),
new WaitCommand(250),
new ExtensionCommand(0),
new WaitCommand(150),
new ExtensionCommand(75),
new WaitCommand(50),
new ArmCommand(0.2),
new ArmLiftCommand(0.735),
new ClawCommand(IntakeSubsystem.ClawState.CLOSED, ClawSide.BOTH),
Expand Down
2 changes: 1 addition & 1 deletion ...tinspires/ftc/teamcode/common/commandbase/cycleautocommand/FirstDepositExtendCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
public class FirstDepositExtendCommand extends SequentialCommandGroup {
public FirstDepositExtendCommand() {
super(
new WaitCommand(1250),
// new WaitCommand(700),
new ArmCommand(3.05),
new ArmFloatCommand(false),
new ArmLiftCommand(0.3),
Expand Down
5 changes: 4 additions & 1 deletion ...firstinspires/ftc/teamcode/common/commandbase/cycleautocommand/FirstStackGrabCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ArmLiftCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ClawCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ExtensionCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.PivotCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.PivotStateCommand;
import org.firstinspires.ftc.teamcode.common.hardware.Globals;
import org.firstinspires.ftc.teamcode.common.subsystem.IntakeSubsystem;
Expand All @@ -15,13 +16,15 @@
public class FirstStackGrabCommand extends SequentialCommandGroup {
public FirstStackGrabCommand() {
super(
new PivotStateCommand(IntakeSubsystem.PivotState.FLAT),
new PivotCommand(0.51),
new ExtensionCommand(550),
new WaitCommand(500),
new ClawCommand(IntakeSubsystem.ClawState.CLOSED, Globals.ALLIANCE == Location.BLUE ? ClawSide.RIGHT : ClawSide.LEFT),
new WaitCommand(250),
new ArmLiftCommand(0.63),
new PivotStateCommand(IntakeSubsystem.PivotState.STORED),
new ExtensionCommand(0)
new ExtensionCommand(75)
);
}
}
5 changes: 3 additions & 2 deletions ...irstinspires/ftc/teamcode/common/commandbase/cycleautocommand/FirstStackSetupCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,9 @@ public FirstStackSetupCommand() {
new ArmLiftCommand(0.68),
new WaitCommand(400),
new ArmFloatCommand(true),
new PivotStateCommand(IntakeSubsystem.PivotState.FLAT),
new PivotCommand(0.51)
// new PivotStateCommand(IntakeSubsystem.PivotState.FLAT),
new PivotCommand(0.25)
// new PivotCommand(0.51)
);
}
}
2 changes: 2 additions & 0 deletions ...tinspires/ftc/teamcode/common/commandbase/cycleautocommand/PurplePixelDepositCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ArmCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ClawCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.ExtensionCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.PivotCommand;
import org.firstinspires.ftc.teamcode.common.commandbase.subsytemcommand.PivotStateCommand;
import org.firstinspires.ftc.teamcode.common.hardware.Globals;
import org.firstinspires.ftc.teamcode.common.subsystem.IntakeSubsystem;
Expand All @@ -20,6 +21,7 @@ public PurplePixelDepositCommand() {
new ExtensionCommand(0),
new ArmCommand(0.2),
new PivotStateCommand(IntakeSubsystem.PivotState.STORED),

new WaitCommand(250)
);
}
Expand Down
4 changes: 2 additions & 2 deletions .../firstinspires/ftc/teamcode/common/commandbase/cycleautocommand/SecondDepositCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
public class SecondDepositCommand extends SequentialCommandGroup {
public SecondDepositCommand() {
super(
new WaitCommand(1250),
new WaitCommand(700),
new ArmCommand(2.82),
new ArmFloatCommand(false),
new ArmLiftCommand(0.3),
Expand All @@ -29,7 +29,7 @@ public SecondDepositCommand() {
new WaitCommand(250),
new ArmCommand(2.805),
new WaitCommand(750),
new ExtensionCommand(0),
new ExtensionCommand(75),
new WaitCommand(150),
new ArmCommand(0.2),
new ClawCommand(IntakeSubsystem.ClawState.CLOSED, ClawSide.BOTH),
Expand Down
2 changes: 1 addition & 1 deletion ...irstinspires/ftc/teamcode/common/commandbase/cycleautocommand/SecondStackGrabCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public SecondStackGrabCommand() {
new WaitCommand(250),
new ArmLiftCommand(0.63),
new PivotStateCommand(IntakeSubsystem.PivotState.STORED),
new ExtensionCommand(0)
new ExtensionCommand(75)
);
}
}
30 changes: 30 additions & 0 deletions ...irstinspires/ftc/teamcode/common/commandbase/cycleautocommand/StackRelocalizeCommand.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package org.firstinspires.ftc.teamcode.common.commandbase.cycleautocommand;

import com.arcrobotics.ftclib.command.InstantCommand;
import com.arcrobotics.ftclib.command.SequentialCommandGroup;
import com.arcrobotics.ftclib.command.WaitCommand;

import org.firstinspires.ftc.teamcode.common.commandbase.drivecommand.PositionCommand;
import org.firstinspires.ftc.teamcode.common.drive.pathing.geometry.Pose;
import org.firstinspires.ftc.teamcode.common.hardware.RobotHardware;
import org.firstinspires.ftc.teamcode.common.vision.StackPipeline;

import java.util.function.DoubleSupplier;

public class StackRelocalizeCommand extends SequentialCommandGroup {
public StackRelocalizeCommand(StackPipeline stackPipeline, Pose targetPose) {
super (
// new WaitCommand(50), // TODO: lower this. due to processing times with the pipeline, giving this a lenient time period
new InstantCommand(() -> {
double correction = stackPipeline.getStrafeCorrection();
System.out.println("CORRECTION342 " + correction);
Pose currentPose = RobotHardware.getInstance().localizer.getPose();
System.out.println("CORRECTION342 " + currentPose);
RobotHardware.getInstance().localizer.setPose(new Pose(currentPose.x + correction, currentPose.y, currentPose.heading));
System.out.println("CORRECTION342 " + RobotHardware.getInstance().localizer.getPose());
}),
// new WaitCommand(100),
new PositionCommand(targetPose)
);
}
}
4 changes: 2 additions & 2 deletions ...g/firstinspires/ftc/teamcode/common/commandbase/cycleautocommand/ThirdDepositCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
public class ThirdDepositCommand extends SequentialCommandGroup {
public ThirdDepositCommand() {
super(
new WaitCommand(1250),
new WaitCommand(700),
new ArmCommand(2.82),
new ArmFloatCommand(false),
new ArmLiftCommand(0.3),
Expand All @@ -29,7 +29,7 @@ public ThirdDepositCommand() {
new WaitCommand(250),
new ArmCommand(2.805),
new WaitCommand(750),
new ExtensionCommand(0),
new ExtensionCommand(75),
new WaitCommand(150),
new ArmCommand(0.2),
new ClawCommand(IntakeSubsystem.ClawState.CLOSED, ClawSide.BOTH),
Expand Down
2 changes: 1 addition & 1 deletion ...firstinspires/ftc/teamcode/common/commandbase/cycleautocommand/ThirdStackGrabCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ public ThirdStackGrabCommand() {
new ArmFloatCommand(false),
new ArmCommand(0),
new PivotStateCommand(IntakeSubsystem.PivotState.STORED),
new ExtensionCommand(0)
new ExtensionCommand(75)
);
}
}
6 changes: 3 additions & 3 deletions .../java/org/firstinspires/ftc/teamcode/common/commandbase/drivecommand/PositionCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,9 @@ public Pose getPower(Pose robotPose) {
double x_rotated = xPower * Math.cos(-robotPose.heading) - yPower * Math.sin(-robotPose.heading);
double y_rotated = xPower * Math.sin(-robotPose.heading) + yPower * Math.cos(-robotPose.heading);

hPower = Range.clip(hPower, -0.4, 0.4);
x_rotated = Range.clip(x_rotated, -0.5, 0.5);
y_rotated = Range.clip(y_rotated, -0.5, 0.5);
hPower = Range.clip(hPower, -0.8, 0.8);
x_rotated = Range.clip(x_rotated, -1.0, 1.0);
y_rotated = Range.clip(y_rotated, -1.0, 1.0);

return new Pose(x_rotated * 1.6, y_rotated, hPower);
}
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/RobotHardware.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,7 @@ public void init(final HardwareMap hardwareMap) {

for (LynxModule m : modules) {
m.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
if (m.isParent() && LynxConstants.isEmbeddedSerialNumber(m.getSerialNumber()) && CONTROL_HUB == null) CONTROL_HUB = m;
if (m.isParent() && LynxConstants.isEmbeddedSerialNumber(m.getSerialNumber())) CONTROL_HUB = m;
}


Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ public void updateState(@NotNull PivotState state) {
public void periodic() {
double pos = robot.armActuator.getOverallTargetPosition();
if (pivotState == PivotState.SCORING) {
double targetAngle = (pos) - (((pos < Math.PI / 2) ? 0.22 : 0.78) * Math.PI);
double targetAngle = (pos) - (((pos < Math.PI / 2) ? 0.275 : 0.725) * Math.PI);
robot.intakePivotActuator.setTargetPosition(MathUtils.clamp(MathUtils.map(targetAngle, 0, Math.PI / 2 - 0.35, 0.5, 0.93), 0.03, 0.97));
} else if (pivotState == PivotState.FLAT) {
// double targetAngle = ((pos) - ((((pos < Math.PI / 2) ? 0 : 1) * Math.PI)));
Expand Down
37 changes: 26 additions & 11 deletions ...teamcode/common/vision/PixelPipeline.java → ...teamcode/common/vision/StackPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import android.graphics.Canvas;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
Expand All @@ -18,14 +19,16 @@
import java.util.ArrayList;
import java.util.List;

public class PixelPipeline implements VisionProcessor {
public class StackPipeline implements VisionProcessor {

public static int TOPLEFT_X = 200;
public static int TOPLEFT_X = 600;
public static int TOPLEFT_Y = 450;

public static int WIDTH = 1520;
public static int WIDTH = 1120;
public static int HEIGHT = 630;

// public volatile double correctionAmt = 0.0;

ContourData closestPixelContour = new ContourData(0, 0, 0, 0);
ContourData closestTapeContour = new ContourData(0, 0, 0, 0);

Expand All @@ -36,14 +39,12 @@ public void init(int width, int height, CameraCalibration calibration) {

@Override
public Object processFrame(Mat frame, long captureTimeNanos) {
System.out.println(frame.width());
System.out.println(frame.height());
frame = frame.submat(new Rect(TOPLEFT_X, TOPLEFT_Y, WIDTH, HEIGHT));
Mat frame2 = frame.submat(new Rect(TOPLEFT_X, TOPLEFT_Y, WIDTH, HEIGHT));

Imgproc.cvtColor(frame, frame, Imgproc.COLOR_BGR2HLS);
Imgproc.cvtColor(frame2, frame2, Imgproc.COLOR_BGR2HLS);

Mat mask = new Mat();
Core.inRange(frame, new Scalar(0, 185, 0), new Scalar(179, 255, 255), mask);
Core.inRange(frame2, new Scalar(0, 200, 0), new Scalar(179, 255, 255), mask);

Mat element = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
Imgproc.erode(mask, mask, element);
Expand All @@ -58,7 +59,6 @@ public Object processFrame(Mat frame, long captureTimeNanos) {
Imgproc.findContours(pixel_mask, pixelContours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);
Imgproc.findContours(tape_mask, tapeContours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);


// Go through all pixel contours
double minDistance = Double.MAX_VALUE;
for (MatOfPoint contour : pixelContours) {
Expand All @@ -79,7 +79,9 @@ public Object processFrame(Mat frame, long captureTimeNanos) {
}
}
}
System.out.println("PIXEL POSE: (" + closestPixelContour.x + ", " + closestPixelContour.y + ") - AREA: (" + closestPixelContour.area + ") - LENGTH: (" + closestPixelContour.length + ")");

Imgproc.circle(frame2, new Point(closestPixelContour.x, closestPixelContour.y), 7, new Scalar(0, 0, 255), -1);


// Go through all tape contours
minDistance = Double.MAX_VALUE;
Expand All @@ -95,17 +97,25 @@ public Object processFrame(Mat frame, long captureTimeNanos) {

double distance = Math.sqrt(Math.pow(cX - 976, 2) + Math.pow(cY - 138, 2));

System.out.println("TAPE POSE: (" + cX + ", " + cY + ")");

if (distance < minDistance) {
minDistance = distance;
closestTapeContour = new ContourData(cX, cY, area, length);
}
}
}
System.out.println("TAPE POSE: (" + closestTapeContour.x + ", " + closestTapeContour.y + ") - AREA: (" + closestTapeContour.area + ") - LENGTH: (" + closestTapeContour.length + ")");

Imgproc.circle(frame2, new Point(closestTapeContour.x, closestTapeContour.y), 7, new Scalar(0, 0, 255), -1);
// frame2.copyTo(frame);
// frame2.copyTo(frame);

// System.out.println("TAPE POSE: (" + closestTapeContour.x + ", " + closestTapeContour.y + ")");

mask.release();
hierarchy.release();
element.release();
frame2.release();

return null;
}
Expand All @@ -123,6 +133,11 @@ public ContourData getClosestTapeContour() {
return closestTapeContour;
}

public double getStrafeCorrection() {
// correctionAmt = -0.0120*closestTapeContour.x + 12.42;
return -0.0120*(closestTapeContour.x + 400) + 12.26;
}

public class ContourData {
public int x;
public int y;
Expand Down
Loading

0 comments on commit 259d0f8

Please sign in to comment.