KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

@DANIEELL

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-04 19:38:50 -06:00
parent 24473aeabb
commit a278fc0489
8 changed files with 268 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -13,5 +13,9 @@ public class ServoPositions {
public static double spindexer_outtakeBall2 = 0.48;
public static double spindexer_outtakeBall1 = 0.1;
public static double transferServo_out = 0.13;
public static double transferServo_in = 0.4;
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -34,6 +34,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
@@ -57,33 +58,33 @@ public final class MecanumDrive {
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
// drive model parameters
public double inPerTick = 1;
public double lateralInPerTick = inPerTick;
public double trackWidthTicks = 0;
public double inPerTick = 0.001978956;
public double lateralInPerTick = 0.0013863732202094405;
public double trackWidthTicks = 6488.883015684446;
// feedforward parameters (in tick units)
public double kS = 0;
public double kV = 0;
public double kA = 0;
public double kS = 1.2147826978829488;
public double kV = 0.00032;
public double kA = 0.000046;
// path profile parameters (in inches)
public double maxWheelVel = 50;
public double maxWheelVel = 80;
public double minProfileAccel = -30;
public double maxProfileAccel = 50;
public double maxProfileAccel = 80;
// turn profile parameters (in radians)
public double maxAngVel = Math.PI; // shared with path
public double maxAngAccel = Math.PI;
public double maxAngVel = 2* Math.PI; // shared with path
public double maxAngAccel = 2* Math.PI;
// path controller gains
public double axialGain = 0.0;
public double lateralGain = 0.0;
public double headingGain = 0.0; // shared with turn
public double axialGain = 4;
public double lateralGain = 4;
public double headingGain = 4; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
@@ -224,10 +225,10 @@ public final class MecanumDrive {
// TODO: make sure your config has motors with these names (or change them)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
rightBack = hardwareMap.get(DcMotorEx.class, "br");
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -235,7 +236,10 @@ public final class MecanumDrive {
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
//
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
@@ -244,7 +248,7 @@ public final class MecanumDrive {
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new DriveLocalizer(pose);
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java
View File

@@ -16,8 +16,8 @@ import java.util.Objects;
@Config
public final class PinpointLocalizer implements Localizer {
public static class Params {
public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
public double parYTicks = 3765.023079161767; // y position of the parallel encoder (in tick units)
public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
}
public static Params PARAMS = new Params();
@@ -38,7 +38,7 @@ public final class PinpointLocalizer implements Localizer {
driver.setOffsets(mmPerTick * PARAMS.parYTicks, mmPerTick * PARAMS.perpXTicks, DistanceUnit.MM);
// TODO: reverse encoder directions if needed
initialParDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
initialParDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
initialPerpDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
driver.setEncoderDirections(initialParDirection, initialPerpDirection);

109
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
View File

@@ -9,6 +9,8 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
public class Spindexer implements Subsystem{
private Servo s1;
@@ -34,6 +36,21 @@ public class Spindexer implements Subsystem{
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;
@@ -59,16 +76,40 @@ public class Spindexer implements Subsystem{
}
public void colorSensorTelemetry() {
TELE.addData("pin0", p0.getState());
TELE.addData("pin1", p1.getState());
TELE.addData("pin2", p2.getState());
TELE.addData("pin3", p3.getState());
TELE.addData("pin4", p4.getState());
TELE.addData("pin5", p5.getState());
TELE.addData("Analog", (input.getVoltage()));
TELE.addData("Analog2", (input2.getVoltage()));
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) {
@@ -79,6 +120,15 @@ public class Spindexer implements Subsystem{
position = spindexer_intakePos;
}
public void intakeShake(double runtime) {
if ((runtime % 0.33) >0.167) {
position = spindexer_intakePos + 0.02;
} else {
position = spindexer_intakePos - 0.02;
}
}
public void outtake3 () {
position = spindexer_outtakeBall3;
}
@@ -91,6 +141,47 @@ public class Spindexer implements Subsystem{
position = spindexer_outtakeBall1;
}
public void outtakeGreen() {
if (green0) {
outtake1();
} else if (green1){
outtake2();
} else if (green2) {
outtake3();
}
}
public void outtakePurpleFs() {
if (!green0 && ball0) {
outtake1();
} else if (!green1 && ball1){
outtake2();
} else if (!green2 && ball2) {
outtake3();
}
}
public void outtakeGreenFs() {
if (green0 && ball0) {
outtake1();
} else if (green1 && ball1){
outtake2();
} else if (green2 && ball2) {
outtake3();
}
}
public void outtakePurple() {
if (!green0) {
outtake1();
} else if (!green1){
outtake2();
} else if (!green2) {
outtake3();
}
}
@Override
@@ -107,5 +198,7 @@ public class Spindexer implements Subsystem{
colorSensorTelemetry();
}
checkForBalls();
}
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java Normal file
View File

@@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Servo;
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;
}
@Override
public void update() {
if (servoPos!=0.501){
servo.setPosition(servoPos);
}
transfer.setPower(motorPow);
}
}

73
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -12,6 +12,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -29,16 +30,30 @@ public class TeleopV1 extends LinearOpMode {
Spindexer spindexer;
Transfer transfer;
MultipleTelemetry TELE;
GamepadEx g1;
GamepadEx g2;
public static double defaultSpeed = 1;
public static double slowMoSpeed = 0.4;
public static double power = 0.0;
public static double pos = 0.501;
ToggleButtonReader g1RightBumper;
ToggleButtonReader g2Circle;
ToggleButtonReader g2Square;
ToggleButtonReader g2Triangle;
public double g1RightBumperStamp = 0.0;
public static int spindexerPos = 0;
@@ -61,6 +76,21 @@ public class TeleopV1 extends LinearOpMode {
g1, GamepadKeys.Button.RIGHT_BUMPER
);
g2 = new GamepadEx(gamepad2);
g2Circle = new ToggleButtonReader(
g2, GamepadKeys.Button.B
);
g2Triangle = new ToggleButtonReader(
g2, GamepadKeys.Button.Y
);
g2Square = new ToggleButtonReader(
g2, GamepadKeys.Button.X
);
drivetrain = new Drivetrain(robot, TELE, g1);
@@ -72,6 +102,8 @@ public class TeleopV1 extends LinearOpMode {
intake = new Intake(robot);
transfer = new Transfer(robot);
spindexer = new Spindexer(robot, TELE);
@@ -90,7 +122,16 @@ public class TeleopV1 extends LinearOpMode {
TELE.update();
spindexer.update();
transfer.setTransferPower(power);
transfer.setTransferPosition(pos);
transfer.update();
@@ -107,6 +148,12 @@ public class TeleopV1 extends LinearOpMode {
g1RightBumper.readValue();
g2Circle.readValue();
g2Square.readValue();
g2Triangle.readValue();
if (g1RightBumper.wasJustPressed()){
@@ -118,15 +165,39 @@ public class TeleopV1 extends LinearOpMode {
spindexer.intake();
g1RightBumperStamp = getRuntime();
}
if (intake.getIntakeState()==1) {
spindexer.intakeShake(getRuntime());
} else {
if (g2Circle.wasJustPressed()){
spindexer.outtake3();
}
if (g2Triangle.wasJustPressed()){
spindexer.outtake2();
}
if (g2Square.wasJustPressed()){
spindexer.outtake1();
}
}
intake.update();
spindexer.update();
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
View File

@@ -15,7 +15,11 @@ public class ConfigureColorRangefinder extends LinearOpMode {
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
pin0 --> purple
pin1 --> green */
crf.setPin0Analog(ColorRangefinder.AnalogMode.HSV);
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0 / 360.0 * 255, 360 / 360.0 * 255); // purple
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green
crf.setLedBrightness(2);
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -24,10 +24,16 @@ public class Robot {
public DcMotorEx intake;
public DcMotorEx transfer;
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;
public Servo transferServo;
public Servo rejecter;
public Servo turr1;
@@ -73,10 +79,10 @@ public class Robot {
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
intake = hardwareMap.get(DcMotorEx.class, "intake");
rejecter = hardwareMap.get(Servo.class, "rejecter");
@@ -116,7 +122,9 @@ public class Robot {
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
transfer = hardwareMap.get(DcMotorEx.class, "transfer");
transferServo = hardwareMap.get(Servo.class, "transferServo");
}