Skip to content

Commit 8d10f35

Browse files
Tejshah88Tejshah88
committed
Mechanism position grouping
1 parent 49290ab commit 8d10f35

File tree

1 file changed

+131
-1
lines changed

1 file changed

+131
-1
lines changed

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 131 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@
44

55
package frc.robot;
66

7+
import static edu.wpi.first.units.Units.Degrees;
8+
import static edu.wpi.first.units.Units.Inches;
79
import static edu.wpi.first.units.Units.Kilograms;
810

911
import java.util.Optional;
@@ -32,6 +34,7 @@
3234
import edu.wpi.first.math.geometry.Translation2d;
3335
import edu.wpi.first.math.system.plant.DCMotor;
3436
import edu.wpi.first.math.trajectory.TrapezoidProfile;
37+
import edu.wpi.first.units.Units;
3538
import edu.wpi.first.units.measure.Angle;
3639
import edu.wpi.first.units.measure.AngularVelocity;
3740
import edu.wpi.first.units.measure.Current;
@@ -40,11 +43,119 @@
4043
import edu.wpi.first.units.measure.Mass;
4144
import edu.wpi.first.units.measure.Time;
4245
import edu.wpi.first.units.measure.Voltage;
43-
import edu.wpi.first.units.Units;
4446
import edu.wpi.first.wpilibj.DriverStation;
4547
import edu.wpi.first.wpilibj.DriverStation.Alliance;
48+
import frc.robot.Constants.MechanismPositionGroup;
4649

