Skip to content

Commit 3a144fe

Browse files
SM-mechSM-mech
committed
Changed Auto, new ramp, Field-Oriented controls, added a file to reset indexer
1 parent 4d77120 commit 3a144fe

File tree

6 files changed

+197
-33
lines changed

6 files changed

+197
-33
lines changed

‎TeamCode2026/src/main/java/com/mech/ftc/twentyfive/BlueAuto.java‎

Lines changed: 33 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
66
import com.acmerobotics.roadrunner.Action;
7+
import com.acmerobotics.roadrunner.ParallelAction;
78
import com.acmerobotics.roadrunner.Vector2d;
89
import com.arcrobotics.ftclib.controller.PIDController;
910
import com.mech.ftc.twentyfive.defaults.Camera;
@@ -13,6 +14,7 @@
1314
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
1415
import com.acmerobotics.roadrunner.Pose2d;
1516
import com.acmerobotics.roadrunner.ftc.Actions;
17+
import com.qualcomm.robotcore.hardware.DcMotor;
1618
import com.qualcomm.robotcore.hardware.DcMotorEx;
1719
import com.qualcomm.robotcore.hardware.DcMotorSimple;
1820
import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -30,34 +32,45 @@ public void runOpMode() throws InterruptedException {
3032
DcMotorEx frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft");
3133
DcMotorEx frontRight = hardwareMap.get(DcMotorEx.class, "frontRight");
3234
Servo kicker = hardwareMap.get(Servo.class, "servo");
35+
Servo wall = hardwareMap.get(Servo.class, "servoTwo");
3336

3437
double p = 0.01, i = 0.022, d = 0.000277;
3538
double f = 0.01;
3639
DcMotorEx indexMotor = hardwareMap.get(DcMotorEx.class, "indexMotor");
3740
launchMotor.setDirection(DcMotorSimple.Direction.REVERSE);
41+
indexMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
3842

3943
waitForStart();
4044

4145
Actions.runBlocking(
4246
drive.actionBuilder(beginPose)
4347
.strafeToConstantHeading(new Vector2d(-15,-15))
4448
.stopAndAdd(new Launch(launchMotor, frontLeft, frontRight, hardwareMap))
45-
.stopAndAdd(new Index(indexMotor, p, i, d, f, 0))
49+
.stopAndAdd(new ParallelAction(new Index(indexMotor, p, i, d, f, 0)))
4650
.waitSeconds(1.5)
51+
.stopAndAdd(new Wall(wall, 0))
52+
.waitSeconds(.5)
4753
.stopAndAdd(new Kick(kicker, .25))
4854
.waitSeconds(1.25)
55+
.stopAndAdd(new Wall(wall, .4))
4956
.stopAndAdd(new Kick(kicker, 0))
5057
.waitSeconds(.5)
51-
.stopAndAdd(new Index(indexMotor, p, i, d, f, 220))
52-
.waitSeconds(1)
58+
.stopAndAdd(new ParallelAction(new Index(indexMotor, p, i, d, f, 230)))
59+
.waitSeconds(1.5)
60+
.stopAndAdd(new Wall(wall, 0))
61+
.waitSeconds(.5)
5362
.stopAndAdd(new Kick(kicker, .25))
5463
.waitSeconds(.5)
64+
.stopAndAdd(new Wall(wall, .4))
5565
.stopAndAdd(new Kick(kicker, 0))
5666
.waitSeconds(.5)
57-
.stopAndAdd(new Index(indexMotor, p, i, d, f, 440))
58-
.waitSeconds(1)
67+
.stopAndAdd(new ParallelAction(new Index(indexMotor, p, i, d, f, 450)))
68+
.waitSeconds(1.5)
69+
.stopAndAdd(new Wall(wall, 0))
70+
.waitSeconds(.5)
5971
.stopAndAdd(new Kick(kicker, .25))
6072
.waitSeconds(.5)
73+
.stopAndAdd(new Wall(wall, .4))
6174
.stopAndAdd(new Kick(kicker, 0))
6275
.waitSeconds(100)
6376
.build());
@@ -77,6 +90,20 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
7790

7891
}
7992
}
93+
public class Wall implements Action {
94+
Servo wall;
95+
double pos;
96+
public Wall (Servo wall, double pos) {
97+
this.wall = wall;
98+
this.pos = pos;
99+
}
100+
@Override
101+
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
102+
wall.setPosition(pos);
103+
return false;
104+
105+
}
106+
}
80107
public class Launch implements Action {
81108
DcMotorEx launcherMotor;
82109
DcMotorEx leftMotor;
@@ -137,9 +164,7 @@ public boolean run(@NonNull TelemetryPacket telemetryPacket) {
137164
double power = pid + ff;
138165

139166
index.setPower(power);
140-
telemetryPacket.put("indexPosition", indexPosition);
141-
telemetryPacket.put("target", targetPosition);
142-
if (indexPosition >= targetPosition -2 && indexPosition <= targetPosition +2) {
167+
if (indexPosition >= targetPosition -1 && indexPosition <= targetPosition +1) {
143168
index.setPower(0);
144169
return false;
145170
}

‎TeamCode2026/src/main/java/com/mech/ftc/twentyfive/DriveCode.java‎

Lines changed: 55 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import com.mech.ftc.twentyfive.defaults.Velocity;
1111
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
1212
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
13+
import com.qualcomm.robotcore.hardware.DcMotor;
1314

1415
@Config
1516
@TeleOp(name="DriveCode")
@@ -35,6 +36,14 @@ public class DriveCode extends OpMode {
3536
FtcDashboard dashboard = FtcDashboard.getInstance();
3637
com.acmerobotics.dashboard.telemetry.TelemetryPacket packet = new com.acmerobotics.dashboard.telemetry.TelemetryPacket();
3738

39+
private PIDController headingController;
40+
public static double headingP = 0.03;
41+
public static double headingI = 0.0;
42+
public static double headingD = 0.001;
43+
public static double headingToleranceDeg = 0.2;
44+
public static double headingMaxPower = 0.6;
45+
private boolean prevY = false;
46+
3847
@Override
3948
public void init() {
4049
driveTrain.init(hardwareMap);
@@ -47,6 +56,8 @@ public void init() {
4756

4857
controller = new PIDController(p, i, d);
4958

59+
headingController = new PIDController(headingP, headingI, headingD);
60+
5061
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
5162
}
5263

@@ -55,14 +66,13 @@ public void init() {
5566

5667
@Override
5768
public void loop() {
58-
driveTrain.drive(gamepad1);
69+
//driveTrain.drive(gamepad1);
5970

6071
boolean fire = gamepad1.right_trigger > 0.5;
6172
boolean tagVisible = camera.TagID() != -1;
6273
double distanceM = camera.getTagDistance();
6374

6475
launcher.setEnabled(fire && tagVisible);
65-
launcher.update(distanceM);
6676

6777
if (gamepad1.b) {
6878
driveTrain.intake.setPower(1);
@@ -71,8 +81,45 @@ public void loop() {
7181
}
7282
if (gamepad1.a) {
7383
driveTrain.kicker.setPosition(0.25);
84+
driveTrain.wall.setPosition(0);
7485
} else {
7586
driveTrain.kicker.setPosition(-.5);
87+
driveTrain.wall.setPosition(.4);
88+
if (gamepad1.dpad_down) {
89+
driveTrain.wall.setPosition(0);
90+
}
91+
}
92+
93+
boolean yPressedOnce = gamepad1.y && !prevY;
94+
if (yPressedOnce) {
95+
headingController = new PIDController(headingP, headingI, headingD);
96+
}
97+
98+
if (gamepad1.y && tagVisible) {
99+
double bearing = camera.getTagBearing();
100+
if (Math.abs(bearing) > headingToleranceDeg) {
101+
double turn = headingController.calculate(bearing, 0);
102+
turn = Math.max(Math.min(turn, headingMaxPower), -headingMaxPower);
103+
driveTrain.drive(0, 0, (float) turn);
104+
} else {
105+
driveTrain.drive(0, 0, 0);
106+
}
107+
} else {
108+
driveTrain.drive(gamepad1);
109+
}
110+
111+
if (gamepad1.left_trigger > 0.5) {
112+
driveTrain.launchMotor.setPower(0.5);
113+
}
114+
else {
115+
launcher.update(distanceM);
116+
}
117+
if (gamepad1.dpad_up) {
118+
driveTrain.indexMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
119+
driveTrain.indexMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
120+
}
121+
if (gamepad1.x) {
122+
driveTrain.fieldOrientedVar(hardwareMap);
76123
}
77124

78125
boolean rb = gamepad1.right_bumper;
@@ -90,26 +137,25 @@ public void loop() {
90137
driveTrain.indexMotor.setPower(power);
91138

92139
if (rbPressedOnce) {
93-
targetPosition += 112;
140+
targetPosition += 115;
94141
} else if (lbPressedOnce) {
95-
targetPosition -= 112;
142+
targetPosition -= 115;
96143
}
97-
if (targetPosition > 561) targetPosition = 0;
98-
if (targetPosition < -561) targetPosition = 0;
144+
if (targetPosition > 576) targetPosition = 0;
145+
if (targetPosition < -576) targetPosition = 0;
99146
prevRightBumper = rb;
100147
prevLeftBumper = lb;
148+
prevY = gamepad1.y;
101149
getTelemetry();
102150
}
103151

104152
public void getTelemetry() {
105153
telemetry.addData("Forward Velocity (m/s): ", v.getForwardVelocity());
106154
telemetry.addData("Lateral Velocity (m/s): ", v.getLateralVelocity());
107155
telemetry.addData("ID", camera.TagID() + " Tag Distance (m) " + camera.getTagDistance());
108-
telemetry.addData("Launch Applied Power", driveTrain.launchMotor.getPower());
109156
telemetry.addData("Index Current Pos", driveTrain.indexMotor.getCurrentPosition());
110-
telemetry.addData("Index Power", driveTrain.indexMotor.getPower());
111-
telemetry.addData("launch velocity", driveTrain.launchMotor.getVelocity());
112157
telemetry.addData("Target", targetPosition);
158+
telemetry.addData("launch velocity", driveTrain.launchMotor.getVelocity());
113159
telemetry.addData("target Velocity", launcher.getTargetVelocity());
114160
dashboard.sendTelemetryPacket(packet);
115161
telemetry.update();

‎TeamCode2026/src/main/java/com/mech/ftc/twentyfive/defaults/Camera.java‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,40 @@ public double getTagDistance() {
8484
}
8585
return tagDistance*0.0254;
8686
}
87+
public double getTagDistanceY() {
88+
List<AprilTagDetection> detections = aprilTagProcessor.getDetections();
89+
double tagDistance = 0;
90+
for (AprilTagDetection tagDetected : detections) {
91+
if (tagDetected.metadata == null) {
92+
return 0;
93+
}
94+
tagDistance = tagDetected.ftcPose.y;
95+
}
96+
return tagDistance*0.0254;
97+
}
98+
public double getTagDistanceX() {
99+
List<AprilTagDetection> detections = aprilTagProcessor.getDetections();
100+
double tagDistance = 0;
101+
for (AprilTagDetection tagDetected : detections) {
102+
if (tagDetected.metadata == null) {
103+
return 0;
104+
}
105+
tagDistance = tagDetected.ftcPose.x;
106+
}
107+
return tagDistance*0.0254;
108+
}
109+
public double getTagBearing() {
110+
List<AprilTagDetection> detections = aprilTagProcessor.getDetections();
111+
double bearing = 0;
112+
for (AprilTagDetection tagDetected : detections) {
113+
if (tagDetected.metadata == null) {
114+
return 0;
115+
}
116+
bearing = tagDetected.ftcPose.bearing;
117+
}
118+
return bearing;
119+
}
120+
87121

88122

89123
}

‎TeamCode2026/src/main/java/com/mech/ftc/twentyfive/defaults/DriveTrain.java‎

Lines changed: 49 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,18 @@
1+
12
package com.mech.ftc.twentyfive.defaults;
23

3-
import com.qualcomm.robotcore.hardware.DcMotor;
4+
import com.qualcomm.hardware.bosch.BNO055IMU;
45
import com.qualcomm.robotcore.hardware.DcMotorEx;
56
import com.qualcomm.robotcore.hardware.DcMotorSimple;
67
import com.qualcomm.robotcore.hardware.Gamepad;
78
import com.qualcomm.robotcore.hardware.HardwareMap;
89
import com.qualcomm.robotcore.hardware.Servo;
910

11+
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
12+
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
13+
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
14+
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
15+
1016
public class DriveTrain {
1117
public DcMotorEx frontLeft;
1218
public DcMotorEx backRight;
@@ -16,6 +22,11 @@ public class DriveTrain {
1622
public DcMotorEx launchMotor;
1723
public DcMotorEx indexMotor;
1824
public Servo kicker;
25+
public Servo wall;
26+
27+
BNO055IMU imu;
28+
Orientation angles = new Orientation();
29+
double originalYaw;
1930

2031
public void init(HardwareMap hardwareMap) {
2132
frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft");
@@ -26,30 +37,41 @@ public void init(HardwareMap hardwareMap) {
2637
launchMotor = hardwareMap.get(DcMotorEx.class, "launchMotor");
2738
indexMotor = hardwareMap.get(DcMotorEx.class, "indexMotor");
2839
kicker = hardwareMap.get(Servo.class, "servo");
29-
40+
wall = hardwareMap.get(Servo.class, "servoTwo");
3041

3142
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
3243
launchMotor.setDirection(DcMotorSimple.Direction.REVERSE);
3344

34-
//launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
35-
indexMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
45+
indexMotor.setZeroPowerBehavior(com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE);
46+
fieldOrientedVar(hardwareMap);
3647
}
3748

49+
3850
public void drive(Gamepad gamepad) {
39-
float vertical = gamepad.left_stick_y;
40-
float horizontal = gamepad.left_stick_x;
41-
float pivot = gamepad.right_stick_x;
51+
angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
52+
double headingRad = Math.toRadians(angles.firstAngle);
53+
54+
double x = gamepad.left_stick_x;
55+
double y = -gamepad.left_stick_y;
56+
double rx = gamepad.right_stick_x;
57+
58+
double cos = Math.cos(-headingRad);
59+
double sin = Math.sin(-headingRad);
60+
double rotX = x * cos - y * sin;
61+
double rotY = x * sin + y * cos;
62+
63+
double frontLeftPower = -rotY - rotX + rx;
64+
double frontRightPower = rotY - rotX + rx;
65+
double backLeftPower = rotY - rotX - rx;
66+
double backRightPower = -rotY - rotX - rx;
4267

43-
float frontRightPower = vertical + horizontal + pivot;
44-
float backRightPower = -vertical + horizontal - pivot;
45-
float frontLeftPower = -vertical + horizontal + pivot;
46-
float backLeftPower = vertical + horizontal - pivot;
4768

4869
frontLeft.setPower(frontLeftPower);
4970
frontRight.setPower(frontRightPower);
50-
backRight.setPower(backRightPower);
5171
backLeft.setPower(backLeftPower);
72+
backRight.setPower(backRightPower);
5273
}
74+
5375
public void drive(float vertical, float horizontal, float pivot) {
5476
float frontRightPower = vertical + horizontal + pivot;
5577
float backRightPower = -vertical + horizontal - pivot;
@@ -61,4 +83,18 @@ public void drive(float vertical, float horizontal, float pivot) {
6183
backRight.setPower(backRightPower);
6284
backLeft.setPower(backLeftPower);
6385
}
64-
}
86+
87+
public void fieldOrientedVar(HardwareMap hardwareMap) {
88+
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
89+
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
90+
parameters.mode = BNO055IMU.SensorMode.IMU;
91+
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
92+
93+
imu = hardwareMap.get(BNO055IMU.class, "imu");
94+
imu.initialize((parameters));
95+
96+
angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
97+
98+
originalYaw = angles.firstAngle;
99+
}
100+
}

‎TeamCode2026/src/main/java/com/mech/ftc/twentyfive/defaults/Launcher.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -67,10 +67,10 @@ public void update(double distanceMeters) {
6767

6868
public double launchPower(double distanceMeters) {
6969
double y = 0.789;
70-
double x = distanceMeters - 2.5*0.0254;
70+
double x = distanceMeters;
7171
double u = v.getForwardVelocity();
72-
double maxInitialSpeed = 7;
73-
double angle = Math.toRadians(70);
72+
double maxInitialSpeed = 8.1;
73+
double angle = Math.toRadians(50);
7474
double g = 9.8;
7575

7676
double initialNeeded = initialNeeded(x, y, u, angle, g);

0 commit comments

Comments
 (0)