forked from FIRST-Tech-Challenge/FtcRobotController
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
commit 90dfb4d Author: rhsftc <[email protected]> Date: Mon Oct 28 14:09:23 2024 -0500 Added comments, cleaned up some unused variables. commit 242990d Author: rhsftc <[email protected]> Date: Sat Oct 26 16:03:20 2024 -0500 Added Test opModes commit 499e3dc Author: rhsftc <[email protected]> Date: Sat Oct 26 15:26:59 2024 -0500 Squashed commit of the following: commit 3e2dbf7 Author: Isha290411 <[email protected]> Date: Sat Oct 26 11:51:21 2024 -0500 made the lift actually work hardware issues, not software commit d21da73 Author: Isha290411 <[email protected]> Date: Sat Oct 26 09:57:33 2024 -0500 Update Lift.java trying to fix the lift motor and state machine commit 69533f6 Author: Isha290411 <[email protected]> Date: Sat Oct 26 09:44:20 2024 -0500 Update DriverControl.java commit 50bda32 Author: Isha290411 <[email protected]> Date: Thu Oct 24 19:46:29 2024 -0500 refactored the lift code made sure it would work commit 2ec1302 Author: Isha290411 <[email protected]> Date: Thu Oct 24 18:10:03 2024 -0500 Update RobotAutoDriveByGyro_Linear.java commit 30d6680 Author: rhsftc <[email protected]> Date: Fri Oct 25 12:27:51 2024 -0500 Some Lift refactors Changes to get the run to position working. commit c5b695b Author: rhsftc <[email protected]> Date: Fri Oct 25 12:07:00 2024 -0500 Squashed commit of the following: commit 50bda32 Author: Isha290411 <[email protected]> Date: Thu Oct 24 19:46:29 2024 -0500 refactored the lift code made sure it would work commit 2ec1302 Author: Isha290411 <[email protected]> Date: Thu Oct 24 18:10:03 2024 -0500 Update RobotAutoDriveByGyro_Linear.java commit 9fd8ae9 Author: rhsftc <[email protected]> Date: Wed Oct 23 13:57:37 2024 -0500 Added TODO comment commit 722c9ed Author: rhsftc <[email protected]> Date: Wed Oct 23 13:48:30 2024 -0500 Added Telemetry commit a7ae979 Author: rhsftc <[email protected]> Date: Wed Oct 23 13:15:25 2024 -0500 Gamepad variable changed to a local. commit c8d34ce Author: rhsftc <[email protected]> Date: Wed Oct 23 13:13:47 2024 -0500 Lift Motor changed to DcMotorEx. Added telementry. commit e486aee Author: rhsftc <[email protected]> Date: Wed Oct 23 12:14:24 2024 -0500 Refactored the Lift Update method. commit a69451d Merge: 8e4a217 d97d158 Author: rhsftc <[email protected]> Date: Wed Oct 23 11:49:46 2024 -0500 Merge branch 'Development' into rhs commit 8e4a217 Merge: 6b32f08 df03fe4 Author: rhsftc <[email protected]> Date: Tue Oct 22 18:48:29 2024 -0500 Merge branch 'Development' into rhs commit 6b32f08 Author: rhsftc <[email protected]> Date: Tue Oct 22 18:48:10 2024 -0500 Revert "Moved GamepadEx initialize to hopefully fix error." This reverts commit 24f2a1b. commit 24f2a1b Author: rhsftc <[email protected]> Date: Wed Oct 16 10:57:45 2024 -0500 Moved GamepadEx initialize to hopefully fix error.
- Loading branch information
Showing
6 changed files
with
510 additions
and
2 deletions.
There are no files selected for viewing
172 changes: 172 additions & 0 deletions
172
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DetectMotorType.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,172 @@ | ||
| /* Copyright (c) 2017 FIRST. All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without modification, | ||
| * are permitted (subject to the limitations in the disclaimer below) provided that | ||
| * the following conditions are met: | ||
| * | ||
| * Redistributions of source code must retain the above copyright notice, this list | ||
| * of conditions and the following disclaimer. | ||
| * | ||
| * Redistributions in binary form must reproduce the above copyright notice, this | ||
| * list of conditions and the following disclaimer in the documentation and/or | ||
| * other materials provided with the distribution. | ||
| * | ||
| * Neither the name of FIRST nor the names of its contributors may be used to endorse or | ||
| * promote products derived from this software without specific prior written permission. | ||
| * | ||
| * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS | ||
| * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, | ||
| * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
| * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE | ||
| * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL | ||
| * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR | ||
| * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, | ||
| * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
| */ | ||
|
|
||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| 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.DcMotorEx; | ||
| import com.qualcomm.robotcore.hardware.PIDFCoefficients; | ||
| import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; | ||
| import com.qualcomm.robotcore.util.ElapsedTime; | ||
|
|
||
| import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; | ||
|
|
||
| /** | ||
| * This op mode gets information about the connected motor. | ||
| * The code is structured as a LinearOpMode | ||
| * <p> | ||
| * This code assumes a DC motor configured with the name "motor" as is found on | ||
| * a Robot. | ||
| * <p> | ||
| * INCREMENT sets how much to increase/decrease the power each cycle | ||
| * CYCLE_MS sets the update period. | ||
| * <p> | ||
| * Remove or comment out the @Disabled line to add this opmode to the Driver | ||
| * Station OpMode list | ||
| */ | ||
| @TeleOp(name = "Detect Motor", group = "Test") | ||
| // @Disabled | ||
| public class DetectMotorType extends LinearOpMode { | ||
|
|
||
| private static final double INCREMENT = 0.01; // amount to ramp motor each CYCLE_MS cycle | ||
| private static final int CYCLE_MS = 25; // period of each cycle | ||
| private ElapsedTime timer; | ||
|
|
||
| // Define class members | ||
| private DcMotorEx motor; | ||
| // Motor information | ||
| private MotorConfigurationType type; | ||
| private double maxRPM; | ||
| double maxVelocity = 0; | ||
| private double velocity = 0; | ||
| private PIDFCoefficients pidfCoefficentsRunToPosition; | ||
| private PIDFCoefficients pidfCoefficientsRunUsing; | ||
| private double achievableTicsPerSecond; | ||
| private double ticsPerRev; | ||
| private int currentPosition; | ||
| private double current; | ||
| private double power = 0; | ||
| private boolean rampUp = true; | ||
|
|
||
| @Override | ||
| public void runOpMode() { | ||
| timer = new ElapsedTime(); | ||
| // Change the text in quotes to match any motor name on your robot. | ||
| motor = hardwareMap.get(DcMotorEx.class, "motor"); | ||
| motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); | ||
| type = motor.getMotorType(); | ||
| maxRPM = type.getMaxRPM(); | ||
| achievableTicsPerSecond = type.getAchieveableMaxTicksPerSecond(); | ||
| ticsPerRev = type.getTicksPerRev(); | ||
| currentPosition = motor.getCurrentPosition(); | ||
| getMaxVelocity(); | ||
| pidfCoefficentsRunToPosition = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION); | ||
| pidfCoefficientsRunUsing = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER); | ||
| telemetry.addData("Device Type", type.getName()); | ||
| telemetry.addData("RPM", "%5.2f", maxRPM); | ||
| telemetry.addData("Max Velocity", "%5.2f", maxVelocity); | ||
| telemetry.addData("Achievable Tics per Second", "%5.2f", achievableTicsPerSecond); | ||
| telemetry.addData("Tics Per Rev", "%5.2f", ticsPerRev); | ||
| telemetry.addData("Current Position", "%d", currentPosition); | ||
| telemetry.addData("PIDF Run Using", pidfCoefficientsRunUsing); | ||
| telemetry.addData("PIDF Run To Position", pidfCoefficentsRunToPosition); | ||
| telemetry.addData(">", "Press Start to run Motors."); | ||
| telemetry.update(); | ||
|
|
||
| // Wait for the start button | ||
| waitForStart(); | ||
| motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); | ||
| motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); | ||
| motor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficientsRunUsing); | ||
|
|
||
| // Ramp motor speeds till stop pressed. | ||
| while (opModeIsActive()) { | ||
|
|
||
| // Ramp the motors, according to the rampUp variable. | ||
| if (rampUp) { | ||
| // Keep stepping up until we hit the max value. | ||
| velocity += INCREMENT * maxVelocity; | ||
| if (velocity >= maxVelocity) { | ||
| velocity = maxVelocity; | ||
| rampUp = !rampUp; // Switch ramp direction | ||
| } | ||
| } else { | ||
| // Keep stepping down until we hit the min value. | ||
| velocity -= INCREMENT * maxVelocity; | ||
| if (velocity <= -maxVelocity) { | ||
| velocity = -maxVelocity; | ||
| rampUp = !rampUp; // Switch ramp direction | ||
| } | ||
| } | ||
|
|
||
| // Set the motor to the new power and pause; | ||
| motor.setVelocity(velocity); | ||
| sleep(CYCLE_MS); | ||
|
|
||
| power = motor.getPower(); | ||
| currentPosition = motor.getCurrentPosition(); | ||
| current = motor.getCurrent(CurrentUnit.MILLIAMPS); | ||
|
|
||
| // Display the current value | ||
| telemetry.addData("Motor Power", "%5.2f", power); | ||
| telemetry.addData("Velocity (Tics per Second)", "%5.2f", velocity); | ||
| telemetry.addData("Current Position", "%d", currentPosition); | ||
| telemetry.addData("Current (milli amps)", "%5.2f", current); | ||
| telemetry.addData(">", "Press Stop to end test."); | ||
| telemetry.update(); | ||
| idle(); | ||
| } | ||
|
|
||
| // Turn off motor and signal done; | ||
| motor.setPower(0); | ||
| telemetry.addData(">", "Done"); | ||
| telemetry.update(); | ||
| } | ||
|
|
||
| private void getMaxVelocity() { | ||
| motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); | ||
| motor.setPower(1); | ||
| timer.reset(); | ||
| while (timer.seconds() < 4 && !isStopRequested()) { | ||
| velocity = motor.getVelocity(); | ||
| if (velocity > maxVelocity) { | ||
| maxVelocity = velocity; | ||
| } | ||
| telemetry.addData("current velocity", "%5.2f", velocity); | ||
| telemetry.addData("maximum velocity", "%5.2f", maxVelocity); | ||
| telemetry.addData("Power", "%5.2f", motor.getPower()); | ||
| telemetry.update(); | ||
| } | ||
|
|
||
| motor.setPower(0); | ||
| velocity = 0; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
165 changes: 165 additions & 0 deletions
165
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EncoderTest.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,165 @@ | ||
| /* Copyright (c) 2017 FIRST. All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without modification, | ||
| * are permitted (subject to the limitations in the disclaimer below) provided that | ||
| * the following conditions are met: | ||
| * | ||
| * Redistributions of source code must retain the above copyright notice, this list | ||
| * of conditions and the following disclaimer. | ||
| * | ||
| * Redistributions in binary form must reproduce the above copyright notice, this | ||
| * list of conditions and the following disclaimer in the documentation and/or | ||
| * other materials provided with the distribution. | ||
| * | ||
| * Neither the name of FIRST nor the names of its contributors may be used to endorse or | ||
| * promote products derived from this software without specific prior written permission. | ||
| * | ||
| * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS | ||
| * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, | ||
| * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
| * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE | ||
| * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL | ||
| * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR | ||
| * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, | ||
| * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
| */ | ||
|
|
||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| import com.arcrobotics.ftclib.gamepad.GamepadEx; | ||
| import com.arcrobotics.ftclib.gamepad.GamepadKeys; | ||
| import com.qualcomm.robotcore.eventloop.opmode.OpMode; | ||
| import com.qualcomm.robotcore.eventloop.opmode.TeleOp; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.DcMotorEx; | ||
| import com.qualcomm.robotcore.hardware.DcMotorSimple; | ||
| import com.qualcomm.robotcore.hardware.PIDFCoefficients; | ||
| import com.qualcomm.robotcore.util.ElapsedTime; | ||
|
|
||
| import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; | ||
|
|
||
| /* | ||
| * Perform encoder tests on a motor. Implemented as an iterative OpMode. | ||
| */ | ||
| @TeleOp(name = "Encoder Test", group = "Test") | ||
| //@Disabled | ||
| public class EncoderTest extends OpMode { | ||
| private ElapsedTime runtime = new ElapsedTime(); | ||
| private GamepadEx gamePad; | ||
| private DcMotorEx motor; | ||
| private DcMotor.RunMode mode; | ||
| private final float ENCODER_INCREMENT = 145.1f * 50f; // 10 revolutions | ||
| private final double MAX_VELOCITY = 2900; | ||
| private double RUN_VELOCITY = MAX_VELOCITY * .7f; | ||
| private PIDFCoefficients pidfVelocityCoefficients; | ||
| private PIDFCoefficients pidfPositionCoefficients; | ||
|
|
||
| /** | ||
| * This method will be called once, when the INIT button is pressed. | ||
| */ | ||
| @Override | ||
| public void init() { | ||
| telemetry.addData("Status", "Initialized"); | ||
| telemetry.addLine("Dpad Up and Dpad Down"); | ||
| telemetry.update(); | ||
| gamePad = new GamepadEx(gamepad1); | ||
| motor = hardwareMap.get(DcMotorEx.class, "motor"); | ||
| motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); | ||
| motor.setDirection(DcMotorSimple.Direction.FORWARD); | ||
| motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); | ||
| pidfVelocityCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER); | ||
| pidfPositionCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION); | ||
| // These values are recommended as a starting point if you are tuning a PID. | ||
| pidfVelocityCoefficients.p = 1.063f; | ||
| pidfVelocityCoefficients.i = 1.063f; | ||
| pidfVelocityCoefficients.f = 10.63f; | ||
| motor.setVelocityPIDFCoefficients(pidfVelocityCoefficients.p, pidfVelocityCoefficients.i, pidfVelocityCoefficients.d, pidfVelocityCoefficients.f); | ||
| motor.setPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION, pidfPositionCoefficients); | ||
| motor.setPositionPIDFCoefficients(10f); | ||
| motor.setTargetPositionTolerance(5); | ||
| } | ||
|
|
||
| /** | ||
| * This method will be called repeatedly during the period between when | ||
| * the init button is pressed and when the play button is pressed (or the | ||
| * OpMode is stopped). | ||
| */ | ||
| @Override | ||
| public void init_loop() { | ||
| telemetry.addLine("Dpad Up and Dpad Down"); | ||
| telemetry.addLine("Y - Stop and Reset"); | ||
| telemetry.addData("Position", motor.getCurrentPosition()); | ||
| telemetry.update(); | ||
| } | ||
|
|
||
| /** | ||
| * This method will be called once, when the play button is pressed. | ||
| */ | ||
| @Override | ||
| public void start() { | ||
| runtime.reset(); | ||
| motor.setPower(0); | ||
| } | ||
|
|
||
| /** | ||
| * This method will be called repeatedly during the period between when | ||
| * the play button is pressed and when the OpMode is stopped. | ||
| */ | ||
| @Override | ||
| public void loop() { | ||
| gamePad.readButtons(); | ||
| telemetry.addData("Status", "Run Time: " + runtime.toString()); | ||
| if (gamePad.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { | ||
| motor.setTargetPosition((int) (motor.getCurrentPosition() + ENCODER_INCREMENT)); | ||
| motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); | ||
| motor.setVelocity(RUN_VELOCITY); | ||
| } | ||
|
|
||
| if (gamePad.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { | ||
| motor.setTargetPosition((int) (motor.getCurrentPosition() - ENCODER_INCREMENT)); | ||
| motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); | ||
| motor.setVelocity(RUN_VELOCITY); | ||
| } | ||
|
|
||
| if (gamePad.wasJustPressed(GamepadKeys.Button.Y)) { | ||
| stopAndResetEncoder(motor); | ||
| } | ||
|
|
||
| mode = motor.getMode(); | ||
| telemetry.addData("Target", "%d", motor.getTargetPosition()); | ||
| telemetry.addData("Position", "%d", motor.getCurrentPosition()); | ||
| telemetry.addData("Velocity", "%6.2f", motor.getVelocity()); | ||
| telemetry.addData("Power", "%6.2f", motor.getPower()); | ||
| telemetry.addData("Busy", motor.isBusy()); | ||
| telemetry.addData("Current (milli amps)", "%6.2f", motor.getCurrent(CurrentUnit.MILLIAMPS)); | ||
| telemetry.addData("Mode", mode); | ||
| telemetry.addData("PIDF Run Using", motor.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER)); | ||
| telemetry.addData("PIDF Run To Position", motor.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION)); | ||
| telemetry.update(); | ||
| } | ||
|
|
||
| /** | ||
| * This method will be called once, when this OpMode is stopped. | ||
| * <p> | ||
| * Your ability to control hardware from this method will be limited. | ||
| */ | ||
| @Override | ||
| public void stop() { | ||
| stopAndResetEncoder(motor); | ||
| } | ||
|
|
||
| /** | ||
| * This seems to be the only way to reliably stop a motor and reset the encoder. | ||
| * This wos only tested on a goBilda motor. | ||
| * | ||
| * @param motor | ||
| */ | ||
| private void stopAndResetEncoder(DcMotorEx motor) { | ||
| motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); | ||
| motor.setPower(0); | ||
| motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.