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