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() {
dualSlides.init();
claw.init();
wrist.init();
arm.init();
wrist.initTeleOp();
arm.initTeleOp();
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
@ -145,7 +145,7 @@ public class CometBotAutoDevelopment {
private void armToParkPosition(){
if(currentGamepad2.x && !previousGamepad2.x){
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.armGrabBlueberrySkyhook;
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 com.qualcomm.robotcore.hardware.HardwareMap;
@ -13,7 +14,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class ArmSubsystem {
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;
@ -45,6 +46,10 @@ public class ArmSubsystem {
arm.setPosition(armBucket);
setState(ArmState.BUCKET);
}
public void toInitPosition(){
arm.setPosition(armInit);
setState(ArmState.INIT);
}
public void setState(ArmState armState) {
this.state = armState;
@ -54,10 +59,14 @@ public class ArmSubsystem {
return this.state;
}
public void init() {
public void initTeleOp() {
arm.resetDeviceConfigurationForOpMode();
toParkPosition();
}
public void initAuto(){
arm.resetDeviceConfigurationForOpMode();
toInitPosition();
}
public double 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.wristGrabBlueberrySkyhook;
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.wristSpeciemen;
@ -19,7 +20,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristSubsystem {
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;
@ -51,9 +52,7 @@ public class WristSubsystem {
wrist.setPosition(wristGrabBlueberrySkyhook);
}
/**
* This function goes after grabBlueberrySkyhook
*/
public void hangBlueberrySkyhook(){
setState(WristState.HANG_BLUEBERRY_SKYHOOK);
wrist.setPosition(wristHangBlueberrySkyhook);
@ -72,6 +71,15 @@ public class WristSubsystem {
setState(WristState.PICKUP);
wrist.setPosition(wristPickup);
}
public void toInitPosition(){
setState(WristState.INIT);
wrist.setPosition(wristInit);
}
public void InitAuto(){
wrist.resetDeviceConfigurationForOpMode();
toInitPosition();
}
public void setState(WristState wristState) {
this.state = wristState;
@ -89,7 +97,7 @@ public class WristSubsystem {
return this.state;
}
public void init() {
public void initTeleOp() {
wrist.resetDeviceConfigurationForOpMode();
toPickupPosition();
}