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

Updated Blueberry Specimen Grabing code and new values. Reprogrammed a few servos also and I haven't tweaked the code yet. The Blueberry Specimen Grab is UNTESTED. CAUTION

Browse Source
This commit is contained in:
robotics3
2025-01-16 17:07:41 -08:00
parent 8c6ce96ae4
commit 1a878eea1c
6 changed files with 68 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,20 +81,36 @@ public class CometBotAutoDevelopment {
//arm
armToParkPosition();
armAndWristToFloor();
armToBucketPosition();
//claw
openClaw();
openThumb();
//hang
grabBlueberrySkyhook();
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();
}
if(currentGamepad1.y){
wrist.hangBlueberrySkyhook();
arm.hangBlueberrySkyhook();
}
}
private void openClaw(){
if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
claw.switchState();
}
}
private void openThumb(){
if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
claw.switchState();
if(currentGamepad2.left_bumper && !currentGamepad2.left_bumper){
claw.switchTState();
}
}
private void armAndWristToFloor(){
@ -103,7 +119,7 @@ public class CometBotAutoDevelopment {
}
if(currentGamepad2.a && !previousGamepad2.a){
double increment = 0.6375 - arm.getPosition();
double increment = 0.5 - arm.getPosition();
for(int i = 0; i < 3; i ++){
arm.setPosition(arm.getPosition() + increment);
try {
@ -140,7 +156,7 @@ public class CometBotAutoDevelopment {
}
private void dualSlidesToFloorPosition() {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
dualSlides.toHighBucketPosition();
dualSlides.toFloorPosotion();
}
}
private void dualSlidesToLowBucketPosition() {

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

@ -7,9 +7,15 @@ public class RobotConstants {
public final static double clawClose = 0.85;
public final static double clawOpen = 0.05;
public final static double armFloor = 0.6375;
public final static double armFloor = 0.5;
public final static double armSubmarine = 0.55;
public final static double armPark = 0.1250;
public final static double armPark = 0.3;
//value for grabbing the hook Specimen
public final static double grabBlueberry = 0.3;
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 armBucket = 0.25;
public final static double wristPickup = 0.6;

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

@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
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.armPark;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -11,7 +13,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class ArmSubsystem {
public enum ArmState {
PARK, FLOOR, BUCKET, SUBMARINE
PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
}
private ServoImplEx arm;
@ -20,6 +22,14 @@ public class ArmSubsystem {
public ArmSubsystem(HardwareMap hardwareMap) {
this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO);
}
public void grabBlueberrySkyhook(){
setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
arm.setPosition(armGrabBlueberrySkyhook);
}
public void hangBlueberrySkyhook(){
setState(ArmState.HANG_BLUEBERRY_SKYHOOK);
arm.setPosition(armHangBlueberrySkyhook);
}
public void toParkPosition() {
arm.setPosition(armPark);

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

@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@ -11,7 +12,7 @@ import com.qualcomm.robotcore.hardware.Servo;
public class ClawSubsystem {
public enum ClawState {
CLOSED, OPEN
CLOSED, OPEN, GRAB_BLUEBERRY
}
public enum ThumbState {
@ -27,7 +28,10 @@ public class ClawSubsystem {
claw = hardwareMap.get(Servo.class, CLAW_NAME);
thumb = hardwareMap.get(Servo.class, THUMB_SERVO);
}
public void grabBlueberry(){
claw.setPosition(grabBlueberry);
state = ClawState.GRAB_BLUEBERRY;
}
public void closeClaw() {
claw.setPosition(clawClose);
state = ClawState.CLOSED;

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

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;
@ -81,7 +82,8 @@ public class DualMotorSliderSubsystem {
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE);
liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@ -121,11 +123,13 @@ public class DualMotorSliderSubsystem {
}
public void toLowBucketPosition() {
setTargetPosition(1500);
}
public void toHighBucketPosition() {
setTargetPosition(3000);
}
public void toHighBucketPosition() {
setTargetPosition(4000);
}
public void toFloorPosotion(){setTargetPosition(5);}
}

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

@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
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.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
@ -17,7 +19,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristSubsystem {
public enum WristState {
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
}
public ServoImplEx wrist;
@ -44,7 +46,18 @@ public class WristSubsystem {
return false;
}
}
public void grabBlueberrySkyhook(){
setState(WristState.GRAB_BLUEBERRY_SKYHOOK);
wrist.setPosition(wristGrabBlueberrySkyhook);
}
/**
* This function goes after grabBlueberrySkyhook
*/
public void hangBlueberrySkyhook(){
setState(WristState.HANG_BLUEBERRY_SKYHOOK);
wrist.setPosition(wristHangBlueberrySkyhook);
}
public void toFloorPosition() {
setState(WristState.FLOOR);
wrist.setPosition(wristFloor);