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

Commit Init Positions

Browse Source
This commit is contained in:
robotics3
2025-01-20 09:39:50 -08:00
parent 172c6659dc
commit 2a1513a2ba
3 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -61,8 +61,8 @@ public class CometBotAutoDevelopment {
public void init() { public void init() {
dualSlides.init(); dualSlides.init();
claw.init(); claw.init();
wrist.init(); wrist.initTeleOp();
arm.init(); arm.initTeleOp();
follower.setMaxPower(MAX_POWER); follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive(); follower.startTeleopDrive();
} }
@ -145,7 +145,7 @@ public class CometBotAutoDevelopment {
private void armToParkPosition(){ private void armToParkPosition(){
if(currentGamepad2.x && !previousGamepad2.x){ if(currentGamepad2.x && !previousGamepad2.x){
arm.toParkPosition(); arm.toParkPosition();
wrist.toFloorPosition();
} }
} }

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
View File

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
@ -13,7 +14,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class ArmSubsystem { public class ArmSubsystem {
public enum ArmState { public enum ArmState {
PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT
} }
private ServoImplEx arm; private ServoImplEx arm;
@ -45,6 +46,10 @@ public class ArmSubsystem {
arm.setPosition(armBucket); arm.setPosition(armBucket);
setState(ArmState.BUCKET); setState(ArmState.BUCKET);
} }
public void toInitPosition(){
arm.setPosition(armInit);
setState(ArmState.INIT);
}
public void setState(ArmState armState) { public void setState(ArmState armState) {
this.state = armState; this.state = armState;
@ -54,10 +59,14 @@ public class ArmSubsystem {
return this.state; return this.state;
} }
public void init() { public void initTeleOp() {
arm.resetDeviceConfigurationForOpMode(); arm.resetDeviceConfigurationForOpMode();
toParkPosition(); toParkPosition();
} }
public void initAuto(){
arm.resetDeviceConfigurationForOpMode();
toInitPosition();
}
public double getPosition() { public double getPosition() {
return this.arm.getPosition(); return this.arm.getPosition();

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
View File

@ -5,6 +5,7 @@ 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.wristFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
@ -19,7 +20,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristSubsystem { public class WristSubsystem {
public enum WristState { public enum WristState {
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT
} }
public ServoImplEx wrist; public ServoImplEx wrist;
@ -51,9 +52,7 @@ public class WristSubsystem {
wrist.setPosition(wristGrabBlueberrySkyhook); wrist.setPosition(wristGrabBlueberrySkyhook);
} }
/**
* This function goes after grabBlueberrySkyhook
*/
public void hangBlueberrySkyhook(){ public void hangBlueberrySkyhook(){
setState(WristState.HANG_BLUEBERRY_SKYHOOK); setState(WristState.HANG_BLUEBERRY_SKYHOOK);
wrist.setPosition(wristHangBlueberrySkyhook); wrist.setPosition(wristHangBlueberrySkyhook);
@ -72,6 +71,15 @@ public class WristSubsystem {
setState(WristState.PICKUP); setState(WristState.PICKUP);
wrist.setPosition(wristPickup); wrist.setPosition(wristPickup);
} }
public void toInitPosition(){
setState(WristState.INIT);
wrist.setPosition(wristInit);
}
public void InitAuto(){
wrist.resetDeviceConfigurationForOpMode();
toInitPosition();
}
public void setState(WristState wristState) { public void setState(WristState wristState) {
this.state = wristState; this.state = wristState;
@ -89,7 +97,7 @@ public class WristSubsystem {
return this.state; return this.state;
} }
public void init() { public void initTeleOp() {
wrist.resetDeviceConfigurationForOpMode(); wrist.resetDeviceConfigurationForOpMode();
toPickupPosition(); toPickupPosition();
} }