eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

senor goob

This commit is contained in:
ImposterVarunoverlord 2024-02-27 16:52:51 -06:00
parent 3b68b39ea9
commit d863843f94
5 changed files with 57 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -22,8 +22,7 @@ public class Arm {
private double armTempPos;
private double doorPos;
private double wristPos;
public static double ARM_KP = 0.2
;
public static double ARM_KP = 0.2;
public static double doorOpenPos = 0.36;
public static double doorClosePos = 0.61;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -17,7 +17,7 @@ public class Slides {
// public static double d = 0;
// public static double f = 0.01;
//p was 0.0014
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.02,0,0.01);
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.05,0,0.01);
public static double pTolerance = 20;
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -28,13 +28,13 @@ public class BlueBackStageAuto extends AutoBase {
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.8, 33.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 31.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.7, 32.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
@ -139,7 +139,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5)));
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.65)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
@ -224,7 +224,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3)));
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.75)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -139,7 +139,7 @@ public class RedBackStageAuto extends AutoBase {
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break;
case 2:

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -3,10 +3,14 @@ package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Arm;
@ -29,8 +33,29 @@ public class MainTeleOp extends OpMode {
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
@ -43,13 +68,35 @@ public class MainTeleOp extends OpMode {
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
// Driver 1
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
@ -175,5 +222,7 @@ public class MainTeleOp extends OpMode {
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}