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

Merge branch 'master' into daniel

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
This commit is contained in:
DanTheMan-byte
2025-11-04 19:42:06 -06:00
parent 96f4f1c639 a278fc0489
commit de096a925c
11 changed files with 873 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java Normal file
View File

@@ -0,0 +1,21 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos = 0.665;
public static double spindexer_outtakeBall3 = 0.845;
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);

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View File

@@ -55,7 +55,7 @@ public class Drivetrain implements Subsystem {
this.slowSpeed = speed;
}
public void FieldCentric(double fwd, double strafe, double turn, double turbo){
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
@@ -83,7 +83,7 @@ public class Drivetrain implements Subsystem {
if (Objects.equals(Mode, "Default")) {
FieldCentric(
RobotCentric(
gamepad.getRightY(),
gamepad.getRightX(),
gamepad.getLeftX(),

63
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
View File

@@ -1,56 +1,77 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoVars.*;
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 com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Intake implements Subsystem {
private GamepadEx gamepad;
public MultipleTelemetry TELE;
private final DcMotorEx intake;
private final Servo kicker;
private DcMotorEx intake;
private double intakePower = 1.0;
private int intakeState = 0;
private double intakePower = 0.0;
public Intake (Robot robot){
private double kickerPos = 0.0;
public Intake (Robot robot, MultipleTelemetry telemetry){
this.intake = robot.intake;
this.kicker = robot.rejecter;
}
public void setIntakePower (double pow){
this.intakePower = pow;
public int getIntakeState() {
return intakeState;
}
public double getIntakePower() {
return this.intakePower;
public void toggle(){
if (intakeState !=0){
intakeState = 0;
} else {
intakeState = 1;
}
}
public void kickOut (){
this.kickerPos = rejecter_Out;
public void setIntakePower(double pow){
intakePower = pow;
}
public void kickIn (){
this.kickerPos = rejecter_In;
public void intake(){
intakeState =1;
}
public void reverse(){
intakeState =-1;
}
public void stop(){
intakeState =-1;
}
@Override
public void update() {
kicker.setPosition(kickerPos);
if (intakeState == 1){
intake.setPower(intakePower);
} else if (intakeState == -1){
intake.setPower(-intakePower);
} else {
intake.setPower(0);
}
}
}

204
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java Normal file
View File

@@ -0,0 +1,204 @@
package org.firstinspires.ftc.teamcode.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.utils.Robot;
import java.util.ArrayList;
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_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;
}
public void outtake2 () {
position = spindexer_outtakeBall2;
}
public void outtake1 () {
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
public void update() {
if (position !=0.501) {
s1.setPosition(position);
s2.setPosition(1 - position);
}
if (telemetryOn) {
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);
}
}

204
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java Normal file
View File

@@ -0,0 +1,204 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
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;
@Config
@TeleOp
public class TeleopV1 extends LinearOpMode {
Robot robot;
Drivetrain drivetrain;
Intake intake;
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;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(),
telemetry
);
g1 = new GamepadEx(gamepad1);
g1RightBumper = new ToggleButtonReader(
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);
drivetrain.setMode("Default");
drivetrain.setDefaultSpeed(defaultSpeed);
drivetrain.setSlowSpeed(slowMoSpeed);
intake = new Intake(robot);
transfer = new Transfer(robot);
spindexer = new Spindexer(robot, TELE);
spindexer.setTelemetryOn(true);
waitForStart();
if (isStopRequested()) return;
while(opModeIsActive()){
intake();
drivetrain.update();
TELE.update();
transfer.setTransferPower(power);
transfer.setTransferPosition(pos);
transfer.update();
}
}
public void intake(){
g1RightBumper.readValue();
g2Circle.readValue();
g2Square.readValue();
g2Triangle.readValue();
if (g1RightBumper.wasJustPressed()){
if (getRuntime() - g1RightBumperStamp < 0.3){
intake.reverse();
} else {
intake.toggle();
}
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();
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java Normal file
View File

@@ -0,0 +1,45 @@
package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@TeleOp
public class MotorDirectionDebugger extends LinearOpMode {
public static double flPower = 0.0;
public static double frPower = 0.0;
public static double blPower = 0.0;
public static double brPower = 0.0;
Robot robot;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
waitForStart();
if(isStopRequested()) return;
while(opModeIsActive()){
robot.frontLeft.setPower(flPower);
robot.frontRight.setPower(frPower);
robot.backRight.setPower(brPower);
robot.backLeft.setPower(blPower);
}
}
}

224
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java Normal file
View File

@@ -0,0 +1,224 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
@TeleOp
public class ConfigureColorRangefinder extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
waitForStart();
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
pin0 --> purple
pin1 --> green */
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);
}
}
/**
* Helper class for configuring the Brushland Labs Color Rangefinder.
* Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
*/
class ColorRangefinder {
private final I2cDeviceSynchSimple i2c;
public ColorRangefinder(RevColorSensorV3 emulator) {
this.i2c = emulator.getDeviceClient();
this.i2c.enableWriteCoalescing(true);
}
/**
* Configure Pin 0 to be in digital mode, and add a threshold.
* Multiple thresholds can be added to the same pin by calling this function repeatedly.
* For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
*/
public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound);
}
/**
* Configure Pin 1 to be in digital mode, and add a threshold.
* Multiple thresholds can be added to the same pin by calling this function repeatedly.
* For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
*/
public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound);
}
/**
* Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
* This is most useful when we want to know if an object is both close and the correct color.
*/
public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
setPin0Digital(digitalMode, mmRequirement, mmRequirement);
}
/**
* Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger.
* This is most useful when we want to know if an object is both close and the correct color.
*/
public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
setPin1Digital(digitalMode, mmRequirement, mmRequirement);
}
/**
* Invert the hue value before thresholding it, meaning that the colors become their opposite.
* This is useful if we want to threshold red; instead of having two thresholds we would invert
* the color and look for blue.
*/
public void setPin0InvertHue() {
setPin0DigitalMaxDistance(DigitalMode.HSV, 200);
}
/**
* Invert the hue value before thresholding it, meaning that the colors become their opposite.
* This is useful if we want to threshold red; instead of having two thresholds we would invert
* the color and look for blue.
*/
public void setPin1InvertHue() {
setPin1DigitalMaxDistance(DigitalMode.HSV, 200);
}
/**
* The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog.
* For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance.
* Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range.
*/
public void setPin0Analog(AnalogMode analogMode, int denominator) {
byte denom0 = (byte) (denominator & 0xFF);
byte denom1 = (byte) ((denominator & 0xFF00) >> 8);
i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1});
}
/**
* Configure Pin 0 as analog output of one of the six data channels.
* To read analog, make sure the physical switch on the sensor is flipped away from the
* connector side.
*/
public void setPin0Analog(AnalogMode analogMode) {
setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF);
}
public float[] getCalibration() {
java.nio.ByteBuffer bytes =
java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()};
}
/**
* Save a brightness value of the LED to the sensor.
*
* @param value brightness between 0-255
*/
public void setLedBrightness(int value) {
i2c.write8(LED_BRIGHTNESS, value);
}
/**
* Change the I2C address at which the sensor will be found. The address can be reset to the
* default of 0x52 by holding the reset button.
*
* @param value new I2C address from 1 to 127
*/
public void setI2cAddress(int value) {
i2c.write8(I2C_ADDRESS_REG, value << 1);
}
/**
* Read distance via I2C
* @return distance in millimeters
*/
public double readDistance() {
java.nio.ByteBuffer bytes =
java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
return bytes.getFloat();
}
private void setDigital(
PinNum pinNum,
DigitalMode digitalMode,
double lowerBound,
double higherBound
) {
int lo, hi;
if (lowerBound == higherBound) {
lo = (int) lowerBound;
hi = (int) higherBound;
} else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255
lo = (int) Math.round(lowerBound / 255.0 * 65535);
hi = (int) Math.round(higherBound / 255.0 * 65535);
} else { // distance in mm
float[] calib = getCalibration();
if (lowerBound < .5) hi = 2048;
else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound);
lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound);
}
byte lo0 = (byte) (lo & 0xFF);
byte lo1 = (byte) ((lo & 0xFF00) >> 8);
byte hi0 = (byte) (hi & 0xFF);
byte hi1 = (byte) ((hi & 0xFF00) >> 8);
i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1});
try {
Thread.sleep(25);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
private double root(double n, double v) {
double val = Math.pow(v, 1.0 / Math.abs(n));
if (n < 0) val = 1.0 / val;
return val;
}
private int rawFromDistance(float a, float b, float c, float x0, double mm) {
return (int) (root(b, (mm - c) / a) + x0);
}
private enum PinNum {
PIN0(0x28), PIN1(0x2D);
private final byte modeAddress;
PinNum(int modeAddress) {
this.modeAddress = (byte) modeAddress;
}
}
// other writeable registers
private static final byte CALIB_A_VAL_0 = 0x32;
private static final byte PS_DISTANCE_0 = 0x42;
private static final byte LED_BRIGHTNESS = 0x46;
private static final byte I2C_ADDRESS_REG = 0x47;
public static int invertHue(int hue360) {
return ((hue360 - 180) % 360);
}
public enum DigitalMode {
RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6);
public final byte value;
DigitalMode(int value) {
this.value = (byte) value;
}
}
public enum AnalogMode {
RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18);
public final byte value;
AnalogMode(int value) {
this.value = (byte) value;
}
}
}

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

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorImplEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -22,33 +24,45 @@ 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;
public Servo turr2;
public DcMotorEx ballUpMotor;
public Servo spin1;
public Servo spin2;
public DigitalChannel pin0;
public DigitalChannel pin1;
public DigitalChannel pin2;
public DigitalChannel pin3;
public DigitalChannel pin4;
public DigitalChannel pin5;
public AnalogInput analogInput;
public AnalogInput analogInput2;
public Servo ballUpServo;
public Servo spindex1;
public Servo spindex2;
public DigitalChannel color1Green;
public DigitalChannel color1Purple;
public DigitalChannel color2Green;
public DigitalChannel color2Purple;
public DigitalChannel color3Green;
public DigitalChannel color3Purple;
@@ -62,33 +76,55 @@ public class Robot {
frontRight = hardwareMap.get(DcMotorEx.class, "fr");
backLeft = hardwareMap.get(DcMotorEx.class, "bl");
backRight = hardwareMap.get(DcMotorEx.class, "br");
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
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");
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
hood = hardwareMap.get(Servo.class, "hood");
turr1 = hardwareMap.get(Servo.class, "t1");
turr2 = hardwareMap.get(Servo.class, "t2");
ballUpMotor = hardwareMap.get(DcMotorEx.class, "ballUpMotor");
ballUpServo = hardwareMap.get(Servo.class, "ballUpServo");
spin1 = hardwareMap.get(Servo.class, "spin1");
spindex1 = hardwareMap.get(Servo.class, "spindex1");
spindex2 = hardwareMap.get(Servo.class, "spindex2");
spin2 = hardwareMap.get(Servo.class, "spin2");
color1Green = hardwareMap.get(DigitalChannel.class, "color1Green");
color1Purple = hardwareMap.get(DigitalChannel.class, "color1Purple");
pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
color2Green = hardwareMap.get(DigitalChannel.class, "color2Green");
color2Purple = hardwareMap.get(DigitalChannel.class, "color2Purple");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
color3Green = hardwareMap.get(DigitalChannel.class, "color1Green");
color3Purple = hardwareMap.get(DigitalChannel.class, "color3Purple");
pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
analogInput = hardwareMap.get(AnalogInput.class, "analog");
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
transfer = hardwareMap.get(DcMotorEx.class, "transfer");
transferServo = hardwareMap.get(Servo.class, "transferServo");
}