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

Commit Hang Subsystem new

Browse Source
This commit is contained in:
robotics3
2025-01-23 16:12:59 -08:00
parent 70d0a17d75
commit b14e91b094
3 changed files with 52 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@ -30,6 +31,7 @@ public class CometBotAutoDevelopment {
private ClawSubsystem claw;
private WristSubsystem wrist;
private ArmSubsystem arm;
private HangMotorSubsystem hang;
/*
Controllers
*/
@ -48,6 +50,7 @@ public class CometBotAutoDevelopment {
claw = new ClawSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
hang = new HangMotorSubsystem(hardwareMap);
this.gamepad1 = gamepad1;
this.gamepad2 = gamepad2;
@ -87,7 +90,10 @@ public class CometBotAutoDevelopment {
openThumb();
//hang
grabBlueberrySkyhook();
hangSkyhook();
robotDown();
robotUp();
stopHook();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
@ -97,10 +103,26 @@ public class CometBotAutoDevelopment {
wrist.grabBlueberrySkyhook();
arm.grabBlueberrySkyhook();
}
if(currentGamepad1.y){
wrist.hangBlueberrySkyhook();
arm.hangBlueberrySkyhook();
if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
claw.init();
}
private void hangSkyhook (){
dualSlides.toHangHeight();
wrist.hangBlueberrySkyhook();
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
arm.hangBlueberrySkyhook();
if(arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK){
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}claw.closeClaw();
}
}
private void openClaw(){
@ -155,12 +177,20 @@ public class CometBotAutoDevelopment {
}
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){
hang.robotToFloor();
}
}
private void stopHook(){
if(gamepad1.left_bumper){
hang.disableMotor();
}
}
private void dualSlidesToFloorPosition() {

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

@ -12,11 +12,13 @@ public class RobotConstants {
public final static double armReverseBucket = 0.08;
public final static double armPark = 0.33;
//value for grabbing the hook Specimen
public final static double grabBlueberry = 0.3;
public final static double grabBlueberry = 0.35;
public final static double armGrabBlueberrySkyhook = 0.15;
public final static double wristGrabBlueberrySkyhook = 0.1;
public final static double armHangBlueberrySkyhook = 0.6;
public final static double wristHangBlueberrySkyhook = 0.3;
public final static double wristGrabBlueberrySkyhook = 0.15;
public final static double armHangBlueberrySkyhook = 0.23;
public final static double wristHangBlueberrySkyhook = 0.7;
public final static int slideHangBlueberrySkyhook = 500;
public final static int slideBelowFloor = -150;
public final static int backwardBucketDrop = 4670;
public final static double armBucket = 0.45;
public final static double armInit = 0.135;

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

@ -5,6 +5,8 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@ -138,6 +140,14 @@ public class DualMotorSliderSubsystem {
public void toFixedPosition(int value) {
setTargetPosition(value);
}
public void toHangHeight(){
setTargetPosition(slideHangBlueberrySkyhook);
}
public void toHangBelowFloor(){
setTargetPosition(slideBelowFloor);
}
public int getFixedPosition() {
return liftSlideLeft.getCurrentPosition();