Skip to content

Commit c98b63a

Browse files
The1AndOnlyAlexThe1AndOnlyAlex
committed
Release version
tested on a light weight robot
1 parent 0608b6b commit c98b63a

File tree

5 files changed

+661
-333
lines changed

5 files changed

+661
-333
lines changed

‎TeamCode/src/main/java/AutoOpmodeMecanumPathPlan.java‎

Lines changed: 274 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,301 @@
1+
//package org.firstinspires.ftc.teamcode;
2+
3+
import android.util.Size;
4+
15
import com.arcrobotics.ftclib.command.CommandOpMode;
6+
import com.arcrobotics.ftclib.gamepad.GamepadEx;
7+
import com.arcrobotics.ftclib.hardware.GyroEx;
8+
import com.arcrobotics.ftclib.hardware.motors.Motor;
9+
10+
import Config.ApriltagsFieldData;
11+
import Config.DriveConstants;
212
import commands.Auto3PushCommand;
313
import subsystems.IntakeSubsystem;
414
import subsystems.MecanumDriveSubsystem;
515
import util.DashServer;
16+
617
import com.qualcomm.hardware.lynx.LynxModule;
18+
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
719
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
20+
import com.qualcomm.robotcore.hardware.DcMotor;
21+
import com.qualcomm.robotcore.hardware.IMU;
22+
23+
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
24+
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
25+
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
826
import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit;
27+
import org.firstinspires.ftc.vision.VisionPortal;
28+
import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
29+
import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary;
30+
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
31+
32+
import com.qualcomm.hardware.limelightvision.Limelight3A;
33+
934