4750
public final class Constants {
51+
52+
public static class MechanismPositionGroup {
53+
public Angle wristAngle;
54+
public Distance liftHeight;
55+
public Angle pivotAngle;
56+
}
57+
58+
public static class constMechanismPositions {
59+
public static final MechanismPositionGroup CLEAN_HIGH = new MechanismPositionGroup();
60+
public static final MechanismPositionGroup CLEAN_LOW = new MechanismPositionGroup();
61+
public static final MechanismPositionGroup INTAKE_CORAL_GROUND = new MechanismPositionGroup();
62+
public static final MechanismPositionGroup INTAKE_CORAL_STATION = new MechanismPositionGroup();
63+
public static final MechanismPositionGroup INTAKE_ALGAE_GROUND = new MechanismPositionGroup();
64+
public static final MechanismPositionGroup PREP_CORAL_L1 = new MechanismPositionGroup();
65+
public static final MechanismPositionGroup PREP_CORAL_L2 = new MechanismPositionGroup();
66+
public static final MechanismPositionGroup PREP_CORAL_L3 = new MechanismPositionGroup();
67+
public static final MechanismPositionGroup PREP_CORAL_L4 = new MechanismPositionGroup();
68+
public static final MechanismPositionGroup INTAKE_ALGAE_STATION = new MechanismPositionGroup();
69+
public static final MechanismPositionGroup PREP_CORAL_ZERO_WITH_ALGAE = new MechanismPositionGroup();
70+
public static final MechanismPositionGroup PREP_ALGAE_NET = new MechanismPositionGroup();
71+
public static final MechanismPositionGroup PREP_CORAL_ZERO = new MechanismPositionGroup();
72+
public static final MechanismPositionGroup PREP_ALGAE_NET_WITH_CORAL = new MechanismPositionGroup();
73+
public static final MechanismPositionGroup INTAKE_CORAL_WITH_ALGAE_GROUND = new MechanismPositionGroup();
74+
public static final MechanismPositionGroup INTAKE_CORAL_WITH_ALGAE_STATION = new MechanismPositionGroup();
75+
public static final MechanismPositionGroup INTAKE_ALGAE_WITH_CORAL_GROUND = new MechanismPositionGroup();
76+
public static final MechanismPositionGroup CLEAN_HIGH_WITH_CORAL = new MechanismPositionGroup();
77+
public static final MechanismPositionGroup CLEAN_LOW_WITH_CORAL = new MechanismPositionGroup();
78+
static {
79+
80+
CLEAN_LOW.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
81+
CLEAN_LOW.liftHeight = Inches.of(0); // TODO: Replace with actual height
82+
CLEAN_LOW.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
83+
84+
CLEAN_HIGH.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
85+
CLEAN_HIGH.liftHeight = Inches.of(0); // TODO: Replace with actual height
86+
CLEAN_HIGH.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
87+
88+
INTAKE_CORAL_GROUND.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
89+
INTAKE_CORAL_GROUND.liftHeight = Inches.of(0); // TODO: Replace with actual height
90+
INTAKE_CORAL_GROUND.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
91+
92+
INTAKE_CORAL_STATION.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
93+
INTAKE_CORAL_STATION.liftHeight = Inches.of(0); // TODO: Replace with actual height
94+
INTAKE_CORAL_STATION.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
95+
96+
INTAKE_ALGAE_GROUND.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
97+
INTAKE_ALGAE_GROUND.liftHeight = Inches.of(0); // TODO: Replace with actual height
98+
INTAKE_ALGAE_GROUND.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
99+
100+
PREP_CORAL_L1.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
101+
PREP_CORAL_L1.liftHeight = Inches.of(0); // TODO: Replace with actual height
102+
PREP_CORAL_L1.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
103+
104+
PREP_CORAL_L2.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
105+
PREP_CORAL_L2.liftHeight = Inches.of(0); // TODO: Replace with actual height
106+
PREP_CORAL_L2.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
107+
108+
PREP_CORAL_L3.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
109+
PREP_CORAL_L3.liftHeight = Inches.of(0); // TODO: Replace with actual height
110+
PREP_CORAL_L3.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
111+
112+
PREP_CORAL_L4.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
113+
PREP_CORAL_L4.liftHeight = Inches.of(0); // TODO: Replace with actual height
114+
PREP_CORAL_L4.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
115+
116+
INTAKE_ALGAE_STATION.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
117+
INTAKE_ALGAE_STATION.liftHeight = Inches.of(0); // TODO: Replace with actual height
118+
INTAKE_ALGAE_STATION.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
119+
120+
PREP_CORAL_ZERO_WITH_ALGAE.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
121+
PREP_CORAL_ZERO_WITH_ALGAE.liftHeight = Inches.of(0); // TODO: Replace with actual height
122+
PREP_CORAL_ZERO_WITH_ALGAE.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
123+
124+
PREP_ALGAE_NET.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
125+
PREP_ALGAE_NET.liftHeight = Inches.of(0); // TODO: Replace with actual height
126+
PREP_ALGAE_NET.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
127+
128+
PREP_CORAL_ZERO.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
129+
PREP_CORAL_ZERO.liftHeight = Inches.of(0); // TODO: Replace with actual height
130+
PREP_CORAL_ZERO.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
131+
132+
PREP_ALGAE_NET_WITH_CORAL.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
133+
PREP_ALGAE_NET_WITH_CORAL.liftHeight = Inches.of(0); // TODO: Replace with actual height
134+
PREP_ALGAE_NET_WITH_CORAL.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
135+
136+
INTAKE_CORAL_WITH_ALGAE_GROUND.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
137+
INTAKE_CORAL_WITH_ALGAE_GROUND.liftHeight = Inches.of(0); // TODO: Replace with actual height
138+
INTAKE_CORAL_WITH_ALGAE_GROUND.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
139+
140+
INTAKE_CORAL_WITH_ALGAE_STATION.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
141+
INTAKE_CORAL_WITH_ALGAE_STATION.liftHeight = Inches.of(0); // TODO: Replace with actual height
142+
INTAKE_CORAL_WITH_ALGAE_STATION.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
143+
144+
INTAKE_ALGAE_WITH_CORAL_GROUND.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
145+
INTAKE_ALGAE_WITH_CORAL_GROUND.liftHeight = Inches.of(0); // TODO: Replace with actual height
146+
INTAKE_ALGAE_WITH_CORAL_GROUND.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
147+
148+
CLEAN_HIGH_WITH_CORAL.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
149+
CLEAN_HIGH_WITH_CORAL.liftHeight = Inches.of(0); // TODO: Replace with actual height
150+
CLEAN_HIGH_WITH_CORAL.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
151+
152+
CLEAN_LOW_WITH_CORAL.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
153+
CLEAN_LOW_WITH_CORAL.liftHeight = Inches.of(0); // TODO: Replace with actual height
154+
CLEAN_LOW_WITH_CORAL.pivotAngle = Degrees.of(0); // TODO: Replace with actual angle
155+
156+
}
157+
}
158+
48159
/**
49160
* Volts
50161
*/
@@ -456,4 +567,23 @@ public static class LIMELIGHT_LEFT {
456567

457568
}
458569
}
570+
571+
public static class constRobotPoses {
572+
public static final Distance ROOT_X = Units.Inches.of(4.5); // TODO: Replace with actual measurement
573+
public static final Distance ROOT_Y = Units.Inches.of(10); // TODO: Replace with actual measurement
574+
575+
public static final Distance ELEVATOR_PIVOT_LENGTH = Units.Inches.of(10);// TODO: Replace with actual measurement
576+
public static final Angle ELEVATOR_PIVOT_DEFAULT_ANGLE = Units.Degrees.of(0); // TODO: Replace with actual
577+
// measurement
578+
public static final Distance ELEVATOR_PIVOT_WIDTH = Units.Inches.of(4.3); // TODO: Replace with actual measurement
579+
580+
public static final Distance ELEVATOR_LENGTH = Units.Inches.of(1000000000); // TODO: Replace with actual measurement
581+
public static final Angle ELEVATOR_DEFAULT_ANGLE = Units.Degrees.of(0); // TODO: Replace with actual measurement
582+
public static final Distance ELEVATOR_WIDTH = Units.Inches.of(6000000.3); // TODO: Replace with actual measurement
583+
584+
public static final Distance INTAKE_WRIST_LENGTH = Units.Inches.of(10); // TODO: Replace with actual measurement
585+
public static final Angle INTAKE_WRIST_DEFAULT_ANGLE = Units.Degrees.of(0); // TODO: Replace with actual
586+
// measurement
587+
public static final Distance INTAKE_WRIST_WIDTH = Units.Inches.of(4.3); // TODO: Replace with actual measurement
588+
}
459589
}

0 commit comments

Comments
 (0)