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 java.util.function.BooleanSupplier; 010import java.util.function.DoubleSupplier; 011import tagalong.commands.TagalongCommand; 012import tagalong.subsystems.TagalongSubsystemBase; 013import tagalong.subsystems.micro.Elevator; 014import tagalong.subsystems.micro.augments.ElevatorAugment; 015 016/** 017 * Command for the elevator to continuously move towards the goal supplier's target height 018 */ 019public class ElevateToDynamicCmd<T extends TagalongSubsystemBase & ElevatorAugment> 020 extends TagalongCommand { 021 /** 022 * Elevator microsystem 023 */ 024 private final Elevator _elevator; 025 /** 026 * Supplier for the goal position in meters 027 */ 028 private final DoubleSupplier _goalSupplierM; 029 /** 030 * Supplier for the condition to start the command 031 */ 032 private final BooleanSupplier _startCondition; 033 /** 034 * Whether or not the elevator should hold its position after the command ends 035 */ 036 private boolean _holdPositionAfter; 037 /** 038 * Maximum velocity of the elevator in meters per second 039 */ 040 private double _maxVelocityMPS; 041 /** 042 * Whether or not the elevator has started moving 043 */ 044 private boolean _startedMovement; 045 /** 046 * Goal height of the elevator in meters 047 */ 048 private double _goalPositionM; 049 050 @Override 051 public void initialize() { 052 _elevator.setHoldPosition(false); 053 _startedMovement = false; 054 } 055 056 @Override 057 public void execute() { 058 // if elevator has not started moving, check for legal states 059 _goalPositionM = _elevator.clampElevatorPosition(_goalSupplierM.getAsDouble()); 060 if (_startedMovement) { 061 _elevator.setElevatorProfile( 062 _goalPositionM, 0.0, _maxVelocityMPS, _elevator._maxAccelerationMPS2, false 063 ); 064 _elevator.followLastProfile(); 065 } else if (_startCondition.getAsBoolean()) { 066 _startedMovement = true; 067 _elevator.setElevatorProfile(_goalPositionM, 0.0, _maxVelocityMPS); 068 _elevator.followLastProfile(); 069 } 070 } 071 072 @Override 073 public void end(boolean interrupted) { 074 _elevator.setHoldPosition(_holdPositionAfter); 075 } 076 077 @Override 078 public boolean isFinished() { 079 return false; 080 } 081 082 /** 083 * Continuously move to the double suppliers target position. The function is 084 * continuous, it has no end condition, so the user must interrupt the command 085 * or decorate the command with an end condition. Useful for dynamic movements 086 * that are dependent on sensor readings, field conditions, or driver 087 * configurations. 088 * 089 * @param id Integer ID of the elevator microsystem inside the 090 * Tagalong Subsystem 091 * @param elevator Tagalong Subsystem containing an elevator 092 * microsystem 093 * @param goalSupplierM DoubleSupplier for the elevator height 094 * @param holdPositionAfter If the elevator should hold position when the 095 * command completes 096 * @param maxVelocityMPS Maximum velocity for the elevator during this command 097 * @param startSupplier BooleanSupplier condition for movement to start, 098 * defaults to elevator::isSafeToMove 099 */ 100 public ElevateToDynamicCmd( 101 int id, 102 T elevator, 103 DoubleSupplier goalSupplierM, 104 boolean holdPositionAfter, 105 double maxVelocityMPS, 106 BooleanSupplier startSupplier 107 ) { 108 _elevator = elevator.getElevator(id); 109 _goalSupplierM = goalSupplierM; 110 _holdPositionAfter = holdPositionAfter; 111 _maxVelocityMPS = maxVelocityMPS; 112 _startCondition = startSupplier; 113 114 addRequirements(elevator); 115 } 116 117 /** 118 * Continuously move to the double suppliers target position. The function is 119 * continuous, it has no end condition, so the user must interrupt the command 120 * or decorate the command with an end condition. Useful for dynamic movements 121 * that are dependent on sensor readings, field conditions, or driver 122 * configurations. 123 * 124 * @param elevator Tagalong Subsystem containing an elevator 125 * microsystem 126 * @param goalSupplierM DoubleSupplier for the elevator height 127 * @param holdPositionAfter If the elevator should hold position when the 128 * command completes 129 * @param maxVelocityMPS Maximum velocity for the elevator during this command 130 * @param startSupplier BooleanSupplier condition for movement to start, 131 * defaults to elevator::isSafeToMove 132 */ 133 public ElevateToDynamicCmd( 134 T elevator, 135 DoubleSupplier goalSupplierM, 136 boolean holdPositionAfter, 137 double maxVelocityMPS, 138 BooleanSupplier startSupplier 139 ) { 140 _elevator = elevator.getElevator(); 141 _goalSupplierM = goalSupplierM; 142 _holdPositionAfter = holdPositionAfter; 143 _maxVelocityMPS = maxVelocityMPS; 144 _startCondition = startSupplier; 145 146 addRequirements(elevator); 147 } 148 149 /** 150 * Continuously move to the double suppliers target position. The function is 151 * continuous, it has no end condition, so the user must interrupt the command 152 * or decorate the command with an end condition. Useful for dynamic movements 153 * that are dependent on sensor readings, field conditions, or driver 154 * configurations. 155 * 156 * @param id Integer ID of the elevator microsystem inside the 157 * Tagalong Subsystem 158 * @param elevator Tagalong Subsystem containing an elevator 159 * microsystem 160 * @param goalSupplierM DoubleSupplier for the elevator height 161 * @param holdPositionAfter If the elevator should hold position when the 162 * command completes 163 * @param maxVelocityMPS Maximum velocity for the elevator during this command 164 */ 165 public ElevateToDynamicCmd( 166 int id, 167 T elevator, 168 DoubleSupplier goalSupplierM, 169 boolean holdPositionAfter, 170 double maxVelocityMPS 171 ) { 172 this( 173 id, 174 elevator, 175 goalSupplierM, 176 holdPositionAfter, 177 maxVelocityMPS, 178 elevator.getElevator(id)::isSafeToMove 179 ); 180 } 181 182 /** 183 * Continuously move to the double suppliers target position. The function is 184 * continuous, it has no end condition, so the user must interrupt the command 185 * or decorate the command with an end condition. Useful for dynamic movements 186 * that are dependent on sensor readings, field conditions, or driver 187 * configurations. 188 * 189 * @param elevator Tagalong Subsystem containing an elevator 190 * microsystem 191 * @param goalSupplierM DoubleSupplier for the elevator height 192 * @param holdPositionAfter If the elevator should hold position when the 193 * command completes 194 * @param maxVelocityMPS Maximum velocity for the elevator during this command 195 */ 196 public ElevateToDynamicCmd( 197 T elevator, DoubleSupplier goalSupplierM, boolean holdPositionAfter, double maxVelocityMPS 198 ) { 199 this( 200 elevator, 201 goalSupplierM, 202 holdPositionAfter, 203 maxVelocityMPS, 204 elevator.getElevator()::isSafeToMove 205 ); 206 } 207 208 /** 209 * Continuously move to the double suppliers target position. The function is 210 * continuous, it has no end condition, so the user must interrupt the command 211 * or decorate the command with an end condition. Useful for dynamic movements 212 * that are dependent on sensor readings, field conditions, or driver 213 * configurations. 214 * 215 * @param id Integer ID of the elevator microsystem inside the 216 * Tagalong Subsystem 217 * @param elevator Tagalong Subsystem containing an elevator 218 * microsystem 219 * @param goalSupplierM DoubleSupplier for the elevator height 220 * @param holdPositionAfter If the elevator should hold position when the 221 * command completes 222 */ 223 public ElevateToDynamicCmd( 224 int id, T elevator, DoubleSupplier goalSupplierM, boolean holdPositionAfter 225 ) { 226 this(id, elevator, goalSupplierM, holdPositionAfter, elevator.getElevator(id)._maxVelocityMPS); 227 } 228 229 /** 230 * Continuously move to the double suppliers target position. The function is 231 * continuous, it has no end condition, so the user must interrupt the command 232 * or decorate the command with an end condition. Useful for dynamic movements 233 * that are dependent on sensor readings, field conditions, or driver 234 * configurations. 235 * 236 * @param elevator Tagalong Subsystem containing an elevator 237 * microsystem 238 * @param goalSupplierM DoubleSupplier for the elevator height 239 * @param holdPositionAfter If the elevator should hold position when the 240 * command completes 241 * command 242 */ 243 public ElevateToDynamicCmd(T elevator, DoubleSupplier goalSupplierM, boolean holdPositionAfter) { 244 this(elevator, goalSupplierM, holdPositionAfter, elevator.getElevator()._maxVelocityMPS); 245 } 246 247 /** 248 * Continuously move to the double suppliers target position. The function is 249 * continuous, it has no end condition, so the user must interrupt the command 250 * or decorate the command with an end condition. Useful for dynamic movements 251 * that are dependent on sensor readings, field conditions, or driver 252 * configurations. 253 * 254 * @param id Integer ID of the elevator microsystem inside the 255 * Tagalong Subsystem 256 * @param elevator Tagalong Subsystem containing an elevator microsystem 257 * @param goalSupplierM DoubleSupplier for the elevator height 258 */ 259 public ElevateToDynamicCmd(int id, T elevator, DoubleSupplier goalSupplierM) { 260 this(id, elevator, goalSupplierM, true); 261 } 262 263 /** 264 * Continuously move to the double suppliers target position. The function is 265 * continuous, it has no end condition, so the user must interrupt the command 266 * or decorate the command with an end condition. Useful for dynamic movements 267 * that are dependent on sensor readings, field conditions, or driver 268 * configurations. 269 * 270 * @param elevator Tagalong Subsystem containing an elevator microsystem 271 * @param goalSupplierM DoubleSupplier for the elevator height 272 */ 273 public ElevateToDynamicCmd(T elevator, DoubleSupplier goalSupplierM) { 274 this(elevator, goalSupplierM, true); 275 } 276}