1035
@Autonomous
11-
public class AutoOpmodeMecanumPathPlan extends CommandOpMode
12-
{
36+
public class AutoOpmodeMecanumPathPlan extends CommandOpMode {
37+
38+
private Motor frontLeft, frontRight, backLeft, backRight;
39+
GyroEx gyro;
1340
private MecanumDriveSubsystem mecanumDriveSubsystem;
1441
private IntakeSubsystem intakeSubsystem;
1542

43+
private AprilTagProcessor webcamAprilTag;
44+
45+
private Limelight3A limelightApriltag;
46+
47+
private static final boolean USE_DEBUG_FIELD_TAGS = true;
48+
49+
private void initWebCamAprilTag()
50+
{
51+
AprilTagLibrary apriltagLib;
52+
VisionPortal visionPortal;
53+
WebcamName apriltagCam;
54+
55+
apriltagCam = hardwareMap.get(WebcamName.class, "Webcam 1");
56+
57+
// Create the AprilTag processor.
58+
AprilTagProcessor.Builder myAprilTagProcessorBuilder = new AprilTagProcessor.Builder();
59+
if(USE_DEBUG_FIELD_TAGS)
60+
{
61+
AprilTagLibrary.Builder libBuilder = new AprilTagLibrary.Builder();
62+
libBuilder.addTag(ApriltagsFieldData.tag_2);
63+
libBuilder.addTag(ApriltagsFieldData.tag_42);
64+
myAprilTagProcessorBuilder.setTagLibrary(libBuilder.build());
65+
}
66+
else {
67+
myAprilTagProcessorBuilder.setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary());//libBuilder.build());////
68+
}
69+
//aprilTag = new AprilTagProcessor.Builder()
70+
71+
// The following default settings are available to un-comment and edit as needed.
72+
//.setDrawAxes(false)
73+
//.setDrawCubeProjection(false)
74+
//.setDrawTagOutline(true)
75+
//.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
76+
//.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
77+
//.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
78+
79+
// == CAMERA CALIBRATION ==
80+
// If you do not manually specify calibration parameters, the SDK will attempt
81+
// to load a predefined calibration for your camera.
82+
//.setLensIntrinsics(578.272, 578.272, 402.145, 221.506)
83+
// ... these parameters are fx, fy, cx, cy.
84+
myAprilTagProcessorBuilder.setDrawTagID(true);
85+
myAprilTagProcessorBuilder.setDrawTagOutline(true);
86+
myAprilTagProcessorBuilder.setDrawAxes(true);
87+
myAprilTagProcessorBuilder.setDrawCubeProjection(true);
88+
myAprilTagProcessorBuilder.setOutputUnits(DistanceUnit.METER, AngleUnit.DEGREES);
89+
//myAprilTagProcessorBuilder.setLensIntrinsics(0,0,0,0);
90+
webcamAprilTag = myAprilTagProcessorBuilder.build();
91+
92+
93+
94+
// Adjust Image Decimation to trade-off detection-range for detection-rate.
95+
// eg: Some typical detection data using a Logitech C920 WebCam
96+
// Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
97+
// Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
98+
// Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default)
99+
// Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default)
100+
// Note: Decimation can be changed on-the-fly to adapt during a match.
101+
//aprilTag.setDecimation(3);
102+
103+
// Create the vision portal by using a builder.
104+
VisionPortal.Builder builder = new VisionPortal.Builder();
105+
106+
builder.setCamera(apriltagCam);
107+
builder.setCameraResolution(new Size(640,480)); //1280, 720));// fps 4
108+
// Enable the RC preview (LiveView). Set "false" to omit camera monitoring.
109+
builder.enableLiveView(true);
110+
// Set the stream format; MJPEG uses less bandwidth than default YUY2.
111+
builder.setStreamFormat(VisionPortal.StreamFormat.MJPEG);//YUY2);
112+
// Choose whether or not LiveView stops if no processors are enabled.
113+
// If set "true", monitor shows solid orange screen if no processors enabled.
114+
// If set "false", monitor shows camera view without annotations.
115+
//builder.setAutoStopLiveView(false);
116+
//builder.setLiveViewContainerId(0);
117+
118+
// Set and enable the processor.
119+
builder.addProcessor(webcamAprilTag);
120+
121+
// Build the Vision Portal, using the above settings.
122+
visionPortal = builder.build();
123+
124+
// Disable or re-enable the aprilTag processor at any time.
125+
visionPortal.setProcessorEnabled(webcamAprilTag, true);
126+
127+
} // end method initAprilTag()
128+
129+
private void initLimelight()
130+
{
131+
try{
132+
limelightApriltag = hardwareMap.get(Limelight3A.class, "limelight");
133+
} catch(Exception e){
134+
limelightApriltag = null;}
135+
136+
if(limelightApriltag != null){
137+
limelightApriltag.pipelineSwitch(0);
138+
139+
/*
140+
* Starts polling for data. If you neglect to call start(), getLatestResult() will return null.
141+
*/
142+
limelightApriltag.start();
143+
}
144+
}
145+
16146
@Override
17147
public void initialize()
18148
{
19149
//telemetry.setMsTransmissionInterval(11);
20150

21151
intakeSubsystem = new IntakeSubsystem(telemetry);
22152

153+
initLimelight();
154+
initWebCamAprilTag();
155+
156+
frontLeft = new Motor(hardwareMap, "frontleft", Motor.GoBILDA.RPM_312);//RPM_435
157+
frontRight = new Motor(hardwareMap, "frontright", Motor.GoBILDA.RPM_312);//RPM_312
158+
backLeft = new Motor(hardwareMap, "backleft", Motor.GoBILDA.RPM_312);
159+
backRight = new Motor(hardwareMap, "backright", Motor.GoBILDA.RPM_312);
160+
161+
frontLeft.setInverted(true);
162+
backLeft.setInverted(true);
163+
164+
// frontLeft.setBuffer(1);
165+
// frontRight.setBuffer(1);
166+
// backLeft.setBuffer(1);
167+
// backRight.setBuffer(1);
168+
frontLeft.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
169+
frontRight.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
170+
backLeft.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
171+
backRight.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
172+
173+
frontLeft.stopAndResetEncoder();
174+
frontRight.stopAndResetEncoder();
175+
backLeft.stopAndResetEncoder();
176+
backRight.stopAndResetEncoder();
177+
178+
frontLeft.setRunMode(Motor.RunMode.VelocityControl);
179+
frontRight.setRunMode(Motor.RunMode.VelocityControl);
180+
backLeft.setRunMode(Motor.RunMode.VelocityControl);
181+
backRight.setRunMode(Motor.RunMode.VelocityControl);
182+
183+
frontLeft.setVeloCoefficients(1.33, 0, 0.007);
184+
frontLeft.setFeedforwardCoefficients(0, 0.1);
185+
frontRight.setVeloCoefficients(1.31, 0, 0.007);
186+
frontRight.setFeedforwardCoefficients(0, 0.1);
187+
backLeft.setVeloCoefficients(1.31, 0, 0.007);
188+
backLeft.setFeedforwardCoefficients(0, 0.1);
189+
backRight.setVeloCoefficients(1.33, 0, 0.007);
190+
backRight.setFeedforwardCoefficients(0, 0.1);
191+
192+
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
193+
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
194+
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
195+
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
196+
197+
frontLeft.setInverted(true);
198+
backLeft.setInverted(true);
199+
200+
frontLeft.encoder.reset();
201+
frontRight.encoder.reset();
202+
backLeft.encoder.reset();
203+
backRight.encoder.reset();
204+
frontLeft.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE);
205+
frontRight.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE);
206+
backLeft.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE);
207+
backRight.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE);
208+
209+
210+
double ACHIEVABLE_MAX_TICKS_PER_SECOND = frontLeft.ACHIEVABLE_MAX_TICKS_PER_SECOND;
211+
double ACHIEVABLE_MAX_DISTANCE_PER_SECOND = ACHIEVABLE_MAX_TICKS_PER_SECOND * DriveConstants.DISTANCE_PER_PULSE;
212+
// about 1.567 m/s
213+
214+
backRight.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
215+
frontRight.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
216+
frontLeft.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
217+
backLeft.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
218+
219+
gyro = new GyroEx() {
220+
IMU imu = hardwareMap.get(IMU.class, "imu");
221+
@Override
222+
public void init() {
223+
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
224+
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
225+
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
226+
227+
imu.initialize(new IMU.Parameters(orientationOnRobot));
228+
imu.resetYaw();
229+
}
230+
231+
@Override
232+
public double getHeading() {
233+
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
234+
}
235+
236+
@Override
237+
public double getAbsoluteHeading() {
238+
return 0;
239+
}
240+
241+
@Override
242+
public double[] getAngles()
243+
{
244+
return new double[] {
245+
imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES),
246+
imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES),
247+
imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)
248+
};
249+
}
250+
251+
@Override
252+
public com.arcrobotics.ftclib.geometry.Rotation2d getRotation2d() {
253+
return new com.arcrobotics.ftclib.geometry.Rotation2d(getHeading());
254+
}
255+
256+
@Override
257+
public void reset() {
258+
imu.resetYaw();
259+
}
260+
261+
@Override
262+
public void disable() {
263+
264+
}
265+
266+
@Override
267+
public String getDeviceType() {
268+
return "Internal IMU";
269+
}
270+
};
271+
272+
gyro.init();
273+
23274
mecanumDriveSubsystem = new MecanumDriveSubsystem(
24-
hardwareMap,
275+
frontLeft,
276+
frontRight,
277+
backLeft,
278+
backRight,
279+
gyro,
280+
webcamAprilTag,
281+
limelightApriltag,
25282
new edu.wpi.first.math.geometry.Pose2d(),
26283
telemetry
27284
);
28285

