From a8b89bf1b5c1c908531385ebe4953bbd466caee3 Mon Sep 17 00:00:00 2001 From: CKK Date: Thu, 30 Nov 2023 17:42:04 +0700 Subject: [PATCH] #3 --- .../org/firstinspires/ftc/teamcode/Robot.java | 19 ++- .../ftc/teamcode/Teleop/Tele.java | 109 ++++++------------ 2 files changed, 51 insertions(+), 77 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 5355b6de4fc7..56684ff41680 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.util.Range; + import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -63,17 +65,24 @@ public static void MoveTargetPosition(double FL_Inches, double FR_Inches, BR.setTargetPosition(BR_Target); } - public static void SetDuoServoPos(double pos, Servo L_servo, Servo R_servo) { - pos = Math.min(Math.max(pos, 0), 1); + /** + * Default Range : float[] minMax = null + *

+ * Custom Range : float[] minMax = Your Range + */ + public static double SetDuoServoPos(double pos, float[] minMax, Servo L_servo, Servo R_servo) { + pos = minMax == null ? Range.clip(pos, 0, 1) : + Range.clip(pos, minMax[0], minMax[1]); L_servo.setPosition(pos); R_servo.setPosition(pos); + return pos; } public static void Initialize(IMU imu, DcMotor.RunMode moveMode, DcMotorEx motor1, DcMotorEx motor2, DcMotorEx motor3, DcMotorEx motor4, DcMotorEx motor5, DcMotorEx motor6, DcMotorEx motor7, DcMotorEx motor8, Servo servo1, Servo servo2, Servo servo3, Servo servo4, Servo servo5, - Servo servo6, Servo servo7, double liftAng) { + Servo servo6, Servo servo7, double[] DuoServoAng) { // Add Variable FL = motor1; FR = motor2; BL = motor3; BR = motor4; LL = motor5; RL = motor6; PU = motor7; V = motor8; @@ -90,7 +99,9 @@ public static void Initialize(IMU imu, DcMotor.RunMode moveMode, LA .setDirection(Servo.Direction.REVERSE); LH .setDirection(Servo.Direction.REVERSE); // Set Servo Position - SetDuoServoPos(liftAng, LLL, LRL); + SetDuoServoPos(DuoServoAng[0], null, LLL, LRL); + SetDuoServoPos(DuoServoAng[1], null, LA, RA); + SetDuoServoPos(DuoServoAng[2], null, LH, RH); // Reverse Motors FL.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Tele.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Tele.java index 31acc4e35df4..1eaaac27fb7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Tele.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Tele.java @@ -31,8 +31,8 @@ public class Tele extends LinearOpMode { DcMotorEx FL, FR, BL, BR, LL, RL, PU, V; /** Variables */ - public static double setpoint = Math.toDegrees(0), liftAng = 0, hoistAng = 0, armAng = 0; - boolean SqPressed = false, VisBusy = false; + public static double setpoint = Math.toDegrees(0), PUPow = 0, liftAng = 0.96, armAng = 0, hoistAng = 0; + boolean VPressed = false, VisBusy = false; private void Init() { // HardwareMap @@ -48,7 +48,7 @@ private void Init() { // Initialize Robot Robot.Initialize(imu, DcMotor.RunMode.RUN_WITHOUT_ENCODER, FL, FR, BL, BR, LL, RL, PU, V, - LLL, LRL, LA, RA, LH, RH, K, 0); + LLL, LRL, LA, RA, LH, RH, K, new double[]{liftAng, armAng, hoistAng}); // Show FTC Dashboard telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); @@ -57,15 +57,15 @@ private void Init() { private void Movement() { double X1 = gamepad1.left_stick_x; double Y1 = -gamepad1.left_stick_y; - double yaw = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double yaw = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double X2 = (Math.cos(yaw) * X1) - (Math.sin(yaw) * Y1); double Y2 = (Math.sin(yaw) * X1) + (Math.cos(yaw) * Y1); // Rotate - double R = pid_R.Calculate(WrapRads(setpoint - yaw)); + double R = pid_R.Calculate(WrapRads(setpoint + yaw)); double X = -gamepad1.right_stick_x; if (X != 0) { R = X; - setpoint = yaw; + setpoint = -yaw; } // Denominator for division to get no more than 1 double D = Math.max(Math.abs(X2) + Math.abs(Y2) + Math.abs(R), 1); @@ -75,14 +75,17 @@ private void Movement() { } private void Vacuum() { - if (!gamepad1.square) { - SqPressed = false; + boolean sq = gamepad1.square; + boolean ci = gamepad1.circle; + if (!(sq || ci)) { + VPressed = false; return; } - if (SqPressed) { return; } - SqPressed = true; + if (VPressed) { return; } + VPressed = true; if (!VisBusy) { - V.setPower(1); + double pow = sq ? 1 : -1; + V.setPower(pow); VisBusy = true; return; } @@ -91,7 +94,6 @@ private void Vacuum() { } private void PullUp() { - double PUPow = 0; if (gamepad1.triangle) { PUPow = 0.1; PU.setPower(1); @@ -102,73 +104,33 @@ private void PullUp() { PU.setPower(PUPow); //0.1 } -// private void DuoServo() { -// double[] pos = {0, 0, 0}; -// double[] speed = {1e-2, 1e-2, 1e-2}; -// boolean[][] button = {{gamepad1.left_bumper, gamepad1.right_bumper}, {gamepad1.dpad_left, gamepad1.dpad_right}, -// {gamepad1.dpad_up, gamepad1.dpad_down}}; -// Servo[][] servos = {{LLL, LRL}, {LA, RA}, {LH, RH}}; -// for (int i = 0; i < 3; i++) { -// pos[i] = button[i][0] ? + speed[i] : -// button[i][1] ? - speed[i] : pos[i]; -// Robot.SetDuoServoPos(pos[i], servos[i][0], servos[i][1]); -// } -// } - private void RaiseLift() { - double raiseSp = 0.01; - liftAng = gamepad1.left_bumper ? + raiseSp : - gamepad1.right_bumper ? - raiseSp : liftAng; - Robot.SetDuoServoPos(liftAng, LLL, LRL); + double raiseSp = 0.02; + liftAng = gamepad1.left_bumper ? liftAng + raiseSp : + gamepad1.right_bumper ? liftAng - raiseSp : liftAng; + liftAng = Robot.SetDuoServoPos(liftAng, new float[]{0, 0.96f}, LLL, LRL); } private void Arm() { - double armSp = 0.01; - armAng = gamepad1.dpad_left ? + armSp : - gamepad1.dpad_right ? - armSp : armAng; - Robot.SetDuoServoPos(armAng, LA, RA); + double armSp = 0.02; + armAng = gamepad1.dpad_left ? armAng + armSp : + gamepad1.dpad_right ? armAng - armSp : armAng; + armAng = Robot.SetDuoServoPos(armAng, null, LA, RA); } private void Hoist() { double hoistSp = 0.01; - hoistAng = gamepad1.dpad_up ? + hoistSp : - gamepad1.dpad_down ? - hoistSp : hoistAng; - Robot.SetDuoServoPos(hoistAng, LH, RH); + hoistAng = gamepad1.dpad_up ? hoistAng + hoistSp : + gamepad1.dpad_down ? hoistAng - hoistSp : hoistAng; + hoistAng = Robot.SetDuoServoPos(hoistAng, null, LH, RH); } -// private void Lift() { -// CurrentPosition = Math.max(LL.getCurrentPosition(), Math.max(ML.getCurrentPosition(), RL.getCurrentPosition())); -// double LT = gamepad1.left_trigger; -// double RT = gamepad1.right_trigger; -// boolean D_Up = gamepad1.dpad_up; -// boolean D_Down = gamepad1.dpad_down; -// int[] Levels = {-50, Robot.Ground_Junction, Robot.Low_Junction, Robot.Medium_Junction, Robot.High_Junction}; -// LT_press = LT >= 0.25 || LT_press; -// RT_press = RT >= 0.25 || RT_press; -// if (LT_press && (LT < 0.25)) { -// LT_press = false; -// Level--; -// } -// if (RT_press && (RT < 0.25)) { -// RT_press = false; -// Level++; -// } -// Level = D_Down || D_Up ? (CurrentPosition >= Robot.High_Junction ? 4 : -// CurrentPosition >= Robot.Medium_Junction ? 3 : -// CurrentPosition >= Robot.Low_Junction ? 2 : -// CurrentPosition >= Robot.Ground_Junction ? 1 : -// CurrentPosition >= 0 ? 0 : Level) : Level; -// -// Level = Range.clip(Level,0, 4); -// boolean Auto_Condition = !(D_Up) && !(D_Down) && (Lift_isAuto || LT >= 0.25 || RT >= 0.25); -// double Min_Power = 0.05; -// double Auto = (AtTargetRange(CurrentPosition, Levels[Level], 10) ? Min_Power : (CurrentPosition < Levels[Level] ? 1 : -0.75)); -//// double Control = (D_Up ? (CurrentPosition >= Robot.Max_Lift ? 0 : 1) : (D_Down ? (CurrentPosition <= 0 ? 0 : -1) : Min_Power)); -// double Control = (D_Up ? 1 : (D_Down ? (CurrentPosition <= -50 ? 0 : -1) : Min_Power)); -// double Lift_Power = Auto_Condition ? Auto : Control; -// Lift_isAuto = Auto_Condition; -// Robot.LiftPower(Lift_Power); -// } + private void Lift() { + double LT = gamepad1.left_trigger; + double RT = gamepad1.right_trigger; + double Lift_Power = LT >= 0.25 ? LT : RT >= 0.25 ? -RT : 0.05; + Robot.LiftPower(Lift_Power); + } @Override public void runOpMode() { @@ -182,10 +144,11 @@ public void runOpMode() { } Movement(); Vacuum(); - RaiseLift(); - Arm(); - Hoist(); - PullUp(); +// Lift(); +// RaiseLift(); +// Arm();ss +// Hoist(); +// PullUp(); telemetry.addData("setpoint",Math.toDegrees(setpoint)); telemetry.addData("LL", LL.getCurrentPosition()); telemetry.addData("RL", RL.getCurrentPosition());