SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Almost Working Speciem hanging code!

Browse Source
This commit is contained in:
robotics1
2024-11-21 17:29:15 -08:00
parent dbc9cf929b
commit c20c5ba624
6 changed files with 175 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
View File

@ -58,6 +58,7 @@ public class ClawTest extends LinearOpMode {
Gamepad previousGamepad1 = new Gamepad();
waitForStart();
claw.init();
runtime.reset();
// run until the end of the match (driver presses STOP)

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java
View File

@ -96,13 +96,14 @@ public class LiftWristArmTest extends LinearOpMode {
lift.toHighRung(),
arm.toBucketPosition(),
wrist.toRungPosition(),
new SleepAction(2),
new SleepAction(0.5),
lift.toHighRungAttach(),
new SleepAction(2),
new SleepAction(0.5),
claw.openClaw())
);
//Delete open claw }
//Delete open claw
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {

149
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java Normal file
View File

@ -0,0 +1,149 @@
/* Copyright (c) 2021 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.cometbots.tests;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
@TeleOp(name = "SpecimenTest", group = "Debug")
public class SpecimenTest extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
Follower robot = new Follower(hardwareMap);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
lift.init();
wrist.init();
arm.init();
claw.init();
robot.setMaxPower(.75);
robot.startTeleopDrive();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
//robot.update();
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
arm.setPosition(arm.getPosition() + .05);
}
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
claw.switchState();
}
if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
Actions.runBlocking(
new SequentialAction(
wrist.toFloorPosition(),
lift.toHighRung(),
wrist.toSpeciemenBar(),
lift.dropToHighRung()
)
);
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
Actions.runBlocking(
new SequentialAction(
claw.openClaw(),
wrist.toFloorPosition(),
lift.toFloorPosition()
)
);
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
wrist.setPosition(wrist.getPosition() + .05);
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.setPosition(lift.getPosition() + 175);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.setPosition(lift.getPosition() - 25);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.addData("Arm Position", arm.getPosition());
telemetry.update();
}
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -17,11 +17,15 @@ public class RobotConstants {
public final static double wristRung = 0.55;
public final static double wristPickup = 0.1;
public final static double wristSpeciemen = 0.1;
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToLowBucketPos = 2250;
public final static int liftToHighRung = 1245;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;
public final static int liftToHighRungAttach = 1050;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
@ -8,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBuc
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
@ -25,7 +28,7 @@ public class LiftActionsSubsystem {
public DcMotor lift;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG, HIGH_RUNG_DROP
}
private LiftState liftState;
@ -64,6 +67,11 @@ public class LiftActionsSubsystem {
return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
}
public Action dropToHighRung() {
return new MoveToPosition(dropToHighRung, LiftState.HIGH_RUNG_DROP);
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java
View File

@ -4,6 +4,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
import androidx.annotation.NonNull;
@ -17,7 +19,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristActionsSubsystem {
public enum WristState {
FLOOR, BUCKET, PICKUP, RUNG
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
}
public ServoImplEx wrist;
@ -48,6 +50,10 @@ public class WristActionsSubsystem {
public Action toFloorPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toSpeciemenBar() {
return new MoveToPosition(wristSpeciemen, WristState.SPECIMEN);
}
public Action toRungPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}