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