API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 25th, 2020
117
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.00 KB | None | 0 0
raw download clone embed print report
  1. @Autonomous(name = "BlueSkystoneNOSplines", group = "Auto")
  2. public class BlueSkystoneNOSplines extends LinearOpMode {
  3. FtcDashboard dashboard = FtcDashboard.getInstance();
  4. Telemetry dashboardTelemetry = dashboard.getTelemetry();
  5. Intake intake = new Intake();
  6. Scorer scorer = new Scorer();
  7. int[] vals;
  8. SkystoneDetector detector = new SkystoneDetector();
  9.  
  10. public void setup() {
  11.  
  12. }
  13.  
  14. public void runOpMode() throws InterruptedException {
  15.  
  16. MecanumBase drive = new MecanumAuto(hardwareMap);
  17. intake.init(hardwareMap);
  18.  
  19.  
  20. //pattern1
  21. //get the first stone
  22. Trajectory mid1 = new TrajectoryBuilder(new Pose2d(-33.0,63.0, Math.toRadians(180.0)),
  23. new DriveConstraints(
  24. 25.0, 20.0, 0,
  25. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  26. .lineTo(new Vector2d(-20.0,36.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-150.0)))
  27. .addMarker(new Vector2d(-20.0,36.0), () ->
  28. {
  29. intake.collect(0.275); //in
  30. return Unit.INSTANCE;
  31.  
  32. })
  33. .lineTo(new Vector2d(-32.0, 24.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-150.0)))
  34. .addMarker(new Vector2d(-32.0, 24.0), () ->{
  35. intake.collect(0);
  36. return Unit.INSTANCE;
  37. })
  38. .build();
  39. //move to delivery
  40. Trajectory mid2 = new TrajectoryBuilder( /*new Pose2d(-32.0, 24.0, Math.toRadians(180.0))*/mid1.end(),
  41. new DriveConstraints(
  42. 45.0, 25.0, 0,
  43. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  44. .lineTo(new Vector2d(-16.0, 38.0), new SplineInterpolator(Math.toRadians(-145.0), Math.toRadians(-180.0)))
  45. .reverse()
  46. .lineTo(new Vector2d(24.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  47. .reverse()
  48. .addMarker(new Vector2d(24.0,40.0), () ->{
  49. intake.collect(-0.8);
  50. return Unit.INSTANCE;
  51. })
  52. .lineTo(new Vector2d(-48.0, 40.0),new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-150.0)))
  53. .addMarker(new Vector2d(0.0, 40.0), () ->{
  54. intake.collect(0);
  55. return Unit.INSTANCE;
  56. })
  57. .addMarker(new Vector2d(-48.0,40.0), () ->
  58. {
  59. intake.collect(0.275); //in
  60. return Unit.INSTANCE;
  61.  
  62. })
  63. .build();
  64. // get second skystone
  65. Trajectory mid3 = new TrajectoryBuilder(/*new Pose2d(-48.0, 40.0, Math.toRadians(180.0))*/mid2.end(),
  66. new DriveConstraints(25.0, 20.0, 0,
  67. Math.toRadians(180.0),Math.toRadians(180.0), 0.0))
  68. .lineTo(new Vector2d(-56.0, 24.0), new SplineInterpolator(Math.toRadians(-150.0), Math.toRadians(-150.0)))
  69. .addMarker(new Vector2d(-56.0, 24.0), () ->{
  70. intake.collect(0);
  71. return Unit.INSTANCE;
  72. })
  73. .build();
  74. //deposit second skystone
  75. Trajectory mid4 = new TrajectoryBuilder(/*new Pose2d(-56.0, 24.0, Math.toRadians(180.0))*/ mid3.end(),
  76. new DriveConstraints(
  77. 45.0, 25.0, 0,
  78. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  79. .reverse()
  80. .lineTo(new Vector2d(24.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  81. .reverse()
  82. .addMarker(new Vector2d(24.0,40.0), () ->{
  83. intake.collect(-0.8);
  84. return Unit.INSTANCE;
  85. })
  86.  
  87. .lineTo(new Vector2d(0.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  88. .addMarker(new Vector2d(0.0, 40.0), () ->{
  89. intake.collect(0);
  90. return Unit.INSTANCE;
  91. })
  92. .build();
  93.  
  94. //left stone
  95. Trajectory left1 = new TrajectoryBuilder(new Pose2d(-33.0,63.0, Math.toRadians(180.0)),
  96. new DriveConstraints(
  97. 25.0, 20.0, 0,
  98. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  99. .lineTo(new Vector2d(-40.0, 36.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-140.0)))
  100. .addMarker(new Vector2d(-40.0,36.0), () ->{
  101. intake.collect(.275);
  102. return Unit.INSTANCE;
  103. })
  104. .lineTo(new Vector2d(-48.0, 24.0), new SplineInterpolator(Math.toRadians(-140.0),Math.toRadians(-140.0)))
  105. .addMarker(new Vector2d(-48.0, 24.0), () ->{
  106. intake.collect(0);
  107. return Unit.INSTANCE;
  108. })
  109. .build();
  110. Trajectory left2 = new TrajectoryBuilder(left1.end() /*new Pose2d(-48.0, 24.0, Math.toRadians(180.0))*/,
  111. new DriveConstraints(
  112. 45.0, 25.0, 0,
  113. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  114. .lineTo(new Vector2d(-40.0,40.0), new SplineInterpolator(Math.toRadians(-140.0), Math.toRadians( -180.0)))
  115. .reverse()
  116. .lineTo(new Vector2d(24.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  117. .reverse()
  118. .addMarker(new Vector2d(24.0,40.0), () ->{
  119. intake.collect(-0.8);
  120. return Unit.INSTANCE;
  121. })
  122. .lineTo(new Vector2d(-15.0, 40.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-130.0)))
  123. .addMarker(new Vector2d(0.0, 40.0), () ->{
  124. intake.collect(0);
  125. return Unit.INSTANCE;
  126. })
  127. .addMarker(new Vector2d(-15.0,40.0), () ->
  128. {
  129. intake.collect(0.275); //in
  130. return Unit.INSTANCE;
  131.  
  132. })
  133. .build();
  134. //move to second skystone
  135. Trajectory left3 = new TrajectoryBuilder(/*new Pose2d(-15.0, 40.0, Math.toRadians(180.0))*/ left2.end(),
  136. new DriveConstraints(25.0, 20.0, 0,
  137. Math.toRadians(180.0),Math.toRadians(180.0), 0.0))
  138. .lineTo(new Vector2d(-23.0, 24.0), new SplineInterpolator(Math.toRadians(-130.0), Math.toRadians(-130.0)))
  139. .addMarker(new Vector2d(-23.0, 24.0), () ->{
  140. intake.collect(0);
  141. return Unit.INSTANCE;
  142. })
  143. .build();
  144. //get second skystone
  145. Trajectory left4 = new TrajectoryBuilder(/*new Pose2d(-23.0, 24.0, Math.toRadians(180.0))*/ left3.end(),
  146. new DriveConstraints(
  147. 45.0, 25.0, 0,
  148. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  149. .lineTo(new Vector2d(-20.0,40.0), new SplineInterpolator(Math.toRadians(-140.0), Math.toRadians( -180.0)))
  150. .reverse()
  151. .lineTo(new Vector2d(24.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  152. .reverse()
  153. .lineTo(new Vector2d(0.0,40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  154. .build();
  155. //pattern 3 or right stone
  156.  
  157. Trajectory right1 = new TrajectoryBuilder(drive.getPoseEstimate(),
  158. new DriveConstraints(
  159. 25.0, 20.0, 0,
  160. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  161. .lineTo(new Vector2d(-30.0,36.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-145.0)))
  162. .addMarker(new Vector2d(-30.0, 38.0), () ->{
  163. intake.collect(0.275);
  164. return Unit.INSTANCE;
  165. })
  166. .lineTo(new Vector2d(-38.0,24.0), new SplineInterpolator(Math.toRadians(-145.0), Math.toRadians(-145.0)))
  167. .addMarker(new Vector2d(-38.0, 24.0), () ->
  168. {
  169. intake.collect(0);
  170. return Unit.INSTANCE;
  171. })
  172. .build();
  173. //move to delivery
  174. Trajectory right2 = new TrajectoryBuilder(right1.end(),
  175. new DriveConstraints(
  176. 45.0, 25.0, 0,
  177. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  178. .lineTo(new Vector2d(-30.0, 40.0), new SplineInterpolator(Math.toRadians(-145.0), Math.toRadians(-180.0)))
  179. .reverse()
  180. .lineTo(new Vector2d(24.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  181. .reverse()
  182. .addMarker(new Vector2d(24.0, 40.0), ()->{
  183. intake.collect(-0.8);
  184. return Unit.INSTANCE;
  185. })
  186. .lineTo(new Vector2d(-53.0,40.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-145.0)))
  187. .addMarker(new Vector2d(0.0, 40.0), ()->{
  188. intake.collect(0);
  189. return Unit.INSTANCE;
  190. })
  191. .addMarker(new Vector2d(-53.0, 40.0), ()->{
  192. intake.collect(0.275);
  193. return Unit.INSTANCE;
  194. })
  195. .build();
  196. //move to second skystone
  197. Trajectory right3 = new TrajectoryBuilder(right2.end(),
  198. new DriveConstraints(25.0, 20.0, 0,
  199. Math.toRadians(180.0),Math.toRadians(180.0), 0.0))
  200. .lineTo(new Vector2d(-61.0,24.0), new SplineInterpolator(Math.toRadians(-145.0), Math.toRadians(-145.0)))
  201. .addMarker(new Vector2d(-61.0, 24.0), ()->{
  202. intake.collect(0);
  203. return Unit.INSTANCE;
  204. })
  205. .build();
  206. //get second skystone
  207. Trajectory right4 = new TrajectoryBuilder(right3.end(),
  208. new DriveConstraints(
  209. 45.0, 20.0, 0,
  210. Math.toRadians(180.0), Math.toRadians(180.0), 0.0))
  211. .reverse()
  212. .lineTo(new Vector2d(24.0, 40.0),new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  213. .reverse()
  214. .addMarker(new Vector2d(24.0,40.0), () ->{
  215. intake.collect(-0.8);
  216. return Unit.INSTANCE;
  217. })
  218.  
  219. .lineTo(new Vector2d(0.0, 40.0), new LinearInterpolator(Math.toRadians(-90.0), Math.toRadians(-90.0)))
  220. .addMarker(new Vector2d(0.0, 40.0), () ->{
  221. intake.collect(0);
  222. return Unit.INSTANCE;
  223. })
  224. .build();
  225.  
  226. //else condition
  227. Trajectory park = new TrajectoryBuilder(drive.getPoseEstimate(), DriveConstants.BASE_CONSTRAINTS)
  228. .lineTo(new Vector2d(-33.0, 40.0), new SplineInterpolator(Math.toRadians(-90.0), Math.toRadians(-180.0)))
  229. .lineTo(new Vector2d(0.0, 40.0), new SplineInterpolator(Math.toRadians(-180.0), Math.toRadians(-180.0)))
  230. .build();
  231.  
  232. detector.setOffset(-1f / 8f, 3f / 8f);
  233. detector.camSetup(hardwareMap);
  234.  
  235. waitForStart();
  236.  
  237. while (!opModeIsActive() && !isStopRequested()) {
  238. telemetry.addData("status", "waiting for start command...");
  239. telemetry.update();
  240. }
  241.  
  242. if (isStopRequested()) return;
  243.  
  244. intake.hitter.setPosition(0.4); //closed
  245.  
  246. if (opModeIsActive()) {
  247.  
  248. detector.updateVals();
  249. vals = detector.getVals();
  250. telemetry.addData("Values", vals[1] + " " + vals[0] + " " + vals[2]);
  251. telemetry.update();
  252.  
  253. if (vals[0] == 0) { //mid
  254. drive.followTrajectorySync(mid1);
  255. drive.followTrajectorySync(mid2);
  256. drive.followTrajectorySync(mid3);
  257. drive.followTrajectorySync(mid4);
  258.  
  259. } else if (vals[1] == 0) {//left
  260. //turn and get first stone
  261. drive.followTrajectorySync(left1);
  262. drive.followTrajectorySync(left2);
  263. drive.followTrajectorySync(left3);
  264. drive.followTrajectorySync(left4);
  265.  
  266. } else if (vals[2] == 0) {
  267. drive.followTrajectorySync(right1);
  268. drive.followTrajectorySync(right2);
  269. drive.followTrajectorySync(right3);
  270. drive.followTrajectorySync(right4);
  271. } else {
  272. drive.followTrajectorySync(park);
  273. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!