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