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.DoubleSupplier; 010import tagalong.commands.TagalongCommand; 011import tagalong.subsystems.TagalongSubsystemBase; 012import tagalong.subsystems.micro.Roller; 013import tagalong.subsystems.micro.augments.RollerAugment; 014 015/** 016 * Command for the roller to continuously move towards the goal supplier's target height 017 */ 018public class RollToDynamicCmd<T extends TagalongSubsystemBase & RollerAugment> 019 extends TagalongCommand { 020 /** 021 * Roller microsystem 022 */ 023 private final Roller _roller; 024 /** 025 * Supplier for the goal position in rotations 026 */ 027 private final DoubleSupplier _goalSupplierRot; 028 /** 029 * Whether or not the roller should holds its position after the command ends 030 */ 031 private boolean _holdPositionAfter; 032 /** 033 * Maximum velocity of the roller in rotations per second 034 */ 035 private double _maxVelocityRPS; 036 /** 037 * Goal position of the roller in rotations 038 */ 039 private double _goalPositionRot; 040 041 @Override 042 public void initialize() { 043 _roller.setHoldPosition(false); 044 _roller.resetToleranceTimer(); 045 _roller.setRollerProfile(_goalPositionRot, 0.0, _maxVelocityRPS); 046 } 047 048 @Override 049 public void execute() { 050 _goalPositionRot = _goalSupplierRot.getAsDouble(); 051 _roller.setRollerProfile( 052 _goalPositionRot, 0.0, _maxVelocityRPS, _roller._maxAccelerationRPS2, false 053 ); 054 _roller.followLastProfile(); 055 } 056 057 @Override 058 public void end(boolean interrupted) { 059 _roller.setHoldPosition(_holdPositionAfter); 060 } 061 062 @Override 063 public boolean isFinished() { 064 return false; 065 } 066 067 /** 068 * Continuously move to the double suppliers target position. The function is 069 * continuous, it has no end condition, so the user must interrupt the command 070 * or decorate the command with an end condition. Useful for dynamic movements 071 * that are dependent on sensor readings, field conditions, or driver 072 * configurations. 073 * 074 * @param id Integer ID of the roller microsystem inside the 075 * Tagalong Subsystem 076 * @param roller Tagalong Subsystem containing an roller 077 * microsystem 078 * @param goalSupplierRot DoubleSupplier for the roller height 079 * @param holdPositionAfter If the roller should hold position when the 080 * command completes 081 * @param maxVelocityRPS Maximum velocity for the roller during this command 082 */ 083 public RollToDynamicCmd( 084 int id, 085 T roller, 086 DoubleSupplier goalSupplierRot, 087 boolean holdPositionAfter, 088 double maxVelocityRPS 089 ) { 090 _roller = roller.getRoller(id); 091 _goalSupplierRot = goalSupplierRot; 092 _holdPositionAfter = holdPositionAfter; 093 _maxVelocityRPS = maxVelocityRPS; 094 095 addRequirements(roller); 096 } 097 098 /** 099 * Continuously move to the double suppliers target position. The function is 100 * continuous, it has no end condition, so the user must interrupt the command 101 * or decorate the command with an end condition. Useful for dynamic movements 102 * that are dependent on sensor readings, field conditions, or driver 103 * configurations. 104 * 105 * @param roller Tagalong Subsystem containing an roller 106 * microsystem 107 * @param goalSupplierRot DoubleSupplier for the roller height 108 * @param holdPositionAfter If the roller should hold position when the 109 * command completes 110 * @param maxVelocityRPS Maximum velocity for the roller during this command 111 */ 112 public RollToDynamicCmd( 113 T roller, DoubleSupplier goalSupplierRot, boolean holdPositionAfter, double maxVelocityRPS 114 ) { 115 _roller = roller.getRoller(); 116 _goalSupplierRot = goalSupplierRot; 117 _holdPositionAfter = holdPositionAfter; 118 _maxVelocityRPS = maxVelocityRPS; 119 120 addRequirements(roller); 121 } 122 123 /** 124 * Continuously move to the double suppliers target position. The function is 125 * continuous, it has no end condition, so the user must interrupt the command 126 * or decorate the command with an end condition. Useful for dynamic movements 127 * that are dependent on sensor readings, field conditions, or driver 128 * configurations. 129 * 130 * @param id Integer ID of the roller microsystem inside the 131 * Tagalong Subsystem 132 * @param roller Tagalong Subsystem containing an roller 133 * microsystem 134 * @param goalSupplierRot DoubleSupplier for the roller height 135 * @param holdPositionAfter If the roller should hold position when the 136 * command completes 137 */ 138 public RollToDynamicCmd( 139 int id, T roller, DoubleSupplier goalSupplierRot, boolean holdPositionAfter 140 ) { 141 this(id, roller, goalSupplierRot, holdPositionAfter, roller.getRoller(id)._maxVelocityRPS); 142 } 143 144 /** 145 * Continuously move to the double suppliers target position. The function is 146 * continuous, it has no end condition, so the user must interrupt the command 147 * or decorate the command with an end condition. Useful for dynamic movements 148 * that are dependent on sensor readings, field conditions, or driver 149 * configurations. 150 * 151 * @param roller Tagalong Subsystem containing an roller 152 * microsystem 153 * @param goalSupplierRot DoubleSupplier for the roller height 154 * @param holdPositionAfter If the roller should hold position when the 155 * command completes 156 * command 157 */ 158 public RollToDynamicCmd(T roller, DoubleSupplier goalSupplierRot, boolean holdPositionAfter) { 159 this(roller, goalSupplierRot, holdPositionAfter, roller.getRoller()._maxVelocityRPS); 160 } 161 162 /** 163 * Continuously move to the double suppliers target position. The function is 164 * continuous, it has no end condition, so the user must interrupt the command 165 * or decorate the command with an end condition. Useful for dynamic movements 166 * that are dependent on sensor readings, field conditions, or driver 167 * configurations. 168 * 169 * @param id Integer ID of the roller microsystem inside the 170 * Tagalong Subsystem 171 * @param roller Tagalong Subsystem containing an roller microsystem 172 * @param goalSupplierRot DoubleSupplier for the roller height 173 */ 174 public RollToDynamicCmd(int id, T roller, DoubleSupplier goalSupplierRot) { 175 this(id, roller, goalSupplierRot, true); 176 } 177 178 /** 179 * Continuously move to the double suppliers target position. The function is 180 * continuous, it has no end condition, so the user must interrupt the command 181 * or decorate the command with an end condition. Useful for dynamic movements 182 * that are dependent on sensor readings, field conditions, or driver 183 * configurations. 184 * 185 * @param roller Tagalong Subsystem containing an roller microsystem 186 * @param goalSupplierRot DoubleSupplier for the roller height 187 */ 188 public RollToDynamicCmd(T roller, DoubleSupplier goalSupplierRot) { 189 this(roller, goalSupplierRot, true); 190 } 191}