Skip to content

Commit

Permalink
FIRST-Tech-Challenge#3
Browse files Browse the repository at this point in the history
  • Loading branch information
@CRF1234
CRF1234 committed Nov 30, 2023
1 parent 4aa98cd commit a8b89bf
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 77 deletions.
19 changes: 15 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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
* <p>
* 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;
Expand All @@ -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);
Expand Down
109 changes: 36 additions & 73 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Tele.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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());
Expand All @@ -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);
Expand All @@ -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;
}
Expand All @@ -91,7 +94,6 @@ private void Vacuum() {
}

private void PullUp() {
double PUPow = 0;
if (gamepad1.triangle) {
PUPow = 0.1;
PU.setPower(1);
Expand All @@ -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() {
Expand All @@ -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());
Expand Down

0 comments on commit a8b89bf

Please sign in to comment.