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

Added teleop code

Browse Source
This commit is contained in:
robotics1
2025-01-28 17:10:36 -08:00
parent 373dfd849a
commit 44448d642d
2 changed files with 86 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
View File

@ -97,71 +97,75 @@ public class CometBotAutoDevelopment {
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
private void grabBlueberrySkyhook () {
if(currentGamepad1.x){
claw.grabBlueberry();
wrist.grabBlueberrySkyhook();
arm.grabBlueberrySkyhook();
private void grabBlueberrySkyhook() {
if (currentGamepad1.x) {
// claw.grabBlueberry();
//wrist.grabBlueberrySkyhook();
//arm.grabBlueberrySkyhook();
}
/*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
claw.closeClaw();*/
}
private void hangSkyhook() {
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
dualSlides.toHangHeight();
//check wrist make go throught test files again
wrist.hangBlueberrySkyhook();
arm.hangBlueberrySkyhook();
if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
try {
Thread.sleep(2000);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
// wrist.hangBlueberrySkyhook();
// arm.hangBlueberrySkyhook();
// if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
// try {
// Thread.sleep(2000);
// } catch (InterruptedException e) {
// throw new RuntimeException(e);
// }
// }
}
}private void openClaw(){
if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){
}
private void openClaw() {
if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
claw.switchState();
}
}
private void openThumb(){
if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){
private void openThumb() {
if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
claw.switchTState();
}
}
private void armAndWristToFloor(){
if(currentGamepad2.a && !previousGamepad2.a){
double increment = 0.7 - arm.getPosition();
for(int i = 0; i < 3; i ++){
arm.setPosition(arm.getPosition() + increment);
try {
Thread.sleep(50);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
private void armAndWristToFloor() {
if (currentGamepad2.a && !previousGamepad2.a) {
arm.toFloorPositionTeleOp();
if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
wrist.toFloorPositionTeleop();
} else {
wrist.toPickupPosition();
}
arm.toFloorPosition();
}
if(currentGamepad2.a && !previousGamepad2.a ){
if (currentGamepad2.a && !previousGamepad2.a) {
wrist.switchState();
}
}
private void armToBucketPosition(){
if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
private void armToBucketPosition() {
if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
arm.toBucketPosition();
wrist.toBucketPosition();
}
}
private void armToParkPosition(){
if(currentGamepad2.x && !previousGamepad2.x){
private void armToParkPosition() {
if (currentGamepad2.x && !previousGamepad2.x) {
arm.toParkPosition();
wrist.toFloorPosition();
wrist.toFloorPositionTeleop();
}
}
@ -170,29 +174,34 @@ public class CometBotAutoDevelopment {
dualSlides.toHighBucketPosition();
}
}
private void robotUp(){
if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){
private void robotUp() {
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
dualSlides.toHangBelowFloor();
dualSlides.update();
hang.hangRobot();
}
}
private void robotDown(){
if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){
private void robotDown() {
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
hang.robotToFloor();
}
}
private void stopHook(){
if(gamepad1.left_bumper){
private void stopHook() {
if (gamepad1.left_bumper) {
hang.disableMotor();
}
}
private void dualSlidesToFloorPosition() {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
dualSlides.toFloorPosition();
}
}
private void dualSlidesToLowBucketPosition() {
if (currentGamepad2.b && !previousGamepad2.b) {
dualSlides.toLowBucketPosition();

37
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java
View File

@ -54,6 +54,11 @@ public class CometBotTeleopCompetition {
*/
public double currentPower = MAX_POWER;
/*
Misc
*/
private int hbCounter = 0;
public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
@ -175,12 +180,38 @@ public class CometBotTeleopCompetition {
*/
public void toHighBucketScore() {
if (this.currentGP2.y && !this.previousGP2.y) {
lift.toHighBucketPosition();
switch(hbCounter) {
case 0:
lift.toHighBucketPosition();
hbCounter = 1;
break;
case 1:
wrist.toBucketPosition();
arm.toBucketPosition();
hbCounter = 2;
break;
case 2:
claw.openClaw();
hbCounter = 3;
break;
case 3:
claw.closeClaw();
wrist.toTravelPosition();
arm.toParkPosition();
hbCounter = 4;
break;
case 4:
lift.toFloorPosition();
break;
default:
break;
}
hbCounter = 1;
if (lift.getFixedPosition() >= 4450) {
arm.toBucketPosition();
if(arm.getPosition() == 0.45)
if(arm.getPosition() == 0.45);
wrist.toBucketPosition();
if(wrist.getPosition() == 0.56)
if(wrist.getPosition() == 0.56);
claw.openClaw();
}
}