001/** 002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868 003 * Open Source Software; you may modify and/or share it under the terms of 004 * the 3-Clause BSD License found in the root directory of this project. 005 */ 006 007package tagalong.commands.base; 008 009import tagalong.commands.TagalongCommand; 010import tagalong.measurements.Angle; 011import tagalong.subsystems.TagalongSubsystemBase; 012import tagalong.subsystems.micro.Pivot; 013import tagalong.subsystems.micro.augments.PivotAugment; 014 015/** 016 * Command that rotates pivot by a target angle based on specified 017 * parameters. 018 */ 019public class PivotXCmd<T extends TagalongSubsystemBase & PivotAugment> extends TagalongCommand { 020 /** 021 * Pivot microsystem 022 */ 023 private final Pivot _pivot; 024 /** 025 * Desired additional movement for the pivot relative to its current position 026 */ 027 private final double _relativeMovementRot; 028 /** 029 * Lower tolerance in rotations 030 */ 031 private final double _lowerToleranceRot; 032 /** 033 * Upper tolerance in rotations 034 */ 035 private final double _upperToleranceRot; 036 /** 037 * Whether or not to require pivot to be in tolerance 038 */ 039 private final boolean _requireInTolerance; 040 /** 041 * How long pivot must be in tolerance for in seconds 042 */ 043 private final double _requiredInToleranceDurationS; 044 045 /** 046 * Lower bound of the goal tolerance 047 */ 048 private double _lowerBoundRot; 049 /** 050 * Upper bound of the goal tolerance 051 */ 052 private double _upperBoundRot; 053 /** 054 * Whether or not the pivot should holds its position after the command ends 055 */ 056 private boolean _holdPositionAfter; 057 /** 058 * whether or not the pivot has started moving 059 */ 060 private boolean _startedMovement; 061 /** 062 *The maximum velocity of the pivot, in rotations per second, during this command 063 */ 064 private double _maxVelocityRPS; 065 /** 066 * Starting angle of the pivot when the command executes 067 */ 068 private double _startAngleRot; 069 /** 070 * Actual goal angle of the pivot when the command executes 071 */ 072 private double _goalAngleRot; 073 074 @Override 075 public void initialize() { 076 _startedMovement = false; 077 _startAngleRot = _pivot.getPivotPosition(); 078 _goalAngleRot = _pivot.clampPivotPosition(_startAngleRot + _relativeMovementRot); 079 _lowerBoundRot = _goalAngleRot - _lowerToleranceRot; 080 _upperBoundRot = _goalAngleRot + _upperToleranceRot; 081 } 082 083 @Override 084 public void execute() { 085 if (!_startedMovement) { 086 if (_pivot.isSafeToMove()) { 087 _pivot.setPivotProfile(_goalAngleRot, 0.0, _maxVelocityRPS); 088 _startedMovement = true; 089 _pivot.followLastProfile(); 090 } 091 } else { 092 _pivot.followLastProfile(); 093 } 094 } 095 096 @Override 097 public void end(boolean interrupted) { 098 _pivot.setFollowProfile(_holdPositionAfter); 099 } 100 101 @Override 102 public boolean isFinished() { 103 // Command is finished when the profile is finished AND 104 // Either the tolerance is bypassed or in tolerance for the desired duration 105 return _pivot.isProfileFinished() 106 && (!_requireInTolerance 107 || _pivot.checkToleranceTime( 108 _pivot.isPivotInTolerance(_lowerBoundRot, _upperBoundRot), 109 _requiredInToleranceDurationS 110 )); 111 } 112 113 /** 114 * Constructor that creates the command with the below parameters. 115 * 116 * @param pivot Tagalong Subsystem containing a pivot 117 * microsystem 118 * @param relativeMovementRot The target amount to move, in rotations 119 */ 120 public PivotXCmd(T pivot, Angle relativeMovementRot) { 121 this(pivot, relativeMovementRot, true); 122 } 123 124 /** 125 * Constructor that creates the command with the below parameters. 126 * 127 * @param id Tagalong Subsystem containing a pivot 128 * microsystem 129 * @param pivot Tagalong Subsystem containing a pivot 130 * microsystem 131 * @param relativeMovementRot The target amount to move, in rotations 132 */ 133 public PivotXCmd(int id, T pivot, Angle relativeMovementRot) { 134 this(id, pivot, relativeMovementRot, true); 135 } 136 137 /** 138 * Constructor that creates the command with the below parameters. 139 * 140 * @param pivot Tagalong Subsystem containing a pivot 141 * microsystem 142 * @param relativeMovementRot The target amount to move, in rotations 143 * @param holdPositionAfter whether or not the pivot must hold position after 144 * reaching target 145 */ 146 public PivotXCmd(T pivot, Angle relativeMovementRot, boolean holdPositionAfter) { 147 this(pivot, relativeMovementRot, holdPositionAfter, pivot.getPivot()._maxVelocityRPS); 148 } 149 150 /** 151 * Constructor that creates the command with the below parameters. 152 * 153 * @param id Tagalong Subsystem containing a pivot 154 * microsystem 155 * @param pivot Tagalong Subsystem containing a pivot 156 * microsystem 157 * @param relativeMovementRot The target amount to move, in rotations 158 * @param holdPositionAfter whether or not the pivot must hold position after 159 * reaching target 160 */ 161 public PivotXCmd(int id, T pivot, Angle relativeMovementRot, boolean holdPositionAfter) { 162 this(id, pivot, relativeMovementRot, holdPositionAfter, pivot.getPivot(id)._maxVelocityRPS); 163 } 164 165 /** 166 * Constructor that creates the command with the below parameters. 167 * 168 * @param pivot Tagalong Subsystem containing a pivot 169 * microsystem 170 * @param relativeMovementRot The target amount to move, in rotations 171 * @param holdPositionAfter whether or not the pivot must hold position after 172 * reaching target 173 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 174 * can move 175 */ 176 public PivotXCmd( 177 T pivot, Angle relativeMovementRot, boolean holdPositionAfter, double maxVelocityRPS 178 ) { 179 this( 180 pivot, 181 relativeMovementRot, 182 holdPositionAfter, 183 maxVelocityRPS, 184 pivot.getPivot()._defaultPivotLowerToleranceRot, 185 pivot.getPivot()._defaultPivotUpperToleranceRot 186 ); 187 } 188 189 /** 190 * Constructor that creates the command with the below parameters. 191 * 192 * @param id Tagalong Subsystem containing a pivot 193 * microsystem 194 * @param pivot Tagalong Subsystem containing a pivot 195 * microsystem 196 * @param relativeMovementRot The target amount to move, in rotations 197 * @param holdPositionAfter whether or not the pivot must hold position after 198 * reaching target 199 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 200 * can move 201 */ 202 public PivotXCmd( 203 int id, T pivot, Angle relativeMovementRot, boolean holdPositionAfter, double maxVelocityRPS 204 ) { 205 this( 206 id, 207 pivot, 208 relativeMovementRot, 209 holdPositionAfter, 210 maxVelocityRPS, 211 pivot.getPivot(id)._defaultPivotLowerToleranceRot, 212 pivot.getPivot(id)._defaultPivotUpperToleranceRot 213 ); 214 } 215 216 /** 217 * Constructor that creates the command with the below parameters. 218 * 219 * @param pivot Tagalong Subsystem containing a pivot 220 * microsystem 221 * @param relativeMovementRot The target amount to move, in rotations 222 * @param holdPositionAfter whether or not the pivot must hold position after 223 * reaching target 224 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 225 * can move 226 * @param toleranceM the desired tolerance for angle, in rotations 227 */ 228 public PivotXCmd( 229 T pivot, 230 Angle relativeMovementRot, 231 boolean holdPositionAfter, 232 double maxVelocityRPS, 233 double toleranceM 234 ) { 235 this(pivot, relativeMovementRot, holdPositionAfter, maxVelocityRPS, toleranceM, toleranceM); 236 } 237 238 /** 239 * Constructor that creates the command with the below parameters. 240 * 241 * @param id Tagalong Subsystem containing a pivot 242 * microsystem 243 * @param pivot Tagalong Subsystem containing a pivot 244 * microsystem 245 * @param relativeMovementRot The target amount to move, in rotations 246 * @param holdPositionAfter whether or not the pivot must hold position after 247 * reaching target 248 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 249 * can move 250 * @param toleranceM the desired tolerance for angle, in rotations 251 */ 252 public PivotXCmd( 253 int id, 254 T pivot, 255 Angle relativeMovementRot, 256 boolean holdPositionAfter, 257 double maxVelocityRPS, 258 double toleranceM 259 ) { 260 this(id, pivot, relativeMovementRot, holdPositionAfter, maxVelocityRPS, toleranceM, toleranceM); 261 } 262 263 /** 264 * Constructor that creates the command with the below parameters. 265 * 266 * @param pivot Tagalong Subsystem containing a pivot 267 * microsystem 268 * @param relativeMovementRot The target amount to move, in rotations 269 * @param holdPositionAfter whether or not the pivot must hold position after 270 * reaching target 271 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 272 * can move 273 * @param lowerToleranceM the lower bound for angle tolerance, in rotations 274 * @param upperToleranceM the upper bound for angle tolerance, in rotations 275 */ 276 public PivotXCmd( 277 T pivot, 278 Angle relativeMovementRot, 279 boolean holdPositionAfter, 280 double maxVelocityRPS, 281 double lowerToleranceM, 282 double upperToleranceM 283 ) { 284 this( 285 pivot, 286 relativeMovementRot, 287 holdPositionAfter, 288 maxVelocityRPS, 289 lowerToleranceM, 290 upperToleranceM, 291 -1.0 292 ); 293 } 294 295 /** 296 * Constructor that creates the command with the below parameters. 297 * 298 * @param id Tagalong Subsystem containing a pivot 299 * microsystem 300 * @param pivot Tagalong Subsystem containing a pivot 301 * microsystem 302 * @param relativeMovementRot The target amount to move, in rotations 303 * @param holdPositionAfter whether or not the pivot must hold position after 304 * reaching target 305 * @param maxVelocityRPS maximum velocity, in rotations per second, the pivot 306 * can move 307 * @param lowerToleranceM the lower bound for angle tolerance, in rotations 308 * @param upperToleranceM the upper bound for angle tolerance, in rotations 309 */ 310 public PivotXCmd( 311 int id, 312 T pivot, 313 Angle relativeMovementRot, 314 boolean holdPositionAfter, 315 double maxVelocityRPS, 316 double lowerToleranceM, 317 double upperToleranceM 318 ) { 319 this( 320 id, 321 pivot, 322 relativeMovementRot, 323 holdPositionAfter, 324 maxVelocityRPS, 325 lowerToleranceM, 326 upperToleranceM, 327 -1.0 328 ); 329 } 330 331 /** 332 * Constructor that creates the command with the below parameters. 333 * 334 * @param pivot Tagalong Subsystem containing a pivot 335 * microsystem 336 * @param relativeMovement the target amount to move 337 * @param holdPositionAfter whether or not the pivot must hold 338 * position after reaching target 339 * @param maxVelocityRPS maximum velocity, in rotations per second, 340 * the pivot can move 341 * @param lowerToleranceM the lower bound for angle tolerance, in 342 * rotations 343 * @param upperToleranceM the upper bound for angle tolerance, in 344 * rotations 345 * @param requiredInToleranceDurationS the duration (in seconds) the pivot 346 * must stay in 347 * tolerance 348 */ 349 public PivotXCmd( 350 T pivot, 351 Angle relativeMovement, 352 boolean holdPositionAfter, 353 double maxVelocityRPS, 354 double lowerToleranceM, 355 double upperToleranceM, 356 double requiredInToleranceDurationS 357 ) { 358 this( 359 pivot, 360 relativeMovement.getRotations(), 361 holdPositionAfter, 362 maxVelocityRPS, 363 lowerToleranceM, 364 upperToleranceM, 365 requiredInToleranceDurationS 366 ); 367 } 368 369 /** 370 * Constructor that creates the command with the below parameters. 371 * 372 * @param id Tagalong Subsystem containing a pivot 373 * microsystem 374 * @param pivot Tagalong Subsystem containing a pivot 375 * microsystem 376 * @param relativeMovement the target amount to move 377 * @param holdPositionAfter whether or not the pivot must hold 378 * position after reaching target 379 * @param maxVelocityRPS maximum velocity, in rotations per second, 380 * the pivot can move 381 * @param lowerToleranceM the lower bound for angle tolerance, in 382 * rotations 383 * @param upperToleranceM the upper bound for angle tolerance, in 384 * rotations 385 * @param requiredInToleranceDurationS the duration (in seconds) the pivot 386 * must stay in 387 * tolerance 388 */ 389 public PivotXCmd( 390 int id, 391 T pivot, 392 Angle relativeMovement, 393 boolean holdPositionAfter, 394 double maxVelocityRPS, 395 double lowerToleranceM, 396 double upperToleranceM, 397 double requiredInToleranceDurationS 398 ) { 399 this( 400 id, 401 pivot, 402 relativeMovement.getRotations(), 403 holdPositionAfter, 404 maxVelocityRPS, 405 lowerToleranceM, 406 upperToleranceM, 407 requiredInToleranceDurationS 408 ); 409 } 410 411 /** 412 * Full constructor, allows for double for goal in rare cases of advance use 413 * needing a direct way to interact 414 * 415 * @param pivot Tagalong Subsystem containing a pivot 416 * microsystem 417 * @param relativeMovementRot the target amount to move, in rotations 418 * @param holdPositionAfter whether or not the pivot must hold 419 * position after reaching target 420 * @param maxVelocityRPS maximum velocity, in rotations per second, 421 * the pivot can move 422 * @param lowerToleranceM the lower bound for angle tolerance, in 423 * rotations 424 * @param upperToleranceM the upper bound for angle tolerance, in 425 * rotations 426 * @param requiredInToleranceDurationS the duration (in seconds) the pivot 427 * must stay in 428 * tolerance 429 */ 430 public PivotXCmd( 431 T pivot, 432 double relativeMovementRot, 433 boolean holdPositionAfter, 434 double maxVelocityRPS, 435 double lowerToleranceM, 436 double upperToleranceM, 437 double requiredInToleranceDurationS 438 ) { 439 _pivot = pivot.getPivot(); 440 _relativeMovementRot = relativeMovementRot; 441 _holdPositionAfter = holdPositionAfter; 442 _lowerToleranceRot = Math.abs(lowerToleranceM); 443 _upperToleranceRot = Math.abs(upperToleranceM); 444 _maxVelocityRPS = maxVelocityRPS; 445 _requiredInToleranceDurationS = requiredInToleranceDurationS; 446 _requireInTolerance = _requiredInToleranceDurationS >= 0.0; 447 448 addRequirements(pivot); 449 } 450 451 /** 452 * Full constructor, allows for double for goal in rare cases of advance use 453 * needing a direct way to interact 454 * 455 * @param id Tagalong Subsystem containing a pivot 456 * microsystem 457 * @param pivot Tagalong Subsystem containing a pivot 458 * microsystem 459 * @param relativeMovementRot the target amount to move, in rotations 460 * @param holdPositionAfter whether or not the pivot must hold 461 * position after reaching target 462 * @param maxVelocityRPS maximum velocity, in rotations per second, 463 * the pivot can move 464 * @param lowerToleranceM the lower bound for angle tolerance, in 465 * rotations 466 * @param upperToleranceM the upper bound for angle tolerance, in 467 * rotations 468 * @param requiredInToleranceDurationS the duration (in seconds) the pivot 469 * must stay in 470 * tolerance 471 */ 472 public PivotXCmd( 473 int id, 474 T pivot, 475 double relativeMovementRot, 476 boolean holdPositionAfter, 477 double maxVelocityRPS, 478 double lowerToleranceM, 479 double upperToleranceM, 480 double requiredInToleranceDurationS 481 ) { 482 _pivot = pivot.getPivot(id); 483 _relativeMovementRot = relativeMovementRot; 484 _holdPositionAfter = holdPositionAfter; 485 _lowerToleranceRot = _goalAngleRot - Math.abs(lowerToleranceM); 486 _upperToleranceRot = _goalAngleRot + Math.abs(upperToleranceM); 487 _maxVelocityRPS = maxVelocityRPS; 488 _requiredInToleranceDurationS = requiredInToleranceDurationS; 489 _requireInTolerance = _requiredInToleranceDurationS >= 0.0; 490 491 addRequirements(pivot); 492 } 493}