Skip to content

Commit

Permalink
add purple pixel, start adapting OpMode for Psi
Browse files Browse the repository at this point in the history
  • Loading branch information
@JJTech0130
JJTech0130 committed Jan 6, 2024
1 parent d9623b7 commit 970045d
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 51 deletions.
57 changes: 36 additions & 21 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,21 @@
package org.firstinspires.ftc.teamcode.drive;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;

public class PsiBot extends Robot{
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;

@Config
public class PsiBot extends Robot {
public static double LEFTFORTYFIVE = -45;
public static double LEFTSEVENTY = 70;
public static double RIGHTFORTYFIVE = 45;
public static double RIGHTSEVENTY = -70;
private final AprilTagCamera[] cameras;
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0);
private final TrajectoryDrive drive;
Expand All @@ -29,8 +40,18 @@ public static double getMotorVelocityF(double ticksPerSecond) {
public static double kV = 0.014129716300132542;
public static double kA = 0.0032;
public static double kStatic = 0;
protected PsiBot(HardwareMap hardwareMap) {

private final Servo purpleServo;

public PsiBot(HardwareMap hardwareMap) {
super(hardwareMap);
cameras = new AprilTagCamera[3];
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE));
cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0));
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE));

purpleServo = hardwareMap.get(Servo.class, "purple");

// TODO: Reverse Motors, encoders & such
drive = new TrajectoryDrive(
hardwareMap,
Expand All @@ -52,31 +73,25 @@ protected PsiBot(HardwareMap hardwareMap) {
kA,
kV,
kStatic











);


}

@Override
public AprilTagCamera[] getCameras() {
return cameras;
}



@Override
public void dropPurplePixel(boolean state) {
if (state) {
purpleServo.setPosition(0.3);
} else {
purpleServo.setPosition(0.5);
}
}

@Override
public TrajectoryDrive getDrive() {
return drive;
}



}
18 changes: 9 additions & 9 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,21 @@
package org.firstinspires.ftc.teamcode.drive;

import com.acmerobotics.roadrunner.drive.MecanumDrive;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.drive.psi.PsiDrive;
import org.firstinspires.ftc.teamcode.drive.testbot.TestBotDrive;
import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;

public abstract class Robot {

public abstract TrajectoryDrive getDrive();
protected Robot(HardwareMap hardwareMap) {



// Any shared initialization goes here
}

public abstract AprilTagCamera[] getCameras();


/**
* Sets the purple pixel on the robot;
* true: dropped;
* false: closed
*/
public abstract void dropPurplePixel(boolean state);
}
12 changes: 12 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,24 @@

import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;

public class RoboticaBot extends Robot {

protected RoboticaBot(HardwareMap hardwareMap) {
super(hardwareMap);
}

@Override
public AprilTagCamera[] getCameras() {
return new AprilTagCamera[0];
}

@Override
public void dropPurplePixel(boolean state) {
// TODO
}

@Override
public TrajectoryDrive getDrive() {
return null;
Expand Down
39 changes: 18 additions & 21 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PsiAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,15 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.drive.psi.PsiDrive;
import org.firstinspires.ftc.teamcode.drive.PsiBot;
import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
import org.firstinspires.ftc.teamcode.util.Timeout;
import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;
import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer;
import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection;

import java.util.Timer;

@Autonomous(name = "Psi Auto", group = "A")
@Config
public class PsiAuto extends LinearOpMode {
Expand All @@ -39,25 +37,18 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize
return pose;
}


public static double LEFTFORTYFIVE = -45;
public static double LEFTSEVENTY = 70;
public static double RIGHTFORTYFIVE = 45;
public static double RIGHTSEVENTY = -70;
Servo droneServo;


@Override
public void runOpMode() throws InterruptedException {
GlobalOpMode.opMode = this;
PsiDrive drive = new PsiDrive(hardwareMap);
PsiBot bot = new PsiBot(hardwareMap);
TrajectoryDrive drive = bot.getDrive();
//PsiDrive drive = new PsiDrive(hardwareMap);

AprilTagCamera[] cameras = new AprilTagCamera[3];
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE));
cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0));
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE));
AprilTagCamera[] cameras = bot.getCameras();

Servo purpleServo = hardwareMap.get(Servo.class, "purple");

// Search for AprilTags across the three cameras, until we find one or init ends
AprilTagLocalizer aprilTag = new AprilTagLocalizer(cameras);
Expand Down Expand Up @@ -126,7 +117,8 @@ public void runOpMode() throws InterruptedException {
.turn(Math.toRadians(90))
.addTemporalMarker(() ->{
android.util.Log.i("DROP", "Drop");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})


Expand Down Expand Up @@ -154,7 +146,8 @@ public void runOpMode() throws InterruptedException {
.addTemporalMarker(()->{
//PIXEL DROP
Log.i("DROP", "dropping purple");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})
.waitSeconds(1.0)
// Back up to the BLUE PARK Y coord
Expand Down Expand Up @@ -213,7 +206,8 @@ public void runOpMode() throws InterruptedException {
.lineTo(new Vector2d(startPose.getX(), -2*12 - 3))
.addTemporalMarker(() -> {
Log.i("DROP", "dropping purple");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})
.waitSeconds(1)
.lineTo(new Vector2d(startPose.getX(), -5 * 12+3))
Expand Down Expand Up @@ -286,7 +280,8 @@ public void runOpMode() throws InterruptedException {
.addTemporalMarker(()->{
//PIXEL DROP
Log.i("DROP", "dropping purple");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})
.waitSeconds(1.0)
// Back up to the BLUE PARK Y coord
Expand All @@ -306,7 +301,8 @@ public void runOpMode() throws InterruptedException {
.addTemporalMarker(()->{
//PIXEL DROP
Log.i("DROP", "dropping purple");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})
.lineTo(new Vector2d(3*12+7, 3*12-2))
.build();
Expand All @@ -319,7 +315,8 @@ public void runOpMode() throws InterruptedException {
.addTemporalMarker(()->{
//PIXEL DROP
Log.i("DROP", "dropping purple");
purpleServo.setPosition(.3);
bot.dropPurplePixel(true);
//purpleServo.setPosition(.3);
})
.waitSeconds(1.0)
// Back up to the BLUE PARK Y coord
Expand Down

0 comments on commit 970045d

Please sign in to comment.