29-
schedule(new Auto3PushCommand(
30-
mecanumDriveSubsystem,
31-
intakeSubsystem
32-
));
286+
mecanumDriveSubsystem.enableDrive();
287+
288+
289+
// wpiMecanumDriveSubsystem.setDefaultCommand(
290+
// new ApriltagCommand(wpiMecanumDriveSubsystem, aprilTag, telemetry));
291+
292+
// update telemetry every loop
293+
//schedule(new RunCommand(telemetry::update));
294+
295+
schedule(new Auto3PushCommand(
296+
mecanumDriveSubsystem,
297+
intakeSubsystem,
298+
ACHIEVABLE_MAX_DISTANCE_PER_SECOND));
33299
}
34300

35301

@@ -57,7 +323,7 @@ public void runOpMode() {
57323
sleep(10);
58324
}
59325
reset();
60-
mecanumDriveSubsystem.subsystemExit();
326+
if(limelightApriltag != null) limelightApriltag.stop();
61327
DashServer.Close();
62328
}
63329
}

‎TeamCode/src/main/java/commands/Auto3PushCommand.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,11 +10,12 @@ public class Auto3PushCommand extends SequentialCommandGroup
1010
{
1111
public Auto3PushCommand(
1212
MecanumDriveSubsystem driveSubsystem,
13-
IntakeSubsystem gripSubsystem
13+
IntakeSubsystem gripSubsystem,
14+
double achievableMaxSpeed
1415
)
1516
{
1617
addCommands(
17-
new WaitCommand(1).whenFinished(driveSubsystem::enableDrive),
18+
1819
new commands.MecanumControllerCommand(
1920
Config.Path.Trajectory_GotoA,
2021
driveSubsystem::getCurrentEstimatedPose,
@@ -25,7 +26,7 @@ public Auto3PushCommand(
2526
0.1,0,0.005,
2627
new edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints(
2728
0.5,0.5)),
28-
driveSubsystem.getAchievableMaxSpeed(),
29+
achievableMaxSpeed,
2930
driveSubsystem::driveBySpeedEvent,
3031
driveSubsystem).whenFinished(driveSubsystem::stopDrive)
3132

0 commit comments

Comments
 (0)