From 4b222f62c17779734697089d8a44deeaa8a290f8 Mon Sep 17 00:00:00 2001 From: J A Date: Tue, 9 Sep 2025 09:12:28 -0700 Subject: [PATCH 01/31] Odometry computer implemented and working --- .../DummyClasses/WebcamInitNotes.java | 106 +-- .../Modules/Utils/GoBildaPinpoint.java | 81 ++ .../Modules/Utils/GoBildaPinpointDriver.java | 738 ++++++++++++++++++ .../ftc/teamcode/Tuning/SlidesPIDFTuner.java | 4 +- 4 files changed, 874 insertions(+), 55 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java index e2ad3f297782..a2f4c390df46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java @@ -1,53 +1,53 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; - -@Autonomous -public class WebcamInitNotes extends OpMode -{ - - // why does Android studio have a spell checker - // webcamdude is the webcam. I'm not naming it anything else - OpenCvWebcam webcamdude = null; - - @Override // This stuff happens when you click the init button - public void init() - { - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - // Acquire the camera ID - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); - //set the cam name and id to the webcam. - webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - //set webcam Pipeline - webcamdude.setPipeline(new WebcamPipelineNotes()); - - //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. - webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - telemetry.addLine("Webcam not working"); - } - }); - } - - @Override // This stuff happens when you click the play button - public void loop() - { - telemetry.addLine("Why did you click this lmao"); - } -} - +//package org.firstinspires.ftc.teamcode; +// +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +// +//import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +//import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; +//import org.openftc.easyopencv.OpenCvCamera; +//import org.openftc.easyopencv.OpenCvCameraFactory; +//import org.openftc.easyopencv.OpenCvCameraRotation; +//import org.openftc.easyopencv.OpenCvWebcam; +// +//@Autonomous +//public class WebcamInitNotes extends OpMode +//{ +// +// // why does Android studio have a spell checker +// // webcamdude is the webcam. I'm not naming it anything else +// OpenCvWebcam webcamdude = null; +// +// @Override // This stuff happens when you click the init button +// public void init() +// { +// WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); +// // Acquire the camera ID +// int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); +// //set the cam name and id to the webcam. +// webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); +// +// //set webcam Pipeline +// webcamdude.setPipeline(new WebcamPipelineNotes()); +// +// //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. +// webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { +// @Override +// public void onOpened() { +// webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); +// } +// +// @Override +// public void onError(int errorCode) { +// telemetry.addLine("Webcam not working"); +// } +// }); +// } +// +// @Override // This stuff happens when you click the play button +// public void loop() +// { +// telemetry.addLine("Why did you click this lmao"); +// } +//} +// diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java new file mode 100644 index 000000000000..5f1ff78de0a6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode.Modules.Utils; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; + +@Config +@TeleOp(name = "P2W computer") +public class GoBildaPinpoint extends LinearOpMode { + + GoBildaPinpointDriver odoComputer; + double oldTime = 0; + + @Override + public void runOpMode() throws InterruptedException { + + // INITIALIZATION + odoComputer = hardwareMap.get(GoBildaPinpointDriver.class, "odo"); + odoComputer.setOffsets(127, 299.72, DistanceUnit.MM); + + + // Set what odo pods we using + odoComputer.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + // UNCOMMENT THIS IF USING DIFFERENT PODS AND INPUT TICKS PER UNIT: odo.setEncoderResolution(13.26291192, DistanceUnit.MM); + + + odoComputer.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + // Resets position to 0,0,0 and recalibrates IMU + odoComputer.resetPosAndIMU(); + + + telemetry.addData("Status", "Initialized"); + telemetry.addData("X offset", odoComputer.getXOffset(DistanceUnit.MM)); + telemetry.addData("Y offset", odoComputer.getYOffset(DistanceUnit.MM)); + telemetry.addData("Device Version Number:", odoComputer.getDeviceVersion()); + telemetry.addData("Heading Scalar", odoComputer.getYawScalar()); + telemetry.update(); + + waitForStart(); + resetRuntime(); + while (opModeIsActive()) { + + odoComputer.update(); + + if (gamepad1.y){ + odoComputer.recalibrateIMU(); + } + + // Calculates time per each cycle and finds number of updates per seconds (frequency) + double newTime = getRuntime(); + double loopTime = newTime-oldTime; + double frequency = 1/loopTime; + oldTime = newTime; + + Pose2D pos = odoComputer.getPosition(); + + telemetry.addLine("\n Positions and Heading \n") + .addData("X Position", pos.getX(DistanceUnit.MM)) + .addData("Y Position", pos.getY(DistanceUnit.MM)) + .addData("Orientation (Degrees)", Math.toDegrees(pos.getHeading(AngleUnit.DEGREES))); + + telemetry.addLine("\n Velocities \n") + .addData("X Velocity", odoComputer.getVelX(DistanceUnit.MM)) + .addData("Y Velocity", odoComputer.getVelY(DistanceUnit.MM)) + .addData("H Velocity", odoComputer.getHeadingVelocity(UnnormalizedAngleUnit.DEGREES)); + + telemetry.addData("Status", odoComputer.getDeviceStatus()); + + telemetry.addData("Pinpoint Frequency", odoComputer.getFrequency()); //prints/gets the current refresh rate of the Pinpoint + + telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java new file mode 100644 index 000000000000..a1a75c902541 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java @@ -0,0 +1,738 @@ +/* MIT License + * Copyright (c) [2025] [Base 10 Assets, LLC] + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode.Modules.Utils; + +import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; + +import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.qualcomm.robotcore.util.TypeConversion; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.Arrays; + + +@I2cDeviceType +@DeviceProperties( + name = "goBILDA® Pinpoint Odometry Computer", + xmlTag = "goBILDAPinpoint", + description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" +) + +public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { + + private int deviceStatus = 0; + private int loopTime = 0; + private int xEncoderValue = 0; + private int yEncoderValue = 0; + private float xPosition = 0; + private float yPosition = 0; + private float hOrientation = 0; + private float xVelocity = 0; + private float yVelocity = 0; + private float hVelocity = 0; + + private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod + private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod + + //i2c address of the device + public static final byte DEFAULT_ADDRESS = 0x31; + + public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + + this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); + super.registerArmingStateCallback(false); + } + + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + @Override + protected synchronized boolean doInitialize() { + ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); + return true; + } + + @Override + public String getDeviceName() { + return "goBILDA® Pinpoint Odometry Computer"; + } + + + //Register map of the i2c device + private enum Register { + DEVICE_ID (1), + DEVICE_VERSION (2), + DEVICE_STATUS (3), + DEVICE_CONTROL (4), + LOOP_TIME (5), + X_ENCODER_VALUE (6), + Y_ENCODER_VALUE (7), + X_POSITION (8), + Y_POSITION (9), + H_ORIENTATION (10), + X_VELOCITY (11), + Y_VELOCITY (12), + H_VELOCITY (13), + MM_PER_TICK (14), + X_POD_OFFSET (15), + Y_POD_OFFSET (16), + YAW_SCALAR (17), + BULK_READ (18); + + private final int bVal; + + Register(int bVal){ + this.bVal = bVal; + } + } + + //Device Status enum that captures the current fault condition of the device + public enum DeviceStatus{ + NOT_READY (0), + READY (1), + CALIBRATING (1 << 1), + FAULT_X_POD_NOT_DETECTED (1 << 2), + FAULT_Y_POD_NOT_DETECTED (1 << 3), + FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), + FAULT_IMU_RUNAWAY (1 << 4), + FAULT_BAD_READ (1 << 5); + + private final int status; + + DeviceStatus(int status){ + this.status = status; + } + } + + //enum that captures the direction the encoders are set to + public enum EncoderDirection{ + FORWARD, + REVERSED; + } + + //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used + public enum GoBildaOdometryPods { + goBILDA_SWINGARM_POD, + goBILDA_4_BAR_POD; + } + //enum that captures a limited scope of read data. More options may be added in future update + public enum ReadData { + ONLY_UPDATE_HEADING, + } + + + /** Writes an int to the i2c device + @param reg the register to write the int to + @param i the integer to write to the register + */ + private void writeInt(final Register reg, int i){ + deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); + } + + /** + * Reads an int from a register of the i2c device + * @param reg the register to read from + * @return returns an int that contains the value stored in the read register + */ + private int readInt(Register reg){ + return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a byte array to a float value + * @param byteArray byte array to transform + * @param byteOrder order of byte array to convert + * @return the float value stored by the byte array + */ + private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ + return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); + } + + /** + * Reads a float from a register + * @param reg the register to read + * @return the float value stored in that register + */ + private float readFloat(Register reg){ + return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a float to a byte array + * @param value the float array to convert + * @return the byte array converted from the float + */ + private byte [] floatToByteArray (float value, ByteOrder byteOrder) { + return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); + } + + /** + * Writes a byte array to a register on the i2c device + * @param reg the register to write to + * @param bytes the byte array to write + */ + private void writeByteArray (Register reg, byte[] bytes){ + deviceClient.write(reg.bVal,bytes); + } + + /** + * Writes a float to a register on the i2c device + * @param reg the register to write to + * @param f the float to write + */ + private void writeFloat (Register reg, float f){ + byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); + deviceClient.write(reg.bVal,bytes); + } + + /** + * Looks up the DeviceStatus enum corresponding with an int value + * @param s int to lookup + * @return the Odometry Computer state + */ + private DeviceStatus lookupStatus (int s){ + if ((s & DeviceStatus.CALIBRATING.status) != 0){ + return DeviceStatus.CALIBRATING; + } + boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; + boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; + + if(!xPodDetected && !yPodDetected){ + return DeviceStatus.FAULT_NO_PODS_DETECTED; + } + if (!xPodDetected){ + return DeviceStatus.FAULT_X_POD_NOT_DETECTED; + } + if (!yPodDetected){ + return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; + } + if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ + return DeviceStatus.FAULT_IMU_RUNAWAY; + } + if ((s & DeviceStatus.READY.status) != 0){ + return DeviceStatus.READY; + } + if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){ + return DeviceStatus.FAULT_BAD_READ; + } + else { + return DeviceStatus.NOT_READY; + } + } + + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the maximum change between this reading and the previous one + * @param bulkUpdate true if we are updating the loopTime variable. If not it should be false. + * @return newValue if the position is good, oldValue otherwise + */ + private Float isPositionCorrupt(float oldValue, float newValue, int threshold, boolean bulkUpdate){ + boolean noData = bulkUpdate && (loopTime < 1); + + boolean isCorrupt = noData || Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the velocity allowed to be reported + * @return newValue if the velocity is good, oldValue otherwise + */ + private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ + boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; + boolean noData = (loopTime <= 1); + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + + /** + * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. + */ + public void update(){ + final int positionThreshold = 5000; //more than one FTC field in mm + final int headingThreshold = 120; //About 20 full rotations in Radians + final int velocityThreshold = 10000; //10k mm/sec is faster than an FTC robot should be going... + final int headingVelocityThreshold = 120; //About 20 rotations per second + + float oldPosX = xPosition; + float oldPosY = yPosition; + float oldPosH = hOrientation; + float oldVelX = xVelocity; + float oldVelY = yVelocity; + float oldVelH = hVelocity; + + byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); + deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); + loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); + xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); + yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); + xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); + yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); + hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); + xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); + yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); + hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); + + /* + * Check to see if any of the floats we have received from the device are NaN or are too large + * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. + */ + xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold, true); + yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold, true); + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, true); + xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); + yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); + hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); + + } + + /** + * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function + * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING + * is supported. + * @param data GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING + */ + public void update(ReadData data) { + if (data == ReadData.ONLY_UPDATE_HEADING) { + final int headingThreshold = 120; + + float oldPosH = hOrientation; + + hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); + + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, false); + + if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ + deviceStatus = DeviceStatus.READY.status; + } + } + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public void setOffsets(double xOffset, double yOffset){ + writeFloat(Register.X_POD_OFFSET, (float) xOffset); + writeFloat(Register.Y_POD_OFFSET, (float) yOffset); + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @param distanceUnit the unit of distance used for offsets. + */ + public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit){ + writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); + writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset)); + } + + /** + * Recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} + + /** + * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} + + /** + * Can reverse the direction of each encoder. + * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward + * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left + */ + public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ + if (xEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<5); + } + if (xEncoder == EncoderDirection.REVERSED) { + writeInt(Register.DEVICE_CONTROL,1<<4); + } + + if (yEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<3); + } + if (yEncoder == EncoderDirection.REVERSED){ + writeInt(Register.DEVICE_CONTROL,1<<2); + } + } + + /** + * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

+ * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD + */ + public void setEncoderResolution(GoBildaOdometryPods pods){ + if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { + writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); + } + if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); + } + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public void setEncoderResolution(double ticks_per_mm){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_unit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @param distanceUnit unit used for distance + */ + public void setEncoderResolution(double ticks_per_unit, DistanceUnit distanceUnit){ + double resolution = distanceUnit.toMm(ticks_per_unit); + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) resolution,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Tuning this value should be unnecessary.
+ * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

+ * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

+ * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. + * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

+ * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com + * @param yawOffset A scalar for the robot's heading. + */ + public void setYawScalar(double yawOffset){ + writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. You can use this to + * update the estimated position of your robot with new external sensor data, or to run a robot + * in field coordinates.

+ * This overrides the current position.

+ * Using this feature to track your robot's position in field coordinates:
+ * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
+ * Say you're on the red alliance, your robot is against the wall and closer to the audience side, + * and the front of your robot is pointing towards the center of the field. + * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always + * keep track of how far away from the center of the field you are.

+ * Using this feature to update your position with additional sensors:
+ * Some robots have a secondary way to locate their robot on the field. This is commonly + * Apriltag localization in FTC, but it can also be something like a distance sensor. + * Often these external sensors are absolute (meaning they measure something about the field) + * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific + * position on the field to use them. In that case, spend most of your time relying on the Pinpoint + * to determine your location. Then when you pull a new position from your secondary sensor, + * send a setPosition command with the new position. The Pinpoint will then track your movement + * relative to that new, more accurate position. + * @param pos a Pose2D describing the robot's new position. + */ + public Pose2D setPosition(Pose2D pos){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); + return pos; + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posX the new X position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posX + */ + public void setPosX(double posX, DistanceUnit distanceUnit){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) distanceUnit.toMm(posX), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posY the new Y position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posY + */ + public void setPosY(double posY, DistanceUnit distanceUnit){ + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) distanceUnit.toMm(posY), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a heading that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param heading the new heading you'd like the Pinpoint to track your robot relive to. + * @param angleUnit Radians or Degrees + */ + public void setHeading(double heading, AngleUnit angleUnit){ + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) angleUnit.toRadians(heading), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Checks the deviceID of the Odometry Computer. Should return 1. + * @return 1 if device is functional. + */ + public int getDeviceID(){return readInt(Register.DEVICE_ID);} + + /** + * @return the firmware version of the Odometry Computer + */ + public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } + + /** + * @return a scalar that the IMU measured heading is multiplied by. This is tuned for each unit + * and should not need adjusted. + */ + public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } + + /** + * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: + * @return one of the following states:
+ * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
+ * READY - The device is currently functioning as normal. GREEN LED
+ * CALIBRATING - The device is currently recalibrating the gyro. RED LED
+ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
+ * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
+ * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ * FAULT_BAD_READ - The Java code has detected a bad I²C read, the result reported is a + * duplicate of the last good read. + */ + public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } + + /** + * Checks the Odometry Computer's most recent loop time.

+ * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return loop time in microseconds (1/1,000,000 seconds) + */ + public int getLoopTime(){return loopTime; } + + /** + * Checks the Odometry Computer's most recent loop frequency.

+ * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return Pinpoint Frequency in Hz (loops per second), + */ + public double getFrequency(){ + if (loopTime != 0){ + return 1000000.0/loopTime; + } + else { + return 0; + } + } + + /** + * @return the raw value of the X (forward) encoder in ticks + */ + public int getEncoderX(){return xEncoderValue; } + + /** + * @return the raw value of the Y (strafe) encoder in ticks + */ + public int getEncoderY(){return yEncoderValue; } + + /** + * @return the estimated X (forward) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getPosX(){ + return xPosition; + } + + /** + * @return the estimated X (forward) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xPosition); + } + + /** + * @return the estimated Y (Strafe) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getPosY(){ + return yPosition; + } + + /** + * @return the estimated Y (Strafe) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yPosition); + } + + /** + * @return the unnormalized estimated H (heading) position of the robot in radians + * unnormalized heading is not constrained from -180° to 180°. It will continue counting multiple rotations. + * @deprecated two overflows for this function exist with AngleUnit parameter. These minimize the possibility of unit confusion. + */ + public double getHeading(){ + return hOrientation; + } + + /** + * @return the normalized estimated H (heading) position of the robot in specified unit + * normalized heading is wrapped from -180°, to 180°. + */ + public double getHeading(AngleUnit angleUnit){ + return angleUnit.fromRadians((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI; + } + + /** + * @return the unnormalized estimated H (heading) position of the robot in specified unit + * unnormalized heading is not constrained from -180° to 180°. It will continue counting + * multiple rotations. + */ + public double getHeading(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hOrientation); + } + + /** + * @return the estimated X (forward) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getVelX(){ + return xVelocity; + } + + /** + * @return the estimated X (forward) velocity of the robot in specified unit/sec + */ + public double getVelX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xVelocity); + } + + /** + * @return the estimated Y (strafe) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getVelY(){ + return yVelocity; + } + + /** + * @return the estimated Y (strafe) velocity of the robot in specified unit/sec + */ + public double getVelY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yVelocity); + } + + /** + * @return the estimated H (heading) velocity of the robot in radians/sec + * @deprecated The overflow for this function has an AngleUnit, which can reduce the chance of unit confusion. + */ + public double getHeadingVelocity() { + return hVelocity; + } + + /** + * @return the estimated H (heading) velocity of the robot in specified unit/sec + */ + public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hVelocity); + } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the X (forward) pod in specified unit + */ + public float getXOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.X_POD_OFFSET)); + } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the Y (strafe) pod + */ + public float getYOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.Y_POD_OFFSET)); + } + + /** + * @return a Pose2D containing the estimated position of the robot + */ + public Pose2D getPosition(){ + return new Pose2D(DistanceUnit.MM, + xPosition, + yPosition, + AngleUnit.RADIANS, + //this wraps the hOrientation variable from -180° to +180° + ((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); + } + + /** + * @deprecated This function is not recommended, as velocity is wrapped from -180° to 180°. + * instead use individual getters. + * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second + */ + public Pose2D getVelocity(){ + return new Pose2D(DistanceUnit.MM, + xVelocity, + yVelocity, + AngleUnit.RADIANS, + ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java index 2c9163c99b17..6474e10ccc4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java @@ -49,9 +49,9 @@ public void runOpMode() throws InterruptedException { } slideController.setPID(kp, ki, kd); - currentPosition = robot.getSlides().getSlidePosition(); + //currentPosition = robot.getSlides().getSlidePosition(); - robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); + //robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); t.addData("Current Position", currentPosition); t.addData("Goal Position", goal); From d9518107efdd8209bf116120f791149bfb5385c6 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 13 Sep 2025 21:12:59 -0700 Subject: [PATCH 02/31] implemented pinpoint to auto robot --- .../teamcode/Autonomous/AutonomousRobot.java | 21 ++--- .../AutonomousScripts/TestAuto.java | 82 +++++++++++++++++-- .../ftc/teamcode/Modules/Drivetrain.java | 10 ++- .../Modules/Utils/GoBildaPinpoint.java | 20 ++--- .../Modules/Utils/GoBildaPinpointDriver.java | 5 ++ .../org/firstinspires/ftc/teamcode/Robot.java | 11 +++ .../teamcode/Tuning/DriveTrainPIDTuner.java | 2 +- .../ftc/teamcode/Tuning/LateralTuning.java | 2 +- .../ftc/teamcode/Tuning/Odemtry.java | 2 +- .../ftc/teamcode/Tuning/PidToPointTuner.java | 2 +- 10 files changed, 120 insertions(+), 37 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java index eda95f69aa4c..e8e1ee6550b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java @@ -15,6 +15,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.Autonomous.Utils.Path; import org.firstinspires.ftc.teamcode.Autonomous.Utils.TrapezoidalMotionProfile; @@ -29,7 +30,7 @@ public class AutonomousRobot extends Robot { Drivetrain drivetrain; - Robot robot; + public AutonomousRobot(HardwareMap hardwareMap) { super(hardwareMap); } @@ -134,9 +135,9 @@ public void step() { //Find the current field positions - double currentX = this.robot.getDrivetrain().getCurrPos().getX(DistanceUnit.INCH); - double currentY = this.robot.getDrivetrain().getCurrPos().getY(DistanceUnit.INCH); - double currentH = this.robot.getDrivetrain().getCurrPos().getH(); + double currentX = this.getOdoComputer().getCurrPos().getX(DistanceUnit.INCH); + double currentY = this.getOdoComputer().getCurrPos().getY(DistanceUnit.INCH); + double currentH = this.getOdoComputer().getCurrPos().getH(); //Find the expected position along the path, as a magnitude of a vector with angle lineSlope // then use some trig to find x and y components @@ -185,9 +186,9 @@ public void step() { //set current position point as start point start = new EditablePose2D( - this.robot.getDrivetrain().getCurrPos().getX(DistanceUnit.INCH), - this.robot.getDrivetrain().getCurrPos().getY(DistanceUnit.INCH), - this.robot.getDrivetrain().getCurrPos().getH(), + this.getOdoComputer().getPosX(DistanceUnit.INCH), + this.getOdoComputer().getPosY(DistanceUnit.INCH), + this.getOdoComputer().getHeading(AngleUnit.DEGREES), DistanceUnit.INCH ); @@ -225,8 +226,8 @@ public void step() { goalPoint = path.get(pathIndex); elapsedTime = (System.nanoTime() / (Math.pow(10, 9)) - time); - cP = super.getDrivetrain().getCurrPos(); - boolean y = atPoint(super.getDrivetrain().getCurrPos(), goalPoint.getWaypoint()); + cP = super.getOdoComputer().getCurrPos(); + boolean y = atPoint(super.getOdoComputer().getCurrPos(), goalPoint.getWaypoint()); atPoint = y; delayUntilNextPointClear = elapsedTime > goalPoint.getDelayUntilNextPoint(); @@ -314,7 +315,7 @@ public void startPath() { Path.PathPoint firstPoint = path.get(0); Waypoint firstPose = firstPoint.getWaypoint(); - if (atPoint(firstPose, robot.getDrivetrain().getCurrPos())) { + if (atPoint(firstPose, this.getOdoComputer().getCurrPos())) { pathIndex = 1; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java index e4a9687594a6..2e157c8f61ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java @@ -7,43 +7,107 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; import org.firstinspires.ftc.teamcode.Autonomous.AutonomousRobot; import org.firstinspires.ftc.teamcode.Autonomous.Utils.Path; import org.firstinspires.ftc.teamcode.Autonomous.Utils.Waypoint; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; +import org.firstinspires.ftc.teamcode.Modules.Utils.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.Robot; + +import java.util.Locale; @Config @Autonomous(name = "TEST AUTO") public class TestAuto extends LinearOpMode { private ElapsedTime timer = new ElapsedTime(); - HardwareMap hardwareMap; AutonomousRobot autoRobot; Drivetrain drivetrain; + + double oldTime = 0; + + Path p = new Path.PathBuilder() - .addNewFullPoint( - new Waypoint(1, 1, 90, 75, 1, DistanceUnit.INCH), - () -> {}, - 1) - .build(); + .addNewFullPoint( + new Waypoint(1, 72, 90, 50, 25, DistanceUnit.INCH), + () -> {}, + 1) + + .addNewFullPoint( + new Waypoint(24, 72, 90, 50, 25, DistanceUnit.INCH), + () -> {}, + 1) + + .build(); + + + @Override public void runOpMode() throws InterruptedException { autoRobot = new AutonomousRobot(hardwareMap); - drivetrain.startOdometry(); - //autoRobot.setPath(p); + autoRobot.setPath(p); autoRobot.startPath(); + InitializePinPoint(); - waitForStart(); + waitForStart(); timer.reset(); + resetRuntime(); while(opModeIsActive()){ + updatePinPoint(); autoRobot.step(); } } + public void updatePinPoint(){ + + autoRobot.getOdoComputer().update(); + + // Calculates time per each cycle and finds number of updates per seconds (frequency) + double newTime = getRuntime(); + oldTime = newTime; + + Pose2D pos = autoRobot.getOdoComputer().getPosition(); + + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", autoRobot.getOdoComputer().getVelX(DistanceUnit.MM), autoRobot.getOdoComputer().getVelY(DistanceUnit.MM), autoRobot.getOdoComputer().getHeadingVelocity(UnnormalizedAngleUnit.DEGREES)); + telemetry.addData("Velocity", velocity); + + telemetry.addData("Status", autoRobot.getOdoComputer().getDeviceStatus()); + + telemetry.addData("Pinpoint Frequency", autoRobot.getOdoComputer().getFrequency()); //prints/gets the current refresh rate of the Pinpoint + + telemetry.addData("Current path point", autoRobot.getPathIndex()); + telemetry.addData("At Point", autoRobot.isAtPoint()); + telemetry.addData("Elapsed Time", autoRobot.getElapsedTime()); + telemetry.addData("Delay", autoRobot.getDelay()); + telemetry.addData("Auton Real Position", autoRobot.autonrealPos()); + telemetry.addData("Auton Pos", autoRobot.autonPos()); + telemetry.addData("Robot not at end of path ", autoRobot.isNotAtEndOfPath()); + //telemetry.addData("Past delay to next point", autoRobot.pastDelayUntilNextPoint()); + telemetry.addData("Path size", autoRobot.pathsize()); + telemetry.update(); + } + + public void InitializePinPoint(){ + autoRobot = new AutonomousRobot(hardwareMap); + autoRobot.getOdoComputer().setOffsets(127, 299.72, DistanceUnit.MM); + + autoRobot.getOdoComputer().setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + + autoRobot.getOdoComputer().setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + autoRobot.getOdoComputer().resetPosAndIMU(); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index 76626d0f2a75..78bdf8f58c72 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.Utils.EditablePose2D; +import org.firstinspires.ftc.teamcode.Robot; // ----- READY TO TRANSFER ----- // @@ -28,6 +29,7 @@ public class Drivetrain { private ElapsedTime timer; private final OdometryLocalizer robotPos; + Robot robot; /////////////////////////////////////////////// //// ///// @@ -100,9 +102,11 @@ public EditablePose2D getCurrPos() { return robotPos.getCurrPos(); } - public void startOdometry() { - Thread localizer = new Thread(robotPos); - localizer.start(); + public void startOdometry(boolean isPinPoint) { + if (!isPinPoint) { + Thread localizer = new Thread(robotPos); + localizer.start(); + } } public OdometryLocalizer getRobotPos() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java index 5f1ff78de0a6..230ecce82755 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpoint.java @@ -9,6 +9,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; +import java.util.Locale; + @Config @TeleOp(name = "P2W computer") public class GoBildaPinpoint extends LinearOpMode { @@ -36,8 +38,8 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "Initialized"); - telemetry.addData("X offset", odoComputer.getXOffset(DistanceUnit.MM)); - telemetry.addData("Y offset", odoComputer.getYOffset(DistanceUnit.MM)); + telemetry.addData("X offset", odoComputer.getXOffset(DistanceUnit.INCH)); + telemetry.addData("Y offset", odoComputer.getYOffset(DistanceUnit.INCH)); telemetry.addData("Device Version Number:", odoComputer.getDeviceVersion()); telemetry.addData("Heading Scalar", odoComputer.getYawScalar()); telemetry.update(); @@ -49,7 +51,7 @@ public void runOpMode() throws InterruptedException { odoComputer.update(); if (gamepad1.y){ - odoComputer.recalibrateIMU(); + odoComputer.resetPosAndIMU(); } // Calculates time per each cycle and finds number of updates per seconds (frequency) @@ -60,15 +62,11 @@ public void runOpMode() throws InterruptedException { Pose2D pos = odoComputer.getPosition(); - telemetry.addLine("\n Positions and Heading \n") - .addData("X Position", pos.getX(DistanceUnit.MM)) - .addData("Y Position", pos.getY(DistanceUnit.MM)) - .addData("Orientation (Degrees)", Math.toDegrees(pos.getHeading(AngleUnit.DEGREES))); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.INCH), pos.getY(DistanceUnit.INCH), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); - telemetry.addLine("\n Velocities \n") - .addData("X Velocity", odoComputer.getVelX(DistanceUnit.MM)) - .addData("Y Velocity", odoComputer.getVelY(DistanceUnit.MM)) - .addData("H Velocity", odoComputer.getHeadingVelocity(UnnormalizedAngleUnit.DEGREES)); + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", odoComputer.getVelX(DistanceUnit.INCH), odoComputer.getVelY(DistanceUnit.INCH), odoComputer.getHeadingVelocity(UnnormalizedAngleUnit.DEGREES)); + telemetry.addData("Velocity", velocity); telemetry.addData("Status", odoComputer.getDeviceStatus()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java index a1a75c902541..0a7db111c70f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Utils/GoBildaPinpointDriver.java @@ -61,6 +61,7 @@ public class GoBildaPinpointDriver extends I2cDeviceSynchDevice Date: Tue, 16 Sep 2025 11:35:27 -0700 Subject: [PATCH 03/31] Made switch for pinpoint in auto robot --- .../teamcode/Autonomous/AutonomousRobot.java | 77 +++++++++++++++---- .../AutonomousScripts/TestAuto.java | 13 +--- .../ftc/teamcode/Modules/Drivetrain.java | 16 +++- 3 files changed, 79 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java index e8e1ee6550b0..a2d3ff11be45 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java @@ -30,6 +30,12 @@ public class AutonomousRobot extends Robot { Drivetrain drivetrain; + Robot robot; + + double currentX = 0; + double currentY = 0; + double currentH = 0; + boolean isPinPoint = true; public AutonomousRobot(HardwareMap hardwareMap) { super(hardwareMap); @@ -133,11 +139,18 @@ public void step() { break; case GOTOPOINT: - //Find the current field positions - double currentX = this.getOdoComputer().getCurrPos().getX(DistanceUnit.INCH); - double currentY = this.getOdoComputer().getCurrPos().getY(DistanceUnit.INCH); - double currentH = this.getOdoComputer().getCurrPos().getH(); + if (isPinPoint){ + currentX = this.getOdoComputer().getCurrPos().getX(DistanceUnit.INCH); + currentY = this.getOdoComputer().getCurrPos().getY(DistanceUnit.INCH); + currentH = this.getOdoComputer().getCurrPos().getH(); + } else if (!isPinPoint){ + currentX = this.robot.getDrivetrain().getCurrPos().getX(DistanceUnit.INCH); + currentY = this.robot.getDrivetrain().getCurrPos().getY(DistanceUnit.INCH); + currentH = this.robot.getDrivetrain().getCurrPos().getH(); + } + + //Find the expected position along the path, as a magnitude of a vector with angle lineSlope // then use some trig to find x and y components @@ -154,6 +167,8 @@ public void step() { turnPid.setPID(TURN_KP, TURN_KI, TURN_KD); //shift the errors to robot centric errors + + double xError = xComponent - currentX; double yError = yComponent - currentY; @@ -185,12 +200,23 @@ public void step() { } //set current position point as start point - start = new EditablePose2D( - this.getOdoComputer().getPosX(DistanceUnit.INCH), - this.getOdoComputer().getPosY(DistanceUnit.INCH), - this.getOdoComputer().getHeading(AngleUnit.DEGREES), - DistanceUnit.INCH - ); + if (isPinPoint){ + start = new EditablePose2D( + this.getOdoComputer().getPosX(DistanceUnit.INCH), + this.getOdoComputer().getPosY(DistanceUnit.INCH), + this.getOdoComputer().getHeading(AngleUnit.DEGREES), + DistanceUnit.INCH + ); + } + if (!isPinPoint){ + start = new EditablePose2D( + this.robot.getDrivetrain().getCurrPos().getX(DistanceUnit.INCH), + this.robot.getDrivetrain().getCurrPos().getY(DistanceUnit.INCH), + this.robot.getDrivetrain().getCurrPos().getH(), + DistanceUnit.INCH + ); + } + //Find distance between two points for motion profile double dx = goalPoint.getWaypoint().getX(DistanceUnit.INCH) - start.getX(DistanceUnit.INCH); @@ -226,8 +252,16 @@ public void step() { goalPoint = path.get(pathIndex); elapsedTime = (System.nanoTime() / (Math.pow(10, 9)) - time); - cP = super.getOdoComputer().getCurrPos(); - boolean y = atPoint(super.getOdoComputer().getCurrPos(), goalPoint.getWaypoint()); + boolean y = false; + if (isPinPoint){ + cP = super.getOdoComputer().getCurrPos(); + y = atPoint(super.getOdoComputer().getCurrPos(), goalPoint.getWaypoint()); + } + if (!isPinPoint){ + cP = super.getDrivetrain().getCurrPos(); + y = atPoint(super.getDrivetrain().getCurrPos(), goalPoint.getWaypoint()); + } + atPoint = y; delayUntilNextPointClear = elapsedTime > goalPoint.getDelayUntilNextPoint(); @@ -315,9 +349,18 @@ public void startPath() { Path.PathPoint firstPoint = path.get(0); Waypoint firstPose = firstPoint.getWaypoint(); - if (atPoint(firstPose, this.getOdoComputer().getCurrPos())) { - pathIndex = 1; + if (isPinPoint){ + if (atPoint(firstPose, this.getOdoComputer().getCurrPos())) { + pathIndex = 1; + } + } + + if (!isPinPoint){ + if (atPoint(firstPose, robot.getDrivetrain().getCurrPos())) { + pathIndex = 1; + } } + } public Path getPath() { @@ -338,4 +381,10 @@ public void calculateDistance(double distance) { } + + public void setPinPoint(boolean IsPinPoint){ + isPinPoint = IsPinPoint; + } + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java index 2e157c8f61ef..9313fd17c3e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java @@ -54,7 +54,7 @@ public void runOpMode() throws InterruptedException { autoRobot.setPath(p); autoRobot.startPath(); - InitializePinPoint(); + drivetrain.startOdometry(true); waitForStart(); @@ -99,15 +99,4 @@ public void updatePinPoint(){ telemetry.update(); } - public void InitializePinPoint(){ - autoRobot = new AutonomousRobot(hardwareMap); - autoRobot.getOdoComputer().setOffsets(127, 299.72, DistanceUnit.MM); - - autoRobot.getOdoComputer().setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - - autoRobot.getOdoComputer().setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); - - autoRobot.getOdoComputer().resetPosAndIMU(); - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index 78bdf8f58c72..95b99b4912a7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.Modules; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; import static java.lang.Thread.sleep; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -10,7 +11,10 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.Autonomous.AutonomousRobot; import org.firstinspires.ftc.teamcode.Modules.Utils.EditablePose2D; +import org.firstinspires.ftc.teamcode.Modules.Utils.GoBildaPinpointDriver; import org.firstinspires.ftc.teamcode.Robot; // ----- READY TO TRANSFER ----- // @@ -27,6 +31,7 @@ public class Drivetrain { private final DcMotor frWheel, flWheel, brWheel, blWheel; private final IMU imu; private ElapsedTime timer; + AutonomousRobot autoRobot; private final OdometryLocalizer robotPos; Robot robot; @@ -74,7 +79,6 @@ public Drivetrain(HardwareMap hardwareMap) { imu.resetYaw(); imu.initialize(parameters); - } // ----------- START/STOP ----------- // @@ -107,6 +111,16 @@ public void startOdometry(boolean isPinPoint) { Thread localizer = new Thread(robotPos); localizer.start(); } + if (isPinPoint){ + autoRobot = new AutonomousRobot(hardwareMap); + autoRobot.getOdoComputer().setOffsets(127, 299.72, DistanceUnit.MM); + + autoRobot.getOdoComputer().setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + + autoRobot.getOdoComputer().setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + autoRobot.getOdoComputer().resetPosAndIMU(); + } } public OdometryLocalizer getRobotPos() { From 03fdc5ab7b024769e9d0e37fa226dc3d01311072 Mon Sep 17 00:00:00 2001 From: J A Date: Fri, 26 Sep 2025 20:10:44 -0700 Subject: [PATCH 04/31] added outtake dummy class --- .../ftc/teamcode/DummyClasses/Outtake.java | 37 +++++++++++++++++++ .../ftc/teamcode/Teleop/Meet0FSM.java | 19 +++++++++- 2 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java new file mode 100644 index 000000000000..b11ac971a59a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.DummyClasses; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Robot; + +public class Outtake { + private DcMotor motorL; + private DcMotor motorR; + + public Outtake(HardwareMap hardwareMap){ + + motorL = hardwareMap.dcMotor.get("motorL"); + motorR = hardwareMap.dcMotor.get("motorR"); + + motorL.setDirection(DcMotorSimple.Direction.FORWARD); + motorR.setDirection(DcMotorSimple.Direction.REVERSE); + + motorL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motorR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void start(int speed){ + motorR.setPower(speed); + motorL.setPower(speed); + } + + public void stop(){ + motorR.setPower(0); + motorL.setPower(0); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 0f2b893aca2f..4882678a0a68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -3,23 +3,28 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Autonomous.AutonomousRobot; +import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; import org.firstinspires.ftc.teamcode.Robot; @Config @TeleOp(name = "Meet 0 FSM") public class Meet0FSM extends LinearOpMode { + //HardwareMap hardwareMap; + + Outtake outtake; Robot robot; + AutonomousRobot autoRobot; boolean hasrun = false; - - public enum states{ IDLE, INTAKE, @@ -32,6 +37,10 @@ public enum states{ public void runOpMode() throws InterruptedException { hasrun = false; + outtake = new Outtake(hardwareMap); + + autoRobot.setPinPoint(true); + telemetry.addData("Robot status:", "succesfully initiated"); telemetry.update(); @@ -93,6 +102,12 @@ private void FSM(){ hasrun = true; } + if (gamepad2.x){ + outtake.start(1); + } + + + if (gamepad2.b){ currentState = states.IDLE; hasrun = false; From 3cde471a7ca19af766a1e42d589212d77c91c986 Mon Sep 17 00:00:00 2001 From: J A Date: Fri, 26 Sep 2025 20:24:54 -0700 Subject: [PATCH 05/31] run2servos test --- .../ftc/teamcode/UnitTests/Run2Servos.java | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java new file mode 100644 index 000000000000..cb90a76d29b3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.UnitTests; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +@Config +@TeleOp(name="run2Servos", group="UnitTests") +public class Run2Servos extends LinearOpMode { + + Servo s1; + Servo s2; + public static double s1Pos = 0; + public static double s2Pos = 0; + + @Override + public void runOpMode() throws InterruptedException { + /* + * * * * * * * * * * * * * * * + * INITIALIZATION + * * * * * * * * * * * * * * * + */ + + s1 = hardwareMap.servo.get("s1"); + s2 = hardwareMap.servo.get("s2"); + s2.setDirection(Servo.Direction.REVERSE); + + telemetry.addData("Robot status", "succesfully initiated"); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + // * * * * * * * * * * * * * * * + // * Start button clicked + // * * * * * * * * * * * * * * * + + telemetry.clear(); + /* + * * * * * * * * * * * * * * * + * LOOP + * * * * * * * * * * * * * * * + */ + + while(opModeIsActive()) { + s1.setPosition(s1Pos); + s2.setPosition(s2Pos); + + + telemetry.update(); + } + } +} From 13dceff76f2272cb6914b83c2b7211699f30c491 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 27 Sep 2025 17:00:45 -0700 Subject: [PATCH 06/31] velocity pid test --- .../ftc/teamcode/DummyClasses/Outtake.java | 13 +-- .../teamcode/DummyClasses/OuttakeServo.java | 17 ++++ .../ftc/teamcode/DummyClasses/TwoServos.java | 28 +++++++ .../Modules/VelocityPidController.java | 44 +++++++++++ .../ftc/teamcode/Teleop/Meet0FSM.java | 49 +++++++++++- .../ftc/teamcode/Tuning/VelocityPidTuner.java | 79 +++++++++++++++++++ .../ftc/teamcode/UnitTests/Run2Servos.java | 16 ++-- .../ftc/teamcode/riptideUtil.java | 8 ++ 8 files changed, 240 insertions(+), 14 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/TwoServos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index b11ac971a59a..23f57a51924f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -9,8 +9,8 @@ import org.firstinspires.ftc.teamcode.Robot; public class Outtake { - private DcMotor motorL; - private DcMotor motorR; + private final DcMotor motorL; + private final DcMotor motorR; public Outtake(HardwareMap hardwareMap){ @@ -19,12 +19,9 @@ public Outtake(HardwareMap hardwareMap){ motorL.setDirection(DcMotorSimple.Direction.FORWARD); motorR.setDirection(DcMotorSimple.Direction.REVERSE); - - motorL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - motorR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } - public void start(int speed){ + public void start(double speed){ motorR.setPower(speed); motorL.setPower(speed); } @@ -34,4 +31,8 @@ public void stop(){ motorL.setPower(0); } + public double currPos(){ + double currPos = (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; + return currPos; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java new file mode 100644 index 000000000000..0f655308ac34 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.DummyClasses; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class OuttakeServo { + + Servo outtakeServo; + + public OuttakeServo(HardwareMap hardwareMap){ + outtakeServo = hardwareMap.servo.get("outtakePitch"); + } + + public void controlPitch(double angle){ + outtakeServo.setPosition(angle); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/TwoServos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/TwoServos.java new file mode 100644 index 000000000000..67490a9dc562 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/TwoServos.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.DummyClasses; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import java.io.Serializable; + +public class TwoServos { + Servo s1; + Servo s2; + public static double s1Pos = 0.43; + public static double s2Pos = 0.43; + + public TwoServos(HardwareMap hardwareMap){ + s1 = hardwareMap.servo.get("s1"); + s2 = hardwareMap.servo.get("s2"); + s2.setDirection(Servo.Direction.REVERSE); + } + + public void lift(){ + s1.setPosition(s1Pos); + s2.setPosition(s2Pos); + } + public void lower(){ + s1.setPosition(0); + s2.setPosition(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java new file mode 100644 index 000000000000..02bec3cc907a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Config +public class VelocityPidController { + private double kI = 0; + private double kP = 0; + private double kD = 0; + private double integral = 0; + private double lastError = 0; + + ElapsedTime timer = new ElapsedTime(); + double lastTime = 0; + + public void setPID(double kP, double kI, double kD){ + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.integral = 0; + this.lastError = 0; + } + + public double calculate(double targetVelocity, double currentVelocity, double dt){ + double error = targetVelocity - currentVelocity; + + integral += error * dt; + + double derivative = (error - lastError) / dt; + + double output = (kP*error) + (kI * integral) + (kD * derivative); + + lastError = error; + + return output; + } + + public void reset(){ + integral = 0; + lastError = 0; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 4882678a0a68..835126db3923 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -8,6 +8,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Autonomous.AutonomousRobot; import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; +import org.firstinspires.ftc.teamcode.DummyClasses.TwoServos; import org.firstinspires.ftc.teamcode.Robot; @@ -17,6 +18,7 @@ public class Meet0FSM extends LinearOpMode { //HardwareMap hardwareMap; Outtake outtake; + TwoServos twoServos; Robot robot; AutonomousRobot autoRobot; @@ -28,7 +30,8 @@ public class Meet0FSM extends LinearOpMode { public enum states{ IDLE, INTAKE, - OUTTAKE + OUTTAKE, + ENDGAME } public states currentState = states.IDLE; @@ -37,7 +40,11 @@ public enum states{ public void runOpMode() throws InterruptedException { hasrun = false; + + outtake = new Outtake(hardwareMap); + twoServos = new TwoServos(hardwareMap); + autoRobot.setPinPoint(true); @@ -68,16 +75,21 @@ private void FSM(){ hasrun = true; } - if (gamepad2.left_trigger > 0.1){ + if (gamepad2.y){ currentState = states.INTAKE; hasrun = false; } - if (gamepad2.left_trigger > 0.1){ + if (gamepad2.x){ currentState = states.OUTTAKE; hasrun = false; } + if (gamepad2.dpad_up){ + currentState = states.ENDGAME; + hasrun = false; + } + break; case INTAKE: if (!hasrun){ @@ -95,6 +107,11 @@ private void FSM(){ hasrun = false; } + if (gamepad2.dpad_up){ + currentState = states.ENDGAME; + hasrun = false; + } + break; case OUTTAKE: if (!hasrun){ @@ -118,8 +135,32 @@ private void FSM(){ hasrun = false; } + if (gamepad2.dpad_up){ + currentState = states.ENDGAME; + hasrun = false; + } + break; + case ENDGAME: + if (!hasrun){ + twoServos.lift(); + hasrun = true; + } + + if (gamepad2.dpad_down){ + twoServos.lower(); + } + + if (gamepad2.dpad_up){ + twoServos.lift(); + } + + if (gamepad2.b){ + currentState = states.IDLE; + twoServos.lower(); + hasrun = false; + } } } @@ -147,4 +188,6 @@ private void fieldCentricDrive() { robot.getDrivetrain().resetImu(); } } + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java new file mode 100644 index 000000000000..1f1987eedd1c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.teamcode.Tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; +import org.firstinspires.ftc.teamcode.Modules.VelocityPidController; + +@Config +@TeleOp(name = "Velocity Pid Tuner", group = "Tuning") +public class VelocityPidTuner extends LinearOpMode { + + public static double kP; + public static double kI; + public static double kD; + + public static double targetvelocity = 10; + double prevTarget; + + public static double ticksPerRev = 0; // IDK what motors we have + + Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + + private DcMotor motor; + + Outtake outtake; + VelocityPidController PIDController; + + ElapsedTime timer = new ElapsedTime(); + + @Override + public void runOpMode() throws InterruptedException{ + + if (prevTarget != targetvelocity){ + prevTarget = targetvelocity; + timer.reset(); + } + + outtake = new Outtake(hardwareMap); + + PIDController.setPID(kP, kI, kD); + + waitForStart(); + + double lastPos = outtake.currPos(); + double lastTime = timer.seconds(); + + while(opModeIsActive()){ + + double currentTime = timer.seconds(); + double currentVelocity = getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime); + + double dt = currentTime - lastTime; + double output = PIDController.calculate(targetvelocity, currentVelocity, dt); + + outtake.start(output); + + t.addData("Current Velocity ", getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime)); + t.addData("Goal Velocity ", targetvelocity); + t.update(); + } + } + + public double getMotorVelocity(double lastPos, double lastTime, double ticksPerRev, double currentTime){ + double currentPos = outtake.currPos(); + double dt = currentTime - lastTime; + + double ticksPerSec = (currentPos - lastPos) / dt; + + return ticksPerSec / ticksPerRev; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java index cb90a76d29b3..c14ad676b1f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/Run2Servos.java @@ -11,8 +11,8 @@ public class Run2Servos extends LinearOpMode { Servo s1; Servo s2; - public static double s1Pos = 0; - public static double s2Pos = 0; + public static double s1Pos = 0.43; + public static double s2Pos = 0.43; @Override public void runOpMode() throws InterruptedException { @@ -44,9 +44,15 @@ public void runOpMode() throws InterruptedException { */ while(opModeIsActive()) { - s1.setPosition(s1Pos); - s2.setPosition(s2Pos); - + if (gamepad1.x) { + s1.setPosition(s1Pos); + s2.setPosition(s2Pos); + } + + if (gamepad1.b){ + s1.setPosition(0); + s2.setPosition(0); + } telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 5708f0ba7af3..3db44a0932a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -39,4 +39,12 @@ public class riptideUtil { // Speed relationships public static double MAX_A = 72; // UNDETERMINED public static double MAX_V = 96; // UNDETERMINED + + + + + + // Robot Lift + public static double s1 = 0.43; + public static double s2 = 0.43; } From 7cfcec3d6898f92550415436061a1b60b22819ed Mon Sep 17 00:00:00 2001 From: TreeSquirrles Date: Sat, 27 Sep 2025 19:12:51 -0700 Subject: [PATCH 07/31] Started Pinpoint integration --- .../teamcode/Autonomous/AutonomousRobot.java | 1 - .../ftc/teamcode/Modules/Drivetrain.java | 28 ++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java index e8e1ee6550b0..67760aeed2da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java @@ -29,7 +29,6 @@ // maybe GOING TO BE REFACTORED, THIS IS GOING TO BE SO DEPRECATED public class AutonomousRobot extends Robot { - Drivetrain drivetrain; public AutonomousRobot(HardwareMap hardwareMap) { super(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index 78bdf8f58c72..d855ae775370 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.Utils.EditablePose2D; +import org.firstinspires.ftc.teamcode.Modules.Utils.GoBildaPinpointDriver; import org.firstinspires.ftc.teamcode.Robot; // ----- READY TO TRANSFER ----- // @@ -28,8 +29,12 @@ public class Drivetrain { private final IMU imu; private ElapsedTime timer; - private final OdometryLocalizer robotPos; - Robot robot; + private volatile OdometryLocalizer robotPos3Wheel; + private volatile GoBildaPinpointDriver robotPosPinpont; + private int time = + + private boolean usingPinpoint = true; + /////////////////////////////////////////////// //// ///// @@ -65,7 +70,19 @@ public Drivetrain(HardwareMap hardwareMap) { flWheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); blWheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robotPos = new OdometryLocalizer(blWheel, brWheel, flWheel, 10); + if (usingPinpoint){ + robotPosPinpont = hardwareMap.get(GoBildaPinpointDriver.class, "odo"); + robotPosPinpont.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + // UNCOMMENT THIS IF USING DIFFERENT PODS AND INPUT TICKS PER UNIT: odo.setEncoderResolution(13.26291192, DistanceUnit.MM); + + + robotPosPinpont.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + // Resets position to 0,0,0 and recalibrates IMU + robotPosPinpont.resetPosAndIMU(); + }else { + robotPos3Wheel = new OdometryLocalizer(blWheel, brWheel, flWheel, 10); + } imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( @@ -107,9 +124,12 @@ public void startOdometry(boolean isPinPoint) { Thread localizer = new Thread(robotPos); localizer.start(); } + } public OdometryLocalizer getRobotPos() { - return robotPos; + if(usingPinpoint) { throw new RuntimeException("Using Pinpoint, don't use this.");} + + return robotPos3Wheel; } } \ No newline at end of file From d5961d7f648c8088bbc958cf43de7a682591c625 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 27 Sep 2025 19:16:46 -0700 Subject: [PATCH 08/31] rumble --- .../ftc/teamcode/DummyClasses/Outtake.java | 9 +++++++++ .../ftc/teamcode/DummyClasses/OuttakeServo.java | 17 ----------------- .../teamcode/Modules/VelocityPidController.java | 3 --- .../ftc/teamcode/Teleop/Meet0FSM.java | 15 +++++++++------ .../ftc/teamcode/Tuning/VelocityPidTuner.java | 8 +++++--- 5 files changed, 23 insertions(+), 29 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 23f57a51924f..9206b41a6bbf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -5,12 +5,14 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Robot; public class Outtake { private final DcMotor motorL; private final DcMotor motorR; + private final Servo outtakeServo; public Outtake(HardwareMap hardwareMap){ @@ -19,6 +21,9 @@ public Outtake(HardwareMap hardwareMap){ motorL.setDirection(DcMotorSimple.Direction.FORWARD); motorR.setDirection(DcMotorSimple.Direction.REVERSE); + + outtakeServo = hardwareMap.servo.get("outtakePitch"); + } public void start(double speed){ @@ -35,4 +40,8 @@ public double currPos(){ double currPos = (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; return currPos; } + + public void controlPitch(double angle){ + outtakeServo.setPosition(angle); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java deleted file mode 100644 index 0f655308ac34..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/OuttakeServo.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.firstinspires.ftc.teamcode.DummyClasses; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; - -public class OuttakeServo { - - Servo outtakeServo; - - public OuttakeServo(HardwareMap hardwareMap){ - outtakeServo = hardwareMap.servo.get("outtakePitch"); - } - - public void controlPitch(double angle){ - outtakeServo.setPosition(angle); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java index 02bec3cc907a..80ffaea4969f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/VelocityPidController.java @@ -11,9 +11,6 @@ public class VelocityPidController { private double integral = 0; private double lastError = 0; - ElapsedTime timer = new ElapsedTime(); - double lastTime = 0; - public void setPID(double kP, double kI, double kD){ this.kP = kP; this.kI = kI; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 835126db3923..cd7fd67101db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Autonomous.AutonomousRobot; @@ -25,6 +26,7 @@ public class Meet0FSM extends LinearOpMode { boolean hasrun = false; + ElapsedTime endTimer = new ElapsedTime(); public enum states{ @@ -59,9 +61,15 @@ public void runOpMode() throws InterruptedException { telemetry.update(); while(opModeIsActive()){ + outtake.controlPitch(gamepad2.left_stick_y); + fieldCentricDrive(); + + double currTime = endTimer.seconds(); + + if (currTime >= 80){gamepad1.rumble(1, 1, 500); gamepad2.rumble(1, 1, 500);} + telemetry.addData("Current State:", currentState); telemetry.update(); - fieldCentricDrive(); } } @@ -69,7 +77,6 @@ public void runOpMode() throws InterruptedException { private void FSM(){ switch(currentState){ case IDLE: - if (!hasrun){ //Do idle things hasrun = true; @@ -123,8 +130,6 @@ private void FSM(){ outtake.start(1); } - - if (gamepad2.b){ currentState = states.IDLE; hasrun = false; @@ -188,6 +193,4 @@ private void fieldCentricDrive() { robot.getDrivetrain().resetImu(); } } - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java index 1f1987eedd1c..5413fdf95ae9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java @@ -16,9 +16,9 @@ @TeleOp(name = "Velocity Pid Tuner", group = "Tuning") public class VelocityPidTuner extends LinearOpMode { - public static double kP; - public static double kI; - public static double kD; + public static double kP = 0; + public static double kI = 0; + public static double kD = 0; public static double targetvelocity = 10; double prevTarget; @@ -62,6 +62,8 @@ public void runOpMode() throws InterruptedException{ outtake.start(output); + lastTime = currentTime; + t.addData("Current Velocity ", getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime)); t.addData("Goal Velocity ", targetvelocity); t.update(); From d5474cfa4b5e6917ca8be0399c4c3363cb6ac28a Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 27 Sep 2025 19:44:18 -0700 Subject: [PATCH 09/31] pinpoint in drivetrain --- .../teamcode/Autonomous/AutonomousRobot.java | 11 +++-------- .../Autonomous/AutonomousScripts/TestAuto.java | 1 + .../ftc/teamcode/Modules/Drivetrain.java | 17 ++++++++++++----- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java index 74b4e11a3bf4..976c37553dac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousRobot.java @@ -228,14 +228,8 @@ public void step() { elapsedTime = (System.nanoTime() / (Math.pow(10, 9)) - time); boolean y = false; - if (isPinPoint){ - cP = super.getOdoComputer().getCurrPos(); - y = atPoint(super.getOdoComputer().getCurrPos(), goalPoint.getWaypoint()); - } - if (!isPinPoint){ - cP = super.getDrivetrain().getCurrPos(); - y = atPoint(super.getDrivetrain().getCurrPos(), goalPoint.getWaypoint()); - } + cP = super.getDrivetrain().getCurrPos(); + y = atPoint(super.getDrivetrain().getCurrPos(), goalPoint.getWaypoint()); atPoint = y; @@ -347,4 +341,5 @@ public void calculateDistance(double distance) { } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java index 9313fd17c3e8..20cfc13c2aef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java @@ -55,6 +55,7 @@ public void runOpMode() throws InterruptedException { autoRobot.setPath(p); autoRobot.startPath(); drivetrain.startOdometry(true); + autoRobot.isPinpoint(true); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index fa474bb990ad..ee9b3b0da683 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -34,7 +34,6 @@ public class Drivetrain { private volatile OdometryLocalizer robotPos3Wheel; private volatile GoBildaPinpointDriver robotPosPinpont; - private int time = private boolean usingPinpoint = true; @@ -81,8 +80,7 @@ public Drivetrain(HardwareMap hardwareMap) { robotPosPinpont.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); - // Resets position to 0,0,0 and recalibrates IMU - robotPosPinpont.resetPosAndIMU(); + }else { robotPos3Wheel = new OdometryLocalizer(blWheel, brWheel, flWheel, 10); } @@ -119,15 +117,24 @@ public void setWheelPowers(double flWheelPower, double frWheelPower, double brWh // ------------ GETTERS ------------ // public EditablePose2D getCurrPos() { - return robotPos.getCurrPos(); + if (usingPinpoint){ + return robotPosPinpont.getCurrPos(); + } else { + return robotPos3Wheel.getCurrPos(); + } } public void startOdometry(boolean isPinPoint) { if (!isPinPoint) { - Thread localizer = new Thread(robotPos); + Thread localizer = new Thread(robotPos3Wheel); localizer.start(); } + if (isPinPoint){ + // Resets position to 0,0,0 and recalibrates IMU + robotPosPinpont.resetPosAndIMU(); + } + } public OdometryLocalizer getRobotPos() { From 76df7cab2d777cac71aa7d52c288da7c07339bc7 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 27 Sep 2025 19:44:54 -0700 Subject: [PATCH 10/31] adfasd --- .../ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java index 20cfc13c2aef..df7a7443332c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomous/AutonomousScripts/TestAuto.java @@ -55,8 +55,6 @@ public void runOpMode() throws InterruptedException { autoRobot.setPath(p); autoRobot.startPath(); drivetrain.startOdometry(true); - autoRobot.isPinpoint(true); - waitForStart(); timer.reset(); From 55cd52c5877715ed6939e074c75784a51deda8c0 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 27 Sep 2025 21:23:11 -0700 Subject: [PATCH 11/31] tuner??? --- .../ftc/teamcode/Teleop/Meet0FSM.java | 3 -- .../ftc/teamcode/Tuning/VelocityPidTuner.java | 29 +++++++++++++------ 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index cd7fd67101db..1561f0e2f769 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -47,9 +47,6 @@ public void runOpMode() throws InterruptedException { outtake = new Outtake(hardwareMap); twoServos = new TwoServos(hardwareMap); - - autoRobot.setPinPoint(true); - telemetry.addData("Robot status:", "succesfully initiated"); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java index 5413fdf95ae9..c3362024a6c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java @@ -20,10 +20,10 @@ public class VelocityPidTuner extends LinearOpMode { public static double kI = 0; public static double kD = 0; - public static double targetvelocity = 10; + public static double targetvelocity = 500; double prevTarget; - public static double ticksPerRev = 0; // IDK what motors we have + public static double ticksPerRev = 28; // IDK what motors we have Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); @@ -38,13 +38,10 @@ public class VelocityPidTuner extends LinearOpMode { @Override public void runOpMode() throws InterruptedException{ - if (prevTarget != targetvelocity){ - prevTarget = targetvelocity; - timer.reset(); - } - outtake = new Outtake(hardwareMap); + + PIDController = new VelocityPidController(); PIDController.setPID(kP, kI, kD); waitForStart(); @@ -54,19 +51,33 @@ public void runOpMode() throws InterruptedException{ while(opModeIsActive()){ + if (prevTarget != targetvelocity){ + prevTarget = targetvelocity; + PIDController.reset(); + timer.reset(); + } + + double currentTime = timer.seconds(); + double currentPos = outtake.currPos(); double currentVelocity = getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime); double dt = currentTime - lastTime; double output = PIDController.calculate(targetvelocity, currentVelocity, dt); + output = Math.max(-1.0, Math.min(1.0, output)); + outtake.start(output); - lastTime = currentTime; - t.addData("Current Velocity ", getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime)); + + t.addData("Current Velocity ", currentVelocity); t.addData("Goal Velocity ", targetvelocity); + t.addData("Output ", output); t.update(); + + lastPos = currentPos; + lastTime = currentTime; } } From b036a38b9b71477106cd2b3ba5e3c740db439e69 Mon Sep 17 00:00:00 2001 From: TreeSquirrles Date: Fri, 3 Oct 2025 19:08:30 -0700 Subject: [PATCH 12/31] starter code --- .../ftc/teamcode/DummyClasses/Outtake.java | 29 ++++++ .../ftc/teamcode/Tuning/VelocityPidTuner.java | 92 ------------------- .../ftc/teamcode/riptideUtil.java | 3 + 3 files changed, 32 insertions(+), 92 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 9206b41a6bbf..b3be4b25eae7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -1,12 +1,16 @@ package org.firstinspires.ftc.teamcode.DummyClasses; +import static org.firstinspires.ftc.teamcode.riptideUtil.FLYWHEEL_KP; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDCoefficients; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.Robot; public class Outtake { @@ -14,6 +18,8 @@ public class Outtake { private final DcMotor motorR; private final Servo outtakeServo; + private final PIDController flywheelVelocityController = new PIDController(FLYWHEEL_KP, 0, 0); + public Outtake(HardwareMap hardwareMap){ motorL = hardwareMap.dcMotor.get("motorL"); @@ -36,6 +42,29 @@ public void stop(){ motorL.setPower(0); } + private double startTime = System.nanoTime() / 1e9; + private int previousTickCount = 0; + + + public void setFlywheelSpeed(double goalRPM){ + + int currentTickCount = motorL.getCurrentPosition(); + + int dtheta = currentTickCount - previousTickCount; + double dt = System.nanoTime() / 1e9 - startTime; + + double currRPM = dtheta / (dt / 60); + + double wantedWheelPower = flywheelVelocityController.calculate(currRPM, goalRPM); + + motorL.setPower(wantedWheelPower); + + } + + public void startFlywheel(){ + this.startTime = System.nanoTime() / 1e9; // Current Time in Seconds + } + public double currPos(){ double currPos = (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; return currPos; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java deleted file mode 100644 index c3362024a6c3..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/VelocityPidTuner.java +++ /dev/null @@ -1,92 +0,0 @@ -package org.firstinspires.ftc.teamcode.Tuning; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; -import org.firstinspires.ftc.teamcode.Modules.VelocityPidController; - -@Config -@TeleOp(name = "Velocity Pid Tuner", group = "Tuning") -public class VelocityPidTuner extends LinearOpMode { - - public static double kP = 0; - public static double kI = 0; - public static double kD = 0; - - public static double targetvelocity = 500; - double prevTarget; - - public static double ticksPerRev = 28; // IDK what motors we have - - Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - - private DcMotor motor; - - Outtake outtake; - VelocityPidController PIDController; - - ElapsedTime timer = new ElapsedTime(); - - @Override - public void runOpMode() throws InterruptedException{ - - outtake = new Outtake(hardwareMap); - - - PIDController = new VelocityPidController(); - PIDController.setPID(kP, kI, kD); - - waitForStart(); - - double lastPos = outtake.currPos(); - double lastTime = timer.seconds(); - - while(opModeIsActive()){ - - if (prevTarget != targetvelocity){ - prevTarget = targetvelocity; - PIDController.reset(); - timer.reset(); - } - - - double currentTime = timer.seconds(); - double currentPos = outtake.currPos(); - double currentVelocity = getMotorVelocity(lastPos, lastTime, ticksPerRev, currentTime); - - double dt = currentTime - lastTime; - double output = PIDController.calculate(targetvelocity, currentVelocity, dt); - - output = Math.max(-1.0, Math.min(1.0, output)); - - outtake.start(output); - - - - t.addData("Current Velocity ", currentVelocity); - t.addData("Goal Velocity ", targetvelocity); - t.addData("Output ", output); - t.update(); - - lastPos = currentPos; - lastTime = currentTime; - } - } - - public double getMotorVelocity(double lastPos, double lastTime, double ticksPerRev, double currentTime){ - double currentPos = outtake.currPos(); - double dt = currentTime - lastTime; - - double ticksPerSec = (currentPos - lastPos) / dt; - - return ticksPerSec / ticksPerRev; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 3db44a0932a4..750d6f0e3623 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -13,6 +13,9 @@ public class riptideUtil { /** General constants */ + public static double FLYWHEEL_KP = 0; + public static int MOTOR_TICKS_PER_REV_6K = 28; + /** Autonomous Constants */ public static final double POINT_TOLERANCE = 2; // UNDETERMINED From 64d00dad1d80967c45e6fc43368380925fd453df Mon Sep 17 00:00:00 2001 From: TreeSquirrles Date: Fri, 3 Oct 2025 19:09:17 -0700 Subject: [PATCH 13/31] better starter code --- .../org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index b3be4b25eae7..f98af4c83a3d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -51,7 +51,9 @@ public void setFlywheelSpeed(double goalRPM){ int currentTickCount = motorL.getCurrentPosition(); int dtheta = currentTickCount - previousTickCount; + previousTickCount = currentTickCount; double dt = System.nanoTime() / 1e9 - startTime; + startTime = System.nanoTime() / 1e9; double currRPM = dtheta / (dt / 60); From 8997cb4f327ead8a47d68c9d478c367d780d9d0a Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 4 Oct 2025 04:40:59 -0700 Subject: [PATCH 14/31] outtake PID --- .../ftc/teamcode/DummyClasses/Outtake.java | 28 +++++-- .../ftc/teamcode/Teleop/Meet0FSM.java | 79 +++++++++++-------- 2 files changed, 64 insertions(+), 43 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index f98af4c83a3d..06e691d35cf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -18,7 +18,8 @@ public class Outtake { private final DcMotor motorR; private final Servo outtakeServo; - private final PIDController flywheelVelocityController = new PIDController(FLYWHEEL_KP, 0, 0); + private final PIDController flywheelVelocityControllerR = new PIDController(FLYWHEEL_KP, 0, 0); + private final PIDController flywheelVelocityControllerL = new PIDController(FLYWHEEL_KP, 0, 0); public Outtake(HardwareMap hardwareMap){ @@ -43,23 +44,34 @@ public void stop(){ } private double startTime = System.nanoTime() / 1e9; - private int previousTickCount = 0; + private int previousTickCountL = 0; + private int previousTickCountR = 0; public void setFlywheelSpeed(double goalRPM){ - int currentTickCount = motorL.getCurrentPosition(); + int currentTickCountL = motorL.getCurrentPosition(); + int currentTickCountR = motorR.getCurrentPosition(); + + int dthetaL = currentTickCountL - previousTickCountL; + previousTickCountL = currentTickCountL; + + int dthetaR = currentTickCountR - previousTickCountR; + previousTickCountR = currentTickCountR; - int dtheta = currentTickCount - previousTickCount; - previousTickCount = currentTickCount; double dt = System.nanoTime() / 1e9 - startTime; startTime = System.nanoTime() / 1e9; - double currRPM = dtheta / (dt / 60); - double wantedWheelPower = flywheelVelocityController.calculate(currRPM, goalRPM); - motorL.setPower(wantedWheelPower); + double currRPML = dthetaL / (dt / 60); + double currRPMR = dthetaR / (dt / 60); + + double wantedWheelPowerR = flywheelVelocityControllerR.calculate(currRPMR, goalRPM); + double wantedWheelPowerL = flywheelVelocityControllerL.calculate(currRPML, goalRPM); + + motorL.setPower(wantedWheelPowerL); + motorR.setPower(wantedWheelPowerR); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 1561f0e2f769..69a15f967bdc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -28,6 +28,7 @@ public class Meet0FSM extends LinearOpMode { ElapsedTime endTimer = new ElapsedTime(); + boolean runFlywheel = false; public enum states{ IDLE, @@ -58,6 +59,18 @@ public void runOpMode() throws InterruptedException { telemetry.update(); while(opModeIsActive()){ + + FSM(); + if (gamepad2.x){runFlywheel = true;} + else{runFlywheel = false;} + + if (runFlywheel){ + outtake.startFlywheel(); + outtake.setFlywheelSpeed(0 /* idk but we want this value to auto change based on distance from goal */); + } else { + outtake.stop(); + } + outtake.controlPitch(gamepad2.left_stick_y); fieldCentricDrive(); @@ -84,10 +97,10 @@ private void FSM(){ hasrun = false; } - if (gamepad2.x){ - currentState = states.OUTTAKE; - hasrun = false; - } +// if (gamepad2.x){ +// currentState = states.OUTTAKE; +// hasrun = false; +// } if (gamepad2.dpad_up){ currentState = states.ENDGAME; @@ -101,10 +114,10 @@ private void FSM(){ hasrun = true; } - if (gamepad2.x){ - currentState = states.OUTTAKE; - hasrun = false; - } +// if (gamepad2.x){ +// currentState = states.OUTTAKE; +// hasrun = false; +// } if (gamepad2.b){ currentState = states.IDLE; @@ -117,33 +130,29 @@ private void FSM(){ } break; - case OUTTAKE: - if (!hasrun){ - //Do outtake setup - hasrun = true; - } - - if (gamepad2.x){ - outtake.start(1); - } - - if (gamepad2.b){ - currentState = states.IDLE; - hasrun = false; - } - - if (gamepad2.left_trigger > 0.1){ - currentState = states.INTAKE; - hasrun = false; - } - - if (gamepad2.dpad_up){ - currentState = states.ENDGAME; - hasrun = false; - } - - - break; +// case OUTTAKE: +// if (!hasrun){ +// //Do outtake setup +// hasrun = true; +// } +// +// if (gamepad2.b){ +// currentState = states.IDLE; +// hasrun = false; +// } +// +// if (gamepad2.left_trigger > 0.1){ +// currentState = states.INTAKE; +// hasrun = false; +// } +// +// if (gamepad2.dpad_up){ +// currentState = states.ENDGAME; +// hasrun = false; +// } +// +// +// break; case ENDGAME: if (!hasrun){ twoServos.lift(); From 47da1e22928f69ef6f6ebb674ba3aaa2fc1b74ea Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Sat, 4 Oct 2025 16:48:44 -0700 Subject: [PATCH 15/31] Your descriptive commit message here --- .../firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java new file mode 100644 index 000000000000..b70e722b6801 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.Tuning; + +public class OuttakePIDTuner { +} From b3a7fa8da8ba488baa1d14214114a386ff2d680d Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Sat, 4 Oct 2025 17:57:25 -0700 Subject: [PATCH 16/31] Created OuttakePIDTuner properly and adjusted Outtake to match --- .../ftc/teamcode/DummyClasses/Outtake.java | 21 ++- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 135 +++++++++++++++++- 2 files changed, 152 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 06e691d35cf4..373eb5b6a6d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -38,6 +38,13 @@ public void start(double speed){ motorL.setPower(speed); } + //This one method is only to set motors individually + public void setFlyWheelPower(double speedL, double speedR) { + motorR.setPower(speedR); + motorL.setPower(speedL); + + } + public void stop(){ motorR.setPower(0); motorL.setPower(0); @@ -53,10 +60,10 @@ public void setFlywheelSpeed(double goalRPM){ int currentTickCountL = motorL.getCurrentPosition(); int currentTickCountR = motorR.getCurrentPosition(); - int dthetaL = currentTickCountL - previousTickCountL; + double dthetaL = (currentTickCountL - previousTickCountL)/28D; previousTickCountL = currentTickCountL; - int dthetaR = currentTickCountR - previousTickCountR; + double dthetaR = (currentTickCountR - previousTickCountR)/28D; previousTickCountR = currentTickCountR; double dt = System.nanoTime() / 1e9 - startTime; @@ -84,6 +91,16 @@ public double currPos(){ return currPos; } + //Created to allow OuttakePIDTuner to access individual positions + public double currPosL(){ + return motorL.getCurrentPosition(); + } + public double currPosR(){ + return motorR.getCurrentPosition(); + } + //Stops here + + public void controlPitch(double angle){ outtakeServo.setPosition(angle); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index b70e722b6801..004693d612be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -1,4 +1,135 @@ package org.firstinspires.ftc.teamcode.Tuning; -public class OuttakePIDTuner { -} +import static org.firstinspires.ftc.teamcode.riptideUtil.FLYWHEEL_KP; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.Modules.PIDController; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; + +@Config +@TeleOp(name = "OuttakeTuner",group = "Tuning") +public class OuttakePIDTuner extends LinearOpMode { + public static double KP = 0; + Outtake outtake; + public static double rpmL = 360; + public static double rpmR = 360; + + public static boolean recordspeed = false; + public static boolean repeatrecord = false; + + + Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + ElapsedTime t = new ElapsedTime(); + + @Override + public void runOpMode() throws InterruptedException { + outtake = new Outtake(hardwareMap); + telemetry.addData("Robot status", "successfully initiated"); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + telemetry.clear(); + + preparefirstframe(); + + while (opModeIsActive()) { +// outtake.startFlywheel(); +// outtake.setFlywheelSpeed(rpm); +// //saveframe(); + + + + if (recordspeed) + // telemetry.addData("Outtake speed", motorvelocity(frame1dist,frame2dist, frame1time, frame2time)); + + tele.addData("Speed of motor",speed()); + } + } + + double frame1dist =-1; + double frame2dist =-1; + double frame1time =-1; + double frame2time =-1; +// public void saveframe() { +// frame2time = frame1time; +// frame2dist = frame1dist; +// frame1time = t.nanoseconds(); +// frame1dist = outtake.currPos(); +// } +// +// +// +// public double getouttakerpm() { +// +// +// +// +// return -1; +// } + public double motorvelocity(double dist1, double dist2, double frame1, double frame2) { + if (!repeatrecord) + recordspeed = false; + return (dist2-dist1)/(frame2-frame1); + } +// +// + public void preparefirstframe() { + frame1dist = outtake.currPos(); + frame1time = t.nanoseconds(); + }; + + public double speed() { + frame2time = frame1time; + frame2dist = frame1dist; + + preparefirstframe(); + + return motorvelocity(frame1dist,frame2dist,frame1time,frame2time); + + + } + + private double prevPosL, prevPosR, currPosL, currPosR; + + private double startTime = System.nanoTime() / 1e9; + + private final PIDController RPMControllerL = new PIDController(KP, 0, 0); + + private final PIDController RPMControllerR = new PIDController(KP, 0, 0); + + + public void pidtunedmotor() { + startTime = System.nanoTime() / 1e9; + + prevPosL = currPosL; + prevPosR = currPosR; + + currPosL = outtake.currPosL(); + currPosR = outtake.currPosR(); + + double dThetaL = (currPosL - prevPosL)/28; + double dThetaR = (currPosR - prevPosR)/28; + + double dt = System.nanoTime() / 1e9 - startTime; + startTime = System.nanoTime() / 1e9; + + double currRPML = dThetaL / (dt / 60); + double currRPMR = dThetaR / (dt / 60); + + double wantedWheelPowerL = RPMControllerL.calculate(currRPMR, rpmL); + double wantedWheelPowerR = RPMControllerR.calculate(currRPML, rpmR); + + outtake.setFlyWheelPower(wantedWheelPowerL,wantedWheelPowerR); + } + +} \ No newline at end of file From 1394ae5e77803c4512e96b43f53e0fb823c4b419 Mon Sep 17 00:00:00 2001 From: J A Date: Tue, 7 Oct 2025 03:49:48 -0700 Subject: [PATCH 17/31] test intake rename --- .../ftc/teamcode/DummyClasses/Intake.java | 23 ++++++ .../ftc/teamcode/DummyClasses/Outtake.java | 12 +-- .../ftc/teamcode/Teleop/Meet0FSM.java | 77 +++++++++---------- .../{RunAMotor.java => TestIntake.java} | 2 +- 4 files changed, 60 insertions(+), 54 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Intake.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/{RunAMotor.java => TestIntake.java} (96%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Intake.java new file mode 100644 index 000000000000..9d3b0714adf9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Intake.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.DummyClasses; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Intake { + private final DcMotor intake; + + public Intake (HardwareMap hardwareMap){ + intake = hardwareMap.dcMotor.get("intake"); + intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void start(double speed){ + intake.setPower(speed); + } + + public void stop(){ + intake.setPower(0); + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 373eb5b6a6d1..6ad5cae578e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -16,7 +16,6 @@ public class Outtake { private final DcMotor motorL; private final DcMotor motorR; - private final Servo outtakeServo; private final PIDController flywheelVelocityControllerR = new PIDController(FLYWHEEL_KP, 0, 0); private final PIDController flywheelVelocityControllerL = new PIDController(FLYWHEEL_KP, 0, 0); @@ -29,8 +28,6 @@ public Outtake(HardwareMap hardwareMap){ motorL.setDirection(DcMotorSimple.Direction.FORWARD); motorR.setDirection(DcMotorSimple.Direction.REVERSE); - outtakeServo = hardwareMap.servo.get("outtakePitch"); - } public void start(double speed){ @@ -87,8 +84,7 @@ public void startFlywheel(){ } public double currPos(){ - double currPos = (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; - return currPos; + return (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; } //Created to allow OuttakePIDTuner to access individual positions @@ -98,10 +94,4 @@ public double currPosL(){ public double currPosR(){ return motorR.getCurrentPosition(); } - //Stops here - - - public void controlPitch(double angle){ - outtakeServo.setPosition(angle); - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 69a15f967bdc..8592237133fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -61,17 +61,6 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ FSM(); - if (gamepad2.x){runFlywheel = true;} - else{runFlywheel = false;} - - if (runFlywheel){ - outtake.startFlywheel(); - outtake.setFlywheelSpeed(0 /* idk but we want this value to auto change based on distance from goal */); - } else { - outtake.stop(); - } - - outtake.controlPitch(gamepad2.left_stick_y); fieldCentricDrive(); double currTime = endTimer.seconds(); @@ -97,10 +86,10 @@ private void FSM(){ hasrun = false; } -// if (gamepad2.x){ -// currentState = states.OUTTAKE; -// hasrun = false; -// } + if (gamepad2.x){ + currentState = states.OUTTAKE; + hasrun = false; + } if (gamepad2.dpad_up){ currentState = states.ENDGAME; @@ -114,10 +103,10 @@ private void FSM(){ hasrun = true; } -// if (gamepad2.x){ -// currentState = states.OUTTAKE; -// hasrun = false; -// } + if (gamepad2.x){ + currentState = states.OUTTAKE; + hasrun = false; + } if (gamepad2.b){ currentState = states.IDLE; @@ -129,30 +118,34 @@ private void FSM(){ hasrun = false; } + if (gamepad2.right_trigger > 0.5){ + + } + + break; + case OUTTAKE: + if (!hasrun){ + //Do outtake setup + hasrun = true; + } + + if (gamepad2.b){ + currentState = states.IDLE; + hasrun = false; + } + + if (gamepad2.left_trigger > 0.1){ + currentState = states.INTAKE; + hasrun = false; + } + + if (gamepad2.dpad_up){ + currentState = states.ENDGAME; + hasrun = false; + } + + break; -// case OUTTAKE: -// if (!hasrun){ -// //Do outtake setup -// hasrun = true; -// } -// -// if (gamepad2.b){ -// currentState = states.IDLE; -// hasrun = false; -// } -// -// if (gamepad2.left_trigger > 0.1){ -// currentState = states.INTAKE; -// hasrun = false; -// } -// -// if (gamepad2.dpad_up){ -// currentState = states.ENDGAME; -// hasrun = false; -// } -// -// -// break; case ENDGAME: if (!hasrun){ twoServos.lift(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/RunAMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/RunAMotor.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java index deef6bbef28e..13a8cf22324f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/RunAMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; -public class RunAMotor extends LinearOpMode { +public class TestIntake extends LinearOpMode { DcMotor motor; From 0d6fb9023903b79cd5a6cef187961e109be832b1 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:27:54 -0700 Subject: [PATCH 18/31] Hopefully finalized OuttakePIDTuner, thing's completely untested --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 110 ++++-------------- 1 file changed, 25 insertions(+), 85 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 004693d612be..39780669a731 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -1,17 +1,14 @@ package org.firstinspires.ftc.teamcode.Tuning; -import static org.firstinspires.ftc.teamcode.riptideUtil.FLYWHEEL_KP; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Modules.PIDController; -import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; @Config @@ -19,15 +16,17 @@ public class OuttakePIDTuner extends LinearOpMode { public static double KP = 0; Outtake outtake; - public static double rpmL = 360; - public static double rpmR = 360; + public static double rpmTop = 360; + public static double rpmBottom = 360; + Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - public static boolean recordspeed = false; - public static boolean repeatrecord = false; - Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - ElapsedTime t = new ElapsedTime(); + private double prevPosTop, prevPosBottom, currPosTop, currPosBottom; + private double startTime = System.nanoTime() / 1e9; + private final PIDController RPMControllerTop = new PIDController(KP, 0, 0); + private final PIDController RPMControllerBottom = new PIDController(KP, 0, 0); + @Override public void runOpMode() throws InterruptedException { @@ -40,96 +39,37 @@ public void runOpMode() throws InterruptedException { telemetry.clear(); - preparefirstframe(); - while (opModeIsActive()) { -// outtake.startFlywheel(); -// outtake.setFlywheelSpeed(rpm); -// //saveframe(); - - - - if (recordspeed) - // telemetry.addData("Outtake speed", motorvelocity(frame1dist,frame2dist, frame1time, frame2time)); - - tele.addData("Speed of motor",speed()); + pidtunedmotor(tele); + tele.update(); } } + public void pidtunedmotor(Telemetry telemetry) { - double frame1dist =-1; - double frame2dist =-1; - double frame1time =-1; - double frame2time =-1; -// public void saveframe() { -// frame2time = frame1time; -// frame2dist = frame1dist; -// frame1time = t.nanoseconds(); -// frame1dist = outtake.currPos(); -// } -// -// -// -// public double getouttakerpm() { -// -// -// -// -// return -1; -// } - public double motorvelocity(double dist1, double dist2, double frame1, double frame2) { - if (!repeatrecord) - recordspeed = false; - return (dist2-dist1)/(frame2-frame1); - } -// -// - public void preparefirstframe() { - frame1dist = outtake.currPos(); - frame1time = t.nanoseconds(); - }; - - public double speed() { - frame2time = frame1time; - frame2dist = frame1dist; - - preparefirstframe(); - - return motorvelocity(frame1dist,frame2dist,frame1time,frame2time); - - - } - - private double prevPosL, prevPosR, currPosL, currPosR; - - private double startTime = System.nanoTime() / 1e9; - - private final PIDController RPMControllerL = new PIDController(KP, 0, 0); - - private final PIDController RPMControllerR = new PIDController(KP, 0, 0); - - - public void pidtunedmotor() { startTime = System.nanoTime() / 1e9; - prevPosL = currPosL; - prevPosR = currPosR; + prevPosTop = currPosTop; + prevPosBottom = currPosBottom; - currPosL = outtake.currPosL(); - currPosR = outtake.currPosR(); + currPosTop = outtake.currPosL(); + currPosBottom = outtake.currPosR(); - double dThetaL = (currPosL - prevPosL)/28; - double dThetaR = (currPosR - prevPosR)/28; + double dThetaTop = (currPosTop - prevPosTop)/28; + double dThetaBottom = (currPosBottom - prevPosBottom)/28; double dt = System.nanoTime() / 1e9 - startTime; startTime = System.nanoTime() / 1e9; - double currRPML = dThetaL / (dt / 60); - double currRPMR = dThetaR / (dt / 60); + double currRPMTop = dThetaTop / (dt / 60); + double currRPMBottom = dThetaBottom / (dt / 60); + + telemetry.addData("Current RPM of top outtake motor", currRPMTop); + telemetry.addData("Current RPM of bottom outtake motor", currRPMBottom); - double wantedWheelPowerL = RPMControllerL.calculate(currRPMR, rpmL); - double wantedWheelPowerR = RPMControllerR.calculate(currRPML, rpmR); + double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); + double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); - outtake.setFlyWheelPower(wantedWheelPowerL,wantedWheelPowerR); + outtake.setFlyWheelPower(wantedWheelPowerTop,wantedWheelPowerBottom); } } \ No newline at end of file From 93505be9e9648cf5a13446b19760a1609b18b3dc Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:30:43 -0700 Subject: [PATCH 19/31] Individual Kp-s for each motor --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 39780669a731..22e7b0a7fdfc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -14,7 +14,9 @@ @Config @TeleOp(name = "OuttakeTuner",group = "Tuning") public class OuttakePIDTuner extends LinearOpMode { - public static double KP = 0; + public static double KPTop = 0; + public static double KPBottom = 0; + Outtake outtake; public static double rpmTop = 360; public static double rpmBottom = 360; @@ -24,8 +26,8 @@ public class OuttakePIDTuner extends LinearOpMode { private double prevPosTop, prevPosBottom, currPosTop, currPosBottom; private double startTime = System.nanoTime() / 1e9; - private final PIDController RPMControllerTop = new PIDController(KP, 0, 0); - private final PIDController RPMControllerBottom = new PIDController(KP, 0, 0); + private final PIDController RPMControllerTop = new PIDController(KPTop, 0, 0); + private final PIDController RPMControllerBottom = new PIDController(KPBottom, 0, 0); @Override From 602f9bf7c96a36f31ad616addcc65981e222f5cc Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:37:01 -0700 Subject: [PATCH 20/31] Individual Kp-s for each motor --- .../firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 22e7b0a7fdfc..818434dcd87c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -65,8 +65,8 @@ public void pidtunedmotor(Telemetry telemetry) { double currRPMTop = dThetaTop / (dt / 60); double currRPMBottom = dThetaBottom / (dt / 60); - telemetry.addData("Current RPM of top outtake motor", currRPMTop); - telemetry.addData("Current RPM of bottom outtake motor", currRPMBottom); + telemetry.addData("currPRMTop", currRPMTop); + telemetry.addData("currRPMBottom", currRPMBottom); double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); From 069e9eced9f440c6fb0c195868d85478c0dc9bd7 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:46:34 -0700 Subject: [PATCH 21/31] Fixed some names --- .../org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 373eb5b6a6d1..bfdbc87c7814 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -23,8 +23,8 @@ public class Outtake { public Outtake(HardwareMap hardwareMap){ - motorL = hardwareMap.dcMotor.get("motorL"); - motorR = hardwareMap.dcMotor.get("motorR"); + motorL = hardwareMap.dcMotor.get("topFlywheel"); + motorR = hardwareMap.dcMotor.get("bottomFlywheel"); motorL.setDirection(DcMotorSimple.Direction.FORWARD); motorR.setDirection(DcMotorSimple.Direction.REVERSE); From 2afb6b24612178b159fa9e58e94d130c34b2b8f9 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:52:52 -0700 Subject: [PATCH 22/31] Took out the trash (the outtakePitch) --- .../firstinspires/ftc/teamcode/DummyClasses/Outtake.java | 6 +----- .../org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index bfdbc87c7814..3589ebc5d7e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -16,7 +16,6 @@ public class Outtake { private final DcMotor motorL; private final DcMotor motorR; - private final Servo outtakeServo; private final PIDController flywheelVelocityControllerR = new PIDController(FLYWHEEL_KP, 0, 0); private final PIDController flywheelVelocityControllerL = new PIDController(FLYWHEEL_KP, 0, 0); @@ -29,7 +28,6 @@ public Outtake(HardwareMap hardwareMap){ motorL.setDirection(DcMotorSimple.Direction.FORWARD); motorR.setDirection(DcMotorSimple.Direction.REVERSE); - outtakeServo = hardwareMap.servo.get("outtakePitch"); } @@ -101,7 +99,5 @@ public double currPosR(){ //Stops here - public void controlPitch(double angle){ - outtakeServo.setPosition(angle); - } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 69a15f967bdc..13296b6023cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -71,7 +71,6 @@ public void runOpMode() throws InterruptedException { outtake.stop(); } - outtake.controlPitch(gamepad2.left_stick_y); fieldCentricDrive(); double currTime = endTimer.seconds(); From 7eb8eff37a7ddc57751e6cafb0a78616598fbd95 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 19:58:30 -0700 Subject: [PATCH 23/31] Added goals to the tele list --- .../org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 818434dcd87c..8e3f71490f8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -71,6 +71,9 @@ public void pidtunedmotor(Telemetry telemetry) { double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); + telemetry.addData("wantedWheelPowerTop", wantedWheelPowerTop); + telemetry.addData("wantedWheelPowerTop", wantedWheelPowerBottom); + outtake.setFlyWheelPower(wantedWheelPowerTop,wantedWheelPowerBottom); } From fb33dc4c60417255c3be01e91e21ad61ca0e7761 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 20:06:05 -0700 Subject: [PATCH 24/31] Changed what was printed --- .../firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 8e3f71490f8c..b975f1cacf76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -42,6 +42,10 @@ public void runOpMode() throws InterruptedException { telemetry.clear(); while (opModeIsActive()) { + tele.addData("goalRPMTop", rpmTop); + tele.addData("goalRPMTop", rpmBottom); + + pidtunedmotor(tele); tele.update(); } @@ -71,9 +75,6 @@ public void pidtunedmotor(Telemetry telemetry) { double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); - telemetry.addData("wantedWheelPowerTop", wantedWheelPowerTop); - telemetry.addData("wantedWheelPowerTop", wantedWheelPowerBottom); - outtake.setFlyWheelPower(wantedWheelPowerTop,wantedWheelPowerBottom); } From b453b5b3034cf6277cb44905cfc3384d5bb3893f Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 20:14:28 -0700 Subject: [PATCH 25/31] Changed what was printed --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index b975f1cacf76..7e669684c5a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -19,15 +19,19 @@ public class OuttakePIDTuner extends LinearOpMode { Outtake outtake; public static double rpmTop = 360; + private static double rpmTopPrev = 360; + public static double rpmBottom = 360; + private static double rpmBottomPrev = 360; + Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); private double prevPosTop, prevPosBottom, currPosTop, currPosBottom; private double startTime = System.nanoTime() / 1e9; - private final PIDController RPMControllerTop = new PIDController(KPTop, 0, 0); - private final PIDController RPMControllerBottom = new PIDController(KPBottom, 0, 0); + private PIDController RPMControllerTop = new PIDController(KPTop, 0, 0); + private PIDController RPMControllerBottom = new PIDController(KPBottom, 0, 0); @Override @@ -43,11 +47,19 @@ public void runOpMode() throws InterruptedException { while (opModeIsActive()) { tele.addData("goalRPMTop", rpmTop); - tele.addData("goalRPMTop", rpmBottom); + tele.addData("goalRPMBottom", rpmBottom); pidtunedmotor(tele); tele.update(); + if (rpmTopPrev != rpmTop) { + rpmTopPrev = rpmTop; + RPMControllerTop = new PIDController(KPTop, 0, 0); + } + if (rpmBottomPrev != rpmBottom) { + rpmBottomPrev = rpmBottom; + RPMControllerBottom = new PIDController(KPBottom, 0, 0); + } } } public void pidtunedmotor(Telemetry telemetry) { From fa5a2396caa832748a387e74e1efe2d9bb24192f Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 20:47:14 -0700 Subject: [PATCH 26/31] Some assorted things --- .../ftc/teamcode/DummyClasses/Outtake.java | 43 ++++++++----------- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 24 ++++++++++- 2 files changed, 41 insertions(+), 26 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java index 3589ebc5d7e3..e9dfd2a0c178 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/Outtake.java @@ -2,50 +2,45 @@ import static org.firstinspires.ftc.teamcode.riptideUtil.FLYWHEEL_KP; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.PIDCoefficients; -import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Modules.PIDController; -import org.firstinspires.ftc.teamcode.Robot; public class Outtake { - private final DcMotor motorL; - private final DcMotor motorR; + private final DcMotor topFlywheel; + private final DcMotor bottomFlywheel; private final PIDController flywheelVelocityControllerR = new PIDController(FLYWHEEL_KP, 0, 0); private final PIDController flywheelVelocityControllerL = new PIDController(FLYWHEEL_KP, 0, 0); public Outtake(HardwareMap hardwareMap){ - motorL = hardwareMap.dcMotor.get("topFlywheel"); - motorR = hardwareMap.dcMotor.get("bottomFlywheel"); + topFlywheel = hardwareMap.dcMotor.get("topFlywheel"); + bottomFlywheel = hardwareMap.dcMotor.get("bottomFlywheel"); - motorL.setDirection(DcMotorSimple.Direction.FORWARD); - motorR.setDirection(DcMotorSimple.Direction.REVERSE); + topFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); + bottomFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); } public void start(double speed){ - motorR.setPower(speed); - motorL.setPower(speed); + bottomFlywheel.setPower(speed); + topFlywheel.setPower(speed); } //This one method is only to set motors individually public void setFlyWheelPower(double speedL, double speedR) { - motorR.setPower(speedR); - motorL.setPower(speedL); + bottomFlywheel.setPower(speedR); + topFlywheel.setPower(speedL); } public void stop(){ - motorR.setPower(0); - motorL.setPower(0); + bottomFlywheel.setPower(0); + topFlywheel.setPower(0); } private double startTime = System.nanoTime() / 1e9; @@ -55,8 +50,8 @@ public void stop(){ public void setFlywheelSpeed(double goalRPM){ - int currentTickCountL = motorL.getCurrentPosition(); - int currentTickCountR = motorR.getCurrentPosition(); + int currentTickCountL = topFlywheel.getCurrentPosition(); + int currentTickCountR = bottomFlywheel.getCurrentPosition(); double dthetaL = (currentTickCountL - previousTickCountL)/28D; previousTickCountL = currentTickCountL; @@ -75,8 +70,8 @@ public void setFlywheelSpeed(double goalRPM){ double wantedWheelPowerR = flywheelVelocityControllerR.calculate(currRPMR, goalRPM); double wantedWheelPowerL = flywheelVelocityControllerL.calculate(currRPML, goalRPM); - motorL.setPower(wantedWheelPowerL); - motorR.setPower(wantedWheelPowerR); + topFlywheel.setPower(wantedWheelPowerL); + bottomFlywheel.setPower(wantedWheelPowerR); } @@ -85,16 +80,16 @@ public void startFlywheel(){ } public double currPos(){ - double currPos = (double) (motorR.getCurrentPosition() + motorL.getCurrentPosition()) / 2; + double currPos = (double) (bottomFlywheel.getCurrentPosition() + topFlywheel.getCurrentPosition()) / 2; return currPos; } //Created to allow OuttakePIDTuner to access individual positions public double currPosL(){ - return motorL.getCurrentPosition(); + return topFlywheel.getCurrentPosition(); } public double currPosR(){ - return motorR.getCurrentPosition(); + return bottomFlywheel.getCurrentPosition(); } //Stops here diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 7e669684c5a3..c56399ea5139 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -45,6 +45,8 @@ public void runOpMode() throws InterruptedException { telemetry.clear(); + startTime = System.nanoTime() / 1e9; + while (opModeIsActive()) { tele.addData("goalRPMTop", rpmTop); tele.addData("goalRPMBottom", rpmBottom); @@ -54,6 +56,25 @@ public void runOpMode() throws InterruptedException { tele.update(); if (rpmTopPrev != rpmTop) { rpmTopPrev = rpmTop; + /* + set: + KPTop=0.2 + KPBottom=0.2 + rpmTop=300 + rpmBottom=300 + 1: + currPRMTop: 0.0 + currRPMBottom: 2691.987410571929 + + 2: + + + + */ + + + + RPMControllerTop = new PIDController(KPTop, 0, 0); } if (rpmBottomPrev != rpmBottom) { @@ -64,8 +85,7 @@ public void runOpMode() throws InterruptedException { } public void pidtunedmotor(Telemetry telemetry) { - startTime = System.nanoTime() / 1e9; - + prevPosTop = currPosTop; prevPosBottom = currPosBottom; From 8109f831156fa6fa087e76b89ccbae18d8539407 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Fri, 10 Oct 2025 21:32:18 -0700 Subject: [PATCH 27/31] completely unfinished --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index c56399ea5139..e983bacdc8e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -11,14 +11,20 @@ import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.DummyClasses.Outtake; +import java.util.LinkedList; +import java.util.Queue; + @Config @TeleOp(name = "OuttakeTuner",group = "Tuning") public class OuttakePIDTuner extends LinearOpMode { public static double KPTop = 0; public static double KPBottom = 0; + LinkedList topRecords = new LinkedList<>(); + LinkedList bottomRecords = new LinkedList<>(); Outtake outtake; public static double rpmTop = 360; + public static int queueSize = 5; private static double rpmTopPrev = 360; public static double rpmBottom = 360; @@ -47,7 +53,9 @@ public void runOpMode() throws InterruptedException { startTime = System.nanoTime() / 1e9; + while (opModeIsActive()) { + tele.addData("goalRPMTop", rpmTop); tele.addData("goalRPMBottom", rpmBottom); @@ -101,13 +109,29 @@ public void pidtunedmotor(Telemetry telemetry) { double currRPMTop = dThetaTop / (dt / 60); double currRPMBottom = dThetaBottom / (dt / 60); - telemetry.addData("currPRMTop", currRPMTop); - telemetry.addData("currRPMBottom", currRPMBottom); + // telemetry.addData("currPRMTop", currRPMTop); + // telemetry.addData("currRPMBottom", currRPMBottom); - double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); - double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); + //double wantedWheelPowerTop = RPMControllerTop.calculate(currRPMTop, rpmTop); + //double wantedWheelPowerBottom = RPMControllerBottom.calculate(currRPMBottom, rpmBottom); - outtake.setFlyWheelPower(wantedWheelPowerTop,wantedWheelPowerBottom); - } + topRecords.add(currRPMTop); + if (topRecords.size() > queueSize) + topRecords.remove(0); + bottomRecords.add(currRPMTop); + if (bottomRecords.size() > queueSize) + bottomRecords.remove(0); + + double averageTop = topRecords.size() >= queueSize ? topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4)/5 : currRPMTop; + double averageBottom = bottomRecords.size() >= queueSize ? bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4)/5 : currRPMBottom; + telemetry.addData("ready", bottomRecords.size() >= queueSize); + telemetry.addData("top", averageTop); + telemetry.addData("bottom", averageBottom); + + double wantedWheelPowerTopAverage = RPMControllerTop.calculate(averageTop, rpmTop); + double wantedWheelPowerBottomAverage = RPMControllerBottom.calculate(averageBottom, rpmBottom); + + outtake.setFlyWheelPower(wantedWheelPowerTopAverage,wantedWheelPowerBottomAverage); + } } \ No newline at end of file From 2b024d28f395221a768150a7cf0c83df3a202b2a Mon Sep 17 00:00:00 2001 From: TreeSquirrles Date: Fri, 10 Oct 2025 21:43:29 -0700 Subject: [PATCH 28/31] fixed average formula --- .../firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index e983bacdc8e3..e1ae0bf24a66 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -123,8 +123,8 @@ public void pidtunedmotor(Telemetry telemetry) { if (bottomRecords.size() > queueSize) bottomRecords.remove(0); - double averageTop = topRecords.size() >= queueSize ? topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4)/5 : currRPMTop; - double averageBottom = bottomRecords.size() >= queueSize ? bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4)/5 : currRPMBottom; + double averageTop = topRecords.size() >= queueSize ? (topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4))/5 : currRPMTop; + double averageBottom = bottomRecords.size() >= queueSize ? (bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4))/5 : currRPMBottom; telemetry.addData("ready", bottomRecords.size() >= queueSize); telemetry.addData("top", averageTop); telemetry.addData("bottom", averageBottom); From 3b5a8349548a41e8a2bcec606823525165f68876 Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Sat, 11 Oct 2025 18:23:37 -0700 Subject: [PATCH 29/31] I have no idea what I changed but it's complaining that I have done something --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index e983bacdc8e3..0bfa4eae55b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.Tuning; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -64,24 +63,6 @@ public void runOpMode() throws InterruptedException { tele.update(); if (rpmTopPrev != rpmTop) { rpmTopPrev = rpmTop; - /* - set: - KPTop=0.2 - KPBottom=0.2 - rpmTop=300 - rpmBottom=300 - 1: - currPRMTop: 0.0 - currRPMBottom: 2691.987410571929 - - 2: - - - - */ - - - RPMControllerTop = new PIDController(KPTop, 0, 0); } From 495eb15b0675095815bed6a74ad760d6819075c6 Mon Sep 17 00:00:00 2001 From: J A Date: Sat, 11 Oct 2025 18:34:38 -0700 Subject: [PATCH 30/31] test intake rename --- .../ftc/teamcode/UnitTests/TestIntake.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java index 13a8cf22324f..ad1b68035e70 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/TestIntake.java @@ -1,12 +1,19 @@ package org.firstinspires.ftc.teamcode.UnitTests; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +@Config +@TeleOp(name = "Test Intake") public class TestIntake extends LinearOpMode { DcMotor motor; + public static double speed; + + @Override public void runOpMode() throws InterruptedException { /* @@ -15,9 +22,9 @@ public void runOpMode() throws InterruptedException { * * * * * * * * * * * * * * * */ - motor = hardwareMap.dcMotor.get("motor"); + motor = hardwareMap.dcMotor.get("intakeMotor"); - motor.setDirection(DcMotor.Direction.FORWARD); + motor.setDirection(DcMotor.Direction.REVERSE); motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -46,7 +53,7 @@ public void runOpMode() throws InterruptedException { if(gamepad1.y){ - motor.setPower(1); + motor.setPower(speed); }else{ motor.setPower(0); } From 6f0159259983512e6bb465488935716f88d80e7d Mon Sep 17 00:00:00 2001 From: sarfie <110944623+sarfie@users.noreply.github.com> Date: Sat, 11 Oct 2025 19:12:01 -0700 Subject: [PATCH 31/31] swatting some big bugs --- .../ftc/teamcode/Tuning/OuttakePIDTuner.java | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java index 00bd65c265a8..1d19d0ba04b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/OuttakePIDTuner.java @@ -100,12 +100,32 @@ public void pidtunedmotor(Telemetry telemetry) { if (topRecords.size() > queueSize) topRecords.remove(0); - bottomRecords.add(currRPMTop); + bottomRecords.add(currRPMBottom); if (bottomRecords.size() > queueSize) bottomRecords.remove(0); - double averageTop = topRecords.size() >= queueSize ? (topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4))/5 : currRPMTop; - double averageBottom = bottomRecords.size() >= queueSize ? (bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4))/5 : currRPMBottom; + //these lines arent happy when you change queueSize +// double averageTop = topRecords.size() >= queueSize ? (topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4))/queueSize : currRPMTop; +// +// double averageBottom = bottomRecords.size() >= queueSize ? (bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4))/queueSize : currRPMBottom; +// //fix? (untested) + double undividedAverageTop = 0; + double undividedAverageBottom = 0; + for (int i = 0; i < topRecords.size(); i++) { + undividedAverageTop += topRecords.get(i); + undividedAverageBottom += topRecords.get(i); + } + double averageTop; + double averageBottom; + if (undividedAverageTop > 0) { + averageTop = undividedAverageTop / queueSize; + averageBottom = undividedAverageBottom / queueSize; + } else { + averageTop = currRPMTop; + averageBottom = currRPMBottom; + } + + telemetry.addData("ready", bottomRecords.size() >= queueSize); telemetry.addData("top", averageTop); telemetry.addData("bottom", averageBottom);