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