Skip to content

Commit

Permalink
readjusted horizontal slides and added saftey for the twist
Browse files Browse the repository at this point in the history
  • Loading branch information
@luenix1
luenix1 committed Jan 12, 2025
1 parent 6e9da9e commit 70f5051
Show file tree
Hide file tree
Showing 4 changed files with 62 additions and 7 deletions.
9 changes: 7 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,11 @@

public class Hardware extends HardwareMapper implements TriOdoProvider {
public static final double spinTickPerRev = 751.8;
public static final double RIGHT_SLIDE_OUT = 0.72;
public static final double RIGHT_SLIDE_OUT = 0.64;
@Deprecated public static final double LEFT_SLIDE_OUT = 1 - RIGHT_SLIDE_OUT;
public static final double RIGHT_SLIDE_IN = 0.45;
public static final double RIGHT_SLIDE_IN = 0.4;
@Deprecated public static final double LEFT_SLIDE_IN = 1 - RIGHT_SLIDE_IN;
public static final double CLAW_TWIST_INIT = 0.48;
public static final double SLIDE_INWARD_TIME = 0.75; // seconds
public static final double SLIDE_OUTWARD_TIME = 0.45; // seconds
public static final double SLIDE_OVERSHOOT = 0.05;
Expand Down Expand Up @@ -138,6 +139,9 @@ public static class Locks {
@HardwareName("clawFlip")
public Servo clawFlip;

@HardwareName("clawTwist")
public Servo clawTwist;

@HardwareName("horizontalSlide")
public Servo horizontalSlide;

Expand All @@ -153,6 +157,7 @@ public static class Locks {
@HardwareName("clawColor")
public ColorSensor clawColor;


@Override
public Encoder getLeftEncoder() {
return encoderLeft;
Expand Down
22 changes: 19 additions & 3 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@ public class MecanumTeleOp extends LinearOpMode {
double VerticalSlideSpeed = 0.75;
double ClawFrontPos = 0.5;
double ClawFlipPos = 1.0;
double TwistINIT = 0.48;



@Override
public void runOpMode() {
Expand All @@ -39,6 +42,7 @@ public void runOpMode() {
hardware.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
hardware.arm.setPower(Hardware.ARM_POWER);
hardware.wrist.setPosition(0.28);
hardware.clawTwist.setPosition(TwistINIT);
IntegratingGyroscope gyro;
NavxMicroNavigationSensor navxMicro;
ElapsedTime timer = new ElapsedTime();
Expand All @@ -63,6 +67,7 @@ public void runOpMode() {
double yaw_offset = 0.0;
while (opModeIsActive()) {


Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS);
if (gamepad1.back) {
yaw_offset = angles.firstAngle;
Expand Down Expand Up @@ -156,6 +161,7 @@ String formatDegrees(double degrees) {
int highChamberTicks = 790;
int highBasketTicks = 2180;


// lifts the vertical slides to a target position in ticks
private void targetLift(Hardware hardware, int targetPosition) {

Expand Down Expand Up @@ -362,7 +368,7 @@ private void stepper(Hardware hardware) {
ClawFlipPos = 1.0;
}
}
hardware.clawFlip.setPosition(ClawFlipPos);
hardware.clawTwist.setPosition(ClawFrontPos);
hardware.clawFront.setPosition(ClawFrontPos);
// clawFront close is 0
//clawFront open is 0.27
Expand Down Expand Up @@ -445,6 +451,11 @@ public void Horizontalpick(Hardware hardware) {
}

public void Flipout(Hardware hardware) {
double TwistPos = hardware.clawTwist.getPosition();
boolean FlipNotSafe = TwistPos != TwistINIT;
if (FlipNotSafe){
hardware.clawTwist.setPosition(TwistINIT);
}
hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_OUT);
hardware.horizontalLeft.setPosition(Hardware.LEFT_SLIDE_OUT);
sleep(500);
Expand All @@ -454,6 +465,11 @@ public void Flipout(Hardware hardware) {
}

public void Flipin(Hardware hardware) {
double TwistPos = hardware.clawTwist.getPosition();
boolean FlipNotSafe = TwistPos != TwistINIT;
if (FlipNotSafe){
hardware.clawTwist.setPosition(TwistINIT);
}
double fliponethird = 0.66;
hardware.clawFlip.setPosition(fliponethird);
ClawFlipPos = fliponethird;
Expand All @@ -467,7 +483,7 @@ public void Flipin(Hardware hardware) {

boolean lastX = false;

public void trasfer(Hardware hardware){
public void trasfer(Hardware hardware) {
boolean x = gamepad2.x;
if (x && !lastX) {
hardware.clawFront.setPosition(Hardware.FRONT_CLOSE);
Expand All @@ -484,7 +500,7 @@ public void trasfer(Hardware hardware){
hardware.clawFront.setPosition(Hardware.FRONT_OPEN);
sleep(500);
hardware.arm.setTargetPosition(0);
armTargetPosDeg=0;
armTargetPosDeg = 0;
sleep(500);
hardware.wrist.setPosition(0.28);
ClawFrontPos = Hardware.FRONT_OPEN;
Expand Down
35 changes: 34 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ private void hardwareInit() {
hardware.arm.setPower(0.3);
hardware.wrist.setPosition(0.28);
hardware.claw.setPosition(Hardware.CLAW_CLOSE);

hardware.clawTwist.setPosition(Hardware.CLAW_TWIST_INIT);
// we don't have the proxy object to handle this for us
// so manually implement the inversion
hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_IN);
Expand Down Expand Up @@ -237,6 +237,7 @@ public void runOpMode() {
stepper();
lift();
arm();
TwistStepper();

boolean shouldScoreHigh = gamepad2.left_trigger > 0.5;
boolean shouldScoreHigh2 = gamepad2.right_trigger > 0.5;
Expand Down Expand Up @@ -592,6 +593,8 @@ public void transferAndDrop() {
}

public void Flipout() {
double TwistPos = hardware.clawTwist.getPosition();
boolean FlipNotSafe = TwistPos != Hardware.CLAW_TWIST_INIT;
abandonLock(hSlideProxy.CONTROL);
abandonLock(hClawProxy.CONTROL_CLAW);
abandonLock(hClawProxy.CONTROL_FLIP);
Expand All @@ -607,6 +610,8 @@ public void Flipout() {
}

public void Flipin() {
double TwistPos = hardware.clawTwist.getPosition();
boolean FlipNotSafe = TwistPos != Hardware.CLAW_TWIST_INIT;
abandonLock(hSlideProxy.CONTROL);
abandonLock(hClawProxy.CONTROL_FLIP);
double flipThird = 0.66;
Expand Down Expand Up @@ -655,5 +660,33 @@ public void transfer() {
abandonLock(Locks.ArmAssembly);
transferInternal();
}
private void TwistStepper() {
double Position = 0.5;
if (gamepad1.dpad_left) {
Position = Hardware.FRONT_OPEN;
}
if (gamepad1.dpad_right) {
Position = Hardware.FRONT_CLOSE;
}
if (gamepad1.dpad_down) {
Position -= 0.01;
if(Position<0){
Position = 0.0;
}
}
if (gamepad1.dpad_up) {
Position += 0.01;
if(Position>1){
Position = 1.0;
}
}
hardware.clawTwist.setPosition(Position);
hardware.clawFront.setPosition(Position);
// clawFront close is 0
//clawFront open is 0.27

telemetry.addData("Pos", Position);
telemetry.addData("Pos", Position);
}

}
3 changes: 2 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoSteper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ public void runOpMode() {
// Set the servo to the new position and pause;
sleep(CYCLE_MS);
idle();
hardware.claw.setPosition(position);
hardware.horizontalSlide.setPosition(position);
hardware.horizontalLeft.setPosition(1-position);

}

Expand Down

0 comments on commit 70f5051

Please sign in to comment.