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