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.aim; 008 009import edu.wpi.first.math.geometry.Translation2d; 010import java.util.function.DoubleSupplier; 011import java.util.function.Supplier; 012import tagalong.commands.base.RollToDynamicCmd; 013import tagalong.math.AlgebraicUtils; 014import tagalong.subsystems.TagalongSubsystemBase; 015import tagalong.subsystems.micro.augments.RollerAugment; 016 017/** 018 * Command that continuously points at a target in 2d space. 019 * For rotational targeting give robot position as (X, Y) pose 020 * For vertical targets give (distance from target, mechanism height) 021 */ 022public class RollerAimAtCmd<T extends TagalongSubsystemBase & RollerAugment> 023 extends RollToDynamicCmd<T> { 024 @SuppressWarnings("unused") 025 /** 026 * Target to aim at 027 */ 028 private final Translation2d _target; 029 @SuppressWarnings("unused") 030 /** 031 * Robot location supplier (x, y) 032 */ 033 private final Supplier<Translation2d> _locationSupplier; 034 035 /** 036 * Transform position into an optimal path angle to target, ignores yaw 037 * 038 * @param location Current robot location supplier 039 * @param rollerPosition Roller position supplier 040 * @param target Target to aim at's location 041 * @return Optimal path target angle for the roller 042 */ 043 protected static DoubleSupplier positionTransform( 044 Supplier<Translation2d> location, DoubleSupplier rollerPosition, Translation2d target 045 ) { 046 return () -> { 047 Translation2d curPos = location.get(); 048 double goalRot = Math.atan((target.getX() - curPos.getX()) / (target.getY() - curPos.getY())); 049 // Take the shortest path to that position 050 return AlgebraicUtils.placeInScopeRot(rollerPosition.getAsDouble(), goalRot); 051 }; 052 } 053 054 /** 055 * Minimal constructor with default parameters 056 * 057 * @param id Integer ID of the roller microsystem inside the 058 * Tagalong Subsystem 059 * @param roller Tagalong Subsystem containing a roller microsystem 060 * @param positionSupplier Supplies the current robot position position 061 * @param target Unmoving 2-dimensional target location 062 */ 063 public RollerAimAtCmd( 064 int id, T roller, Supplier<Translation2d> positionSupplier, Translation2d target 065 ) { 066 this(id, roller, positionSupplier, target, true); 067 } 068 069 /** 070 * Minimal constructor with default parameters 071 * 072 * @param roller Tagalong Subsystem containing a roller microsystem 073 * @param positionSupplier Supplies the current robot position position 074 * @param target Unmoving 2-dimensional target location 075 */ 076 public RollerAimAtCmd(T roller, Supplier<Translation2d> positionSupplier, Translation2d target) { 077 this(roller, positionSupplier, target, true); 078 } 079 080 /** 081 * Constructor that creates the command with the below parameters. 082 * 083 * @param id Integer ID of the roller microsystem inside the 084 * Tagalong Subsystem 085 * @param roller Tagalong Subsystem containing a roller microsystem 086 * @param positionSupplier Supplies the current robot position position 087 * @param target Unmoving 2-dimensional target location 088 * @param holdPositionAfter If the roller should hold position when the command completes 089 * @param maxVelocityRPS The maximum velocity of the roller, in rotations per second, during 090 * this command 091 */ 092 public RollerAimAtCmd( 093 int id, 094 T roller, 095 Supplier<Translation2d> positionSupplier, 096 Translation2d target, 097 boolean holdPositionAfter, 098 double maxVelocityRPS 099 ) { 100 super( 101 id, 102 roller, 103 positionTransform(positionSupplier, roller.getRoller(id)::getRollerPosition, target), 104 holdPositionAfter, 105 maxVelocityRPS 106 ); 107 _target = target; 108 _locationSupplier = positionSupplier; 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 positionSupplier Supplies the current robot position position 116 * @param target Unmoving 2-dimensional target location 117 * @param holdPositionAfter If the roller should hold position when the command completes 118 * @param maxVelocityRPS The maximum velocity of the roller, in rotations per second, during 119 * this command 120 */ 121 public RollerAimAtCmd( 122 T roller, 123 Supplier<Translation2d> positionSupplier, 124 Translation2d target, 125 boolean holdPositionAfter, 126 double maxVelocityRPS 127 ) { 128 super( 129 roller, 130 positionTransform(positionSupplier, roller.getRoller()::getRollerPosition, target), 131 holdPositionAfter, 132 maxVelocityRPS 133 ); 134 _target = target; 135 _locationSupplier = positionSupplier; 136 } 137 138 /** 139 * Constructor that creates the command with the below parameters. 140 * 141 * @param id Integer ID of the roller microsystem inside the 142 * Tagalong Subsystem 143 * @param roller Tagalong Subsystem containing a roller microsystem 144 * @param positionSupplier Supplies the current robot position position 145 * @param target Unmoving 2-dimensional target location 146 * @param holdPositionAfter If the roller should hold position when the command completes 147 */ 148 public RollerAimAtCmd( 149 int id, 150 T roller, 151 Supplier<Translation2d> positionSupplier, 152 Translation2d target, 153 boolean holdPositionAfter 154 ) { 155 this( 156 id, 157 roller, 158 positionSupplier, 159 target, 160 holdPositionAfter, 161 roller.getRoller(id)._maxVelocityRPS 162 ); 163 } 164 165 /** 166 * Constructor that creates the command with the below parameters. 167 * 168 * @param roller Tagalong Subsystem containing a roller microsystem 169 * @param positionSupplier Supplies the current robot position position 170 * @param target Unmoving 2-dimensional target location 171 * @param holdPositionAfter If the roller should hold position when the command completes 172 */ 173 public RollerAimAtCmd( 174 T roller, 175 Supplier<Translation2d> positionSupplier, 176 Translation2d target, 177 boolean holdPositionAfter 178 ) { 179 this(roller, positionSupplier, target, holdPositionAfter, roller.getRoller()._maxVelocityRPS); 180 } 181}