Telelop drivetrain
This commit is contained in:
@@ -1,14 +1,48 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class TeleopV2 extends LinearOpMode {
|
||||
|
||||
Rob
|
||||
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
while (opModeIsActive()) {
|
||||
|
||||
//DRIVETRAIN:
|
||||
|
||||
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
|
||||
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
|
||||
double rx = gamepad1.left_stick_x;
|
||||
|
||||
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
robot.frontLeft.setPower(frontLeftPower);
|
||||
robot.backLeft.setPower(backLeftPower);
|
||||
robot.frontRight.setPower(frontRightPower);
|
||||
robot.backRight.setPower(backRightPower);
|
||||
|
||||
TELE.update();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,238 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.vision.VisionPortal;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
|
||||
public class AprilTag implements Subsystem {
|
||||
|
||||
private AprilTagProcessor aprilTag;
|
||||
private VisionPortal visionPortal;
|
||||
|
||||
private MultipleTelemetry TELE;
|
||||
|
||||
private boolean teleOn = false;
|
||||
|
||||
private int detections = 0;
|
||||
|
||||
List<AprilTagDetection> currentDetections;
|
||||
|
||||
ArrayList<ArrayList<Double>> Data = new ArrayList<>();
|
||||
|
||||
|
||||
|
||||
public AprilTag(Robot robot, MultipleTelemetry tele) {
|
||||
|
||||
|
||||
|
||||
this.aprilTag = robot.aprilTagProcessor;
|
||||
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
currentDetections = aprilTag.getDetections();
|
||||
|
||||
UpdateData();
|
||||
|
||||
if(teleOn){
|
||||
tagTELE();
|
||||
initTelemetry();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void initTelemetry (){
|
||||
|
||||
TELE.addData("Camera Preview", "Check Driver Station for stream");
|
||||
TELE.addData("Status", "Initialized - Press START");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void tagTELE () {
|
||||
|
||||
TELE.addData("# AprilTags Detected", detections);
|
||||
|
||||
// Display info for each detected tag
|
||||
for (ArrayList<Double> detection : Data) {
|
||||
if (detection.get(0) ==1) {
|
||||
// Known AprilTag with metadata
|
||||
TELE.addLine(String.format("\n==== (ID %d) %s ====",
|
||||
detection.get(1).intValue(), ""));
|
||||
|
||||
TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)",
|
||||
detection.get(2),
|
||||
detection.get(3),
|
||||
detection.get(4)));
|
||||
|
||||
TELE.addData("Distance", getDistance(detection.get(1).intValue()));
|
||||
|
||||
TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)",
|
||||
detection.get(5),
|
||||
detection.get(6),
|
||||
detection.get(7)));
|
||||
|
||||
TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)",
|
||||
detection.get(8),
|
||||
detection.get(9),
|
||||
detection.get(10)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void turnTelemetryOn(boolean bool) {
|
||||
|
||||
teleOn = bool;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void UpdateData () {
|
||||
|
||||
Data.clear(); // <--- THIS FIXES YOUR ISSUE
|
||||
|
||||
|
||||
detections = currentDetections.size();
|
||||
|
||||
|
||||
for (AprilTagDetection detection : currentDetections) {
|
||||
|
||||
ArrayList<Double> detectionData = new ArrayList<Double>();
|
||||
|
||||
|
||||
|
||||
if (detection.metadata != null) {
|
||||
|
||||
detectionData.add(1.0);
|
||||
// Known AprilTag with metadata
|
||||
|
||||
detectionData.add( (double) detection.id);
|
||||
|
||||
|
||||
|
||||
detectionData.add(detection.ftcPose.x);
|
||||
detectionData.add(detection.ftcPose.y);
|
||||
detectionData.add(detection.ftcPose.z);
|
||||
|
||||
|
||||
|
||||
detectionData.add(detection.ftcPose.pitch);
|
||||
detectionData.add(detection.ftcPose.roll);
|
||||
detectionData.add(detection.ftcPose.yaw);
|
||||
|
||||
detectionData.add(detection.ftcPose.range);
|
||||
detectionData.add(detection.ftcPose.bearing);
|
||||
detectionData.add(detection.ftcPose.elevation);
|
||||
|
||||
} else {
|
||||
|
||||
detectionData.add(0, 0.0);
|
||||
|
||||
}
|
||||
|
||||
Data.add(detectionData);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public int getDetectionCount() {
|
||||
|
||||
return detections;
|
||||
|
||||
}
|
||||
|
||||
public boolean isDetected (int id){
|
||||
return (!filterID(Data, (double) id ).isEmpty());
|
||||
}
|
||||
|
||||
|
||||
|
||||
public double getDistance(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 5) {
|
||||
double x = d.get(2);
|
||||
double y = d.get(3);
|
||||
double z = d.get(4);
|
||||
return Math.sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
return -1; // tag not found
|
||||
}
|
||||
|
||||
// Returns the position as [x, y, z]
|
||||
public List<Double> getPosition(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 5) {
|
||||
List<Double> pos = new ArrayList<>();
|
||||
pos.add(d.get(2));
|
||||
pos.add(d.get(3));
|
||||
pos.add(d.get(4));
|
||||
return pos;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns orientation as [pitch, roll, yaw]
|
||||
public List<Double> getOrientation(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 8) {
|
||||
List<Double> ori = new ArrayList<>();
|
||||
ori.add(d.get(5));
|
||||
ori.add(d.get(6));
|
||||
ori.add(d.get(7));
|
||||
return ori;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns range, bearing, elevation as [range, bearing, elevation]
|
||||
public List<Double> getRBE(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 11) {
|
||||
List<Double> rbe = new ArrayList<>();
|
||||
rbe.add(d.get(8));
|
||||
rbe.add(d.get(9));
|
||||
rbe.add(d.get(10));
|
||||
return rbe;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns full raw data for debugging or custom processing
|
||||
public ArrayList<Double> getRawData(int id) {
|
||||
return filterID(Data, (double) id);
|
||||
}
|
||||
|
||||
public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
|
||||
for (ArrayList<Double> innerList : data) {
|
||||
// Ensure it has a second element
|
||||
if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
|
||||
return innerList; // Return the first match
|
||||
}
|
||||
}
|
||||
// Return an empty ArrayList if no match found
|
||||
return new ArrayList<>();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
public class Drivetrain implements Subsystem {
|
||||
|
||||
private final GamepadEx gamepad;
|
||||
|
||||
public MultipleTelemetry TELE;
|
||||
|
||||
private String Mode = "Default";
|
||||
|
||||
private final DcMotorEx fl;
|
||||
|
||||
private final DcMotorEx fr;
|
||||
private final DcMotorEx bl;
|
||||
private final DcMotorEx br;
|
||||
|
||||
private double defaultSpeed = 0.7;
|
||||
|
||||
private double slowSpeed = 0.3;
|
||||
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
|
||||
|
||||
this.fl = robot.frontLeft;
|
||||
this.fr = robot.frontRight;
|
||||
this.br = robot.backRight;
|
||||
this.bl = robot.backLeft;
|
||||
|
||||
this.gamepad = gamepad1;
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void setMode (String mode){
|
||||
this.Mode = mode;
|
||||
}
|
||||
|
||||
public void setDefaultSpeed (double speed){
|
||||
this.defaultSpeed = speed;
|
||||
}
|
||||
|
||||
public void setSlowSpeed (double speed){
|
||||
this.slowSpeed = speed;
|
||||
}
|
||||
|
||||
public void RobotCentric(double fwd, double strafe, double turn, double turbo){
|
||||
|
||||
double y = -fwd; // Remember, Y stick value is reversed
|
||||
double x = strafe * 1.1; // Counteract imperfect strafing
|
||||
double rx = turn;
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio,
|
||||
// but only if at least one is out of the range [-1, 1]
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
fl.setPower(frontLeftPower*turbo);
|
||||
bl.setPower(backLeftPower*turbo);
|
||||
fr.setPower(frontRightPower*turbo);
|
||||
br.setPower(backRightPower*turbo);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (Objects.equals(Mode, "Default")) {
|
||||
|
||||
RobotCentric(
|
||||
gamepad.getRightY(),
|
||||
gamepad.getRightX(),
|
||||
gamepad.getLeftX(),
|
||||
(gamepad.getTrigger(
|
||||
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed)
|
||||
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
|
||||
+ defaultSpeed
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Intake implements Subsystem {
|
||||
|
||||
private GamepadEx gamepad;
|
||||
|
||||
public MultipleTelemetry TELE;
|
||||
|
||||
|
||||
private DcMotorEx intake;
|
||||
|
||||
private double intakePower = 1.0;
|
||||
|
||||
private int intakeState = 0;
|
||||
|
||||
|
||||
public Intake (Robot robot){
|
||||
|
||||
|
||||
this.intake = robot.intake;
|
||||
|
||||
|
||||
}
|
||||
|
||||
public int getIntakeState() {
|
||||
return intakeState;
|
||||
}
|
||||
|
||||
public void toggle(){
|
||||
if (intakeState !=0){
|
||||
intakeState = 0;
|
||||
} else {
|
||||
intakeState = 1;
|
||||
}
|
||||
}
|
||||
|
||||
public void intakeMinPower(){
|
||||
intakeState = 2;
|
||||
}
|
||||
|
||||
public void intake(){
|
||||
intakeState =1;
|
||||
}
|
||||
|
||||
public void reverse(){
|
||||
intakeState =-1;
|
||||
}
|
||||
|
||||
|
||||
public void stop(){
|
||||
intakeState =0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (intakeState == 1){
|
||||
intake.setPower(intakePower);
|
||||
} else if (intakeState == -1){
|
||||
intake.setPower(-intakePower);
|
||||
} else if (intakeState == 2){
|
||||
intake.setPower(intakePower);
|
||||
}else {
|
||||
intake.setPower(0);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.arcrobotics.ftclib.controller.PIDFController;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.PIDCoefficients;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.constants.Poses;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
public class Shooter implements Subsystem {
|
||||
private final DcMotorEx fly1;
|
||||
private final DcMotorEx fly2;
|
||||
|
||||
private final DcMotorEx encoder;
|
||||
private final Servo hoodServo;
|
||||
|
||||
private final Servo turret1;
|
||||
|
||||
private final Servo turret2;
|
||||
|
||||
private final MultipleTelemetry telemetry;
|
||||
|
||||
private boolean telemetryOn = false;
|
||||
|
||||
private double manualPower = 0.0;
|
||||
private double hoodPos = 0.0;
|
||||
|
||||
private double turretPos = 0.0;
|
||||
private double velocity = 0.0;
|
||||
private double posPower = 0.0;
|
||||
|
||||
public double velo = 0.0;
|
||||
|
||||
private int targetPosition = 0;
|
||||
|
||||
public double powPID = 1.0;
|
||||
|
||||
private double p = 0.0003, i = 0, d = 0.00001, f=0;
|
||||
|
||||
private PIDFController controller;
|
||||
private double pow = 0.0;
|
||||
|
||||
private String shooterMode = "AUTO";
|
||||
|
||||
private String turretMode = "AUTO";
|
||||
|
||||
public Shooter(Robot robot, MultipleTelemetry TELE) {
|
||||
this.fly1 = robot.shooter1;
|
||||
this.fly2 = robot.shooter2;
|
||||
this.telemetry = TELE;
|
||||
this.hoodServo = robot.hood;
|
||||
|
||||
// Reset encoders
|
||||
fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
controller = new PIDFController(p, i, d, f);
|
||||
|
||||
controller.setPIDF(p, i, d, f);
|
||||
|
||||
this.turret1 = robot.turr1;
|
||||
|
||||
this.turret2 = robot.turr2;
|
||||
|
||||
this.encoder = robot.shooterEncoder;
|
||||
|
||||
}
|
||||
|
||||
public double gethoodPosition() {
|
||||
return (hoodServo.getPosition());
|
||||
}
|
||||
|
||||
public void sethoodPosition(double pos) { hoodPos = pos; }
|
||||
|
||||
public double getTurretPosition() {
|
||||
return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2);
|
||||
}
|
||||
|
||||
public void setTurretPosition(double pos) { turretPos = pos; }
|
||||
|
||||
public double getVelocity(double vel) {
|
||||
return vel;
|
||||
}
|
||||
|
||||
public void setVelocity(double vel) { velocity = vel; }
|
||||
|
||||
public void setPosPower(double power) { posPower = power; }
|
||||
|
||||
public void setTargetPosition(int pos) {
|
||||
targetPosition = pos;
|
||||
}
|
||||
|
||||
public void setTolerance(int tolerance) {
|
||||
controller.setTolerance(tolerance);
|
||||
}
|
||||
|
||||
public void setControllerCoefficients(double kp, double ki, double kd, double kf) {
|
||||
p = kp;
|
||||
i = ki;
|
||||
d = kd;
|
||||
f = kf;
|
||||
controller.setPIDF(p, i, d, f);
|
||||
|
||||
}
|
||||
|
||||
public PIDCoefficients getControllerCoefficients() {
|
||||
|
||||
return new PIDCoefficients(p, i, d);
|
||||
|
||||
}
|
||||
|
||||
public void setManualPower(double power) { manualPower = power; }
|
||||
|
||||
public String getShooterMode() { return shooterMode; }
|
||||
|
||||
public String getTurretMode() { return turretMode; }
|
||||
|
||||
public double getECPRPosition() {
|
||||
return fly1.getCurrentPosition() / (2 * ecpr);
|
||||
}
|
||||
|
||||
public double getMCPRPosition() {
|
||||
return (double) fly1.getCurrentPosition() / 4;
|
||||
}
|
||||
|
||||
public void setShooterMode(String mode) { shooterMode = mode; }
|
||||
|
||||
public void setTurretMode(String mode) { turretMode = mode; }
|
||||
|
||||
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) {
|
||||
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
Pose2d deltaPose = new Pose2d(
|
||||
goalPose.position.x - robotPose.position.x,
|
||||
goalPose.position.y - robotPose.position.y,
|
||||
goalPose.heading.toDouble() - (robotPose.heading.toDouble())
|
||||
);
|
||||
|
||||
double distance = Math.sqrt(
|
||||
deltaPose.position.x * deltaPose.position.x
|
||||
+ deltaPose.position.y * deltaPose.position.y
|
||||
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight
|
||||
);
|
||||
|
||||
telemetry.addData("dst", distance);
|
||||
|
||||
double shooterPow = getPowerByDist(distance);
|
||||
|
||||
double hoodAngle = getAngleByDist(distance);
|
||||
|
||||
// hoodServo.setPosition(hoodAngle);
|
||||
|
||||
moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
|
||||
|
||||
return distance;
|
||||
|
||||
//0.9974 * 355
|
||||
|
||||
}
|
||||
|
||||
public double getTurretPosByDeltaPose(Pose2d dPose, double offset) {
|
||||
|
||||
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
|
||||
|
||||
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x));
|
||||
|
||||
telemetry.addData("deltaAngle", deltaAngle);
|
||||
|
||||
if (deltaAngle > 90) {
|
||||
deltaAngle -= 360;
|
||||
}
|
||||
|
||||
// deltaAngle += aTanAngle;
|
||||
|
||||
deltaAngle /= (335);
|
||||
|
||||
telemetry.addData("dAngle", deltaAngle);
|
||||
|
||||
telemetry.addData("AtanAngle", aTanAngle);
|
||||
|
||||
return ((0.30 - deltaAngle) + offset);
|
||||
|
||||
}
|
||||
|
||||
//62, 0.44
|
||||
|
||||
//56.5, 0.5
|
||||
|
||||
public double getPowerByDist(double dist) {
|
||||
|
||||
//TODO: ADD LOGIC
|
||||
return dist;
|
||||
}
|
||||
|
||||
public double getAngleByDist(double dist) {
|
||||
|
||||
double newDist = dist - 56.5;
|
||||
|
||||
double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
public void setTelemetryOn(boolean state) { telemetryOn = state; }
|
||||
|
||||
public void moveTurret(double pos) {
|
||||
turret1.setPosition(pos);
|
||||
turret2.setPosition(1 - pos);
|
||||
}
|
||||
|
||||
public double getpowPID() {
|
||||
return powPID;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (Objects.equals(shooterMode, "MANUAL")) {
|
||||
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
fly1.setPower(manualPower);
|
||||
fly2.setPower(manualPower);
|
||||
} else if (Objects.equals(shooterMode, "VEL")) {
|
||||
powPID = velocity;
|
||||
|
||||
fly1.setPower(powPID);
|
||||
fly2.setPower(powPID);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,256 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.hardware.AnalogInput;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Spindexer implements Subsystem {
|
||||
|
||||
private Servo s1;
|
||||
private Servo s2;
|
||||
|
||||
private DigitalChannel p0;
|
||||
|
||||
private DigitalChannel p1;
|
||||
private DigitalChannel p2;
|
||||
private DigitalChannel p3;
|
||||
private DigitalChannel p4;
|
||||
|
||||
private DigitalChannel p5;
|
||||
|
||||
private AnalogInput input;
|
||||
|
||||
private AnalogInput input2;
|
||||
|
||||
|
||||
private MultipleTelemetry TELE;
|
||||
|
||||
private double position = 0.501;
|
||||
|
||||
private boolean telemetryOn = false;
|
||||
|
||||
private boolean ball0 = false;
|
||||
|
||||
private boolean ball1 = false;
|
||||
|
||||
private boolean ball2 = false;
|
||||
|
||||
private boolean green0 = false;
|
||||
|
||||
private boolean green1 = false;
|
||||
|
||||
private boolean green2 = false;
|
||||
|
||||
|
||||
|
||||
|
||||
public Spindexer (Robot robot, MultipleTelemetry tele){
|
||||
|
||||
this.s1 = robot.spin1;
|
||||
this.s2 = robot.spin2;
|
||||
|
||||
this.p0 = robot.pin0;
|
||||
this.p1 = robot.pin1;
|
||||
this.p2 = robot.pin2;
|
||||
this.p3 = robot.pin3;
|
||||
this.p4 = robot.pin4;
|
||||
this.p5 = robot.pin5;
|
||||
|
||||
this.input = robot.analogInput;
|
||||
|
||||
this.input2 = robot.analogInput2;
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
}
|
||||
|
||||
public void setTelemetryOn(boolean state){
|
||||
telemetryOn = state;
|
||||
}
|
||||
|
||||
public void colorSensorTelemetry() {
|
||||
|
||||
|
||||
TELE.addData("ball0", ball0);
|
||||
TELE.addData("ball1", ball1);
|
||||
TELE.addData("ball2", ball2);
|
||||
TELE.addData("green0", green0);
|
||||
TELE.addData("green1", green1);
|
||||
TELE.addData("green2", green2);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void checkForBalls() {
|
||||
|
||||
if (p0.getState()){
|
||||
ball0 = true;
|
||||
green0 = p1.getState();
|
||||
} else {
|
||||
ball0 = false;
|
||||
}
|
||||
|
||||
if (p2.getState()){
|
||||
ball1 = true;
|
||||
green1 = p3.getState();
|
||||
} else {
|
||||
ball1 = false;
|
||||
}
|
||||
|
||||
if (p4.getState()){
|
||||
ball2 = true;
|
||||
green2 = p5.getState();
|
||||
} else {
|
||||
ball2 = false;
|
||||
}
|
||||
}
|
||||
|
||||
public void setPosition (double pos) {
|
||||
position = pos;
|
||||
}
|
||||
|
||||
public void intake () {
|
||||
position = spindexer_intakePos1;
|
||||
}
|
||||
|
||||
public void intakeShake(double runtime) {
|
||||
if ((runtime % 0.25) >0.125) {
|
||||
position = spindexer_intakePos1 + 0.04;
|
||||
} else {
|
||||
position = spindexer_intakePos1 - 0.04;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void outtake3Shake(double runtime) {
|
||||
if ((runtime % 0.25) >0.125) {
|
||||
position = spindexer_outtakeBall3 + 0.04;
|
||||
} else {
|
||||
position = spindexer_outtakeBall3 - 0.04;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void outtake3 () {
|
||||
position = spindexer_outtakeBall3;
|
||||
}
|
||||
|
||||
public void outtake2 () {
|
||||
position = spindexer_outtakeBall2;
|
||||
}
|
||||
|
||||
public void outtake1 () {
|
||||
position = spindexer_outtakeBall1;
|
||||
}
|
||||
|
||||
|
||||
public int outtakeGreen(int secLast, int Last) {
|
||||
if (green2 && (secLast!=3) && (Last!=3)) {
|
||||
outtake3();
|
||||
return 3;
|
||||
} else if (green1 && (secLast!=2) && (Last!=2)){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else if (green0 && (secLast!=1) && (Last!=1)) {
|
||||
outtake1();
|
||||
return 1;
|
||||
} else {
|
||||
|
||||
if (secLast!=1 && Last!= 1){
|
||||
outtake1();
|
||||
return 1;
|
||||
} else if (secLast!=2 && Last!=2){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else {
|
||||
outtake3();
|
||||
return 3;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void outtakeGreenFs() {
|
||||
if (green0 && ball0) {
|
||||
outtake1();
|
||||
} else if (green1 && ball1){
|
||||
outtake2();
|
||||
} else if (green2 && ball2) {
|
||||
outtake3();
|
||||
}
|
||||
}
|
||||
|
||||
public int greens() {
|
||||
int num = 0;
|
||||
|
||||
if (green0){num++;}
|
||||
|
||||
if (green1){num++;}
|
||||
|
||||
|
||||
if (green2){num++;}
|
||||
|
||||
return num;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public int outtakePurple(int secLast, int Last) {
|
||||
if (!green2 && (secLast!=3) && (Last!=3)) {
|
||||
outtake3();
|
||||
return 3;
|
||||
} else if (!green1 && (secLast!=2) && (Last!=2)){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else if (!green0 && (secLast!=1) && (Last!=1)) {
|
||||
outtake1();
|
||||
return 1;
|
||||
} else {
|
||||
|
||||
if (secLast!=1 && Last!= 1){
|
||||
outtake1();
|
||||
return 1;
|
||||
} else if (secLast!=2 && Last!=2){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else {
|
||||
outtake3();
|
||||
return 3;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (position !=0.501) {
|
||||
|
||||
s1.setPosition(position);
|
||||
s2.setPosition(1 - position);
|
||||
}
|
||||
|
||||
|
||||
if (telemetryOn) {
|
||||
colorSensorTelemetry();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
public interface Subsystem {
|
||||
|
||||
public void update();
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop.old.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Transfer implements Subsystem {
|
||||
|
||||
private final Servo servo;
|
||||
|
||||
private final DcMotorEx transfer;
|
||||
|
||||
private double motorPow = 0.0;
|
||||
|
||||
private double servoPos = 0.501;
|
||||
|
||||
public Transfer (Robot robot){
|
||||
|
||||
this.servo = robot.transferServo;
|
||||
|
||||
this.transfer = robot.transfer;
|
||||
|
||||
}
|
||||
|
||||
public void setTransferPosition(double pos){
|
||||
this.servoPos = pos;
|
||||
}
|
||||
|
||||
public void setTransferPower (double pow){
|
||||
this.motorPow = pow;
|
||||
}
|
||||
|
||||
public void transferOut(){
|
||||
this.setTransferPosition(transferServo_out);
|
||||
}
|
||||
|
||||
public void transferIn(){
|
||||
this.setTransferPosition(transferServo_in);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (servoPos!=0.501){
|
||||
servo.setPosition(servoPos);
|
||||
}
|
||||
|
||||
transfer.setPower(motorPow);
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user