-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathGameConstants.java
More file actions
163 lines (142 loc) · 7.67 KB
/
GameConstants.java
File metadata and controls
163 lines (142 loc) · 7.67 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
package frc.robot.constants;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
public class GameConstants {
public enum Mode {
/**
* Running on a real robot.
*/
REAL,
/**
* Running a physics simulator.
*/
SIM,
/**
* Replaying from a log file.
*/
REPLAY
}
public static final double DEADBAND = 0.1;
// Mode
public static final Mode simMode = Mode.SIM;
// public static final Mode simMode = Mode.REPLAY;
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
// Logging
public static final long MAX_LOG_TIME_WAIT = 10;
public static final boolean ENABLE_LOGGING = true;
//Debugs
public static final boolean DEBUG = true;
public static final boolean ARM_DEBUG = true;
//Joystick
public static final int DRIVE_JOYSTICK_PORT = 0;
public static final int STEER_JOYSTICK_PORT = 1;
public static final int XBOX_CONTROLLER_PORT = 2;
//Speeds
public static final double ROLLER_SPEED = 0.25;
public static final double TILT_SPEED = -0.5; // Arm motor is inverted - use negative speed
public static final double INTAKE_SPEED = -0.5;
public static final double HOPPER_SPEED = 0.35;//Want to increase this later
public static final double CLIMBER_SPEED_UP = 0.1;
public static final double CLIMBER_SPEED_DOWN = -0.1;
public static final double FEEDER_SPEED = 0.5;//Might make veolcity PID
public static final double MAX_SPEED = Units.feetToMeters(14.5);
public static final double SHOOTER_SPEED = 100;
public static final double INTAKE_DEPLOYER_SPEED = -0.075;
public static final double INTAKE_RETRACTION_SPEED = 0.1;
public static final double INITIAL_INTAKE_DEPLOYMENT_SPEED = -0.1;
public static final double INITIAL_INTAKE_RETRACTION_SPEED = 0.25;
//Diags
public static final double HOPPER_DIAGS_ENCODER = 1;
public static final double INTAKE_ROLLER_DIAGS_ENCODER = 1;
public static final double FEEDER_DIAGS_ENCODER = 1;
public static final double CLIMBER_DIAGS_ENCODER = 1;
public static final double SHOOTER_DIAGS_ENCODER = 1;
public static final double GYRO_DIAGS_ANGLE = 30;
public static final double TURRET_DIAGS_ENCODER = 1;
public static final double INTAKE_DEPLOYER_DIAGS_ENCODER = 1;
public static final double ANGLER_DIAGS_ENCODER = 1;
//Timeouts
public static final double SPIN_TIMEOUT = 5;
public static final double TILT_TIMEOUT = 5;
public static final double HOPPER_TIMEOUT = 60;
public static final double CLIMBER_TIMEOUT = 10;
public static final double FEEDER_TIMEOUT = 60;
public static final double ANGLER_TIMEOUT = 60;
public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000;
public static final double SHOOTER_TIMEOUT = 5;
public static final double INTAKE_DEPLOYER_BURNOUT_TIMER = 2;
public static final double TURRET_TIMEOUT = 5;
//Angles
public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45);
public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90);
public static final Rotation2d ANGLER_MIN_ANGLE = Rotation2d.fromDegrees(45);
public static final Rotation2d ANGLER_MAX_ANGLE = Rotation2d.fromDegrees(90);
public static final Rotation2d TURRET_MIN_ANGLE = Rotation2d.fromDegrees(-90);
public static final Rotation2d TURRET_MAX_ANGLE = Rotation2d.fromDegrees(90);
public static final double TILT_LENGTH = 0.2;
public static final double TILT_INERTIA = 0.5;
public static final double TILT_GEARING = 45.0;
public static final boolean TILT_SIMULATE_GRAVITY = false;
public static final double ANGLER_LENGTH = 0.2;
public static final double ANGLER_INERTIA = 0.5;
public static final double ANGLER_GEARING = 45.0;
public static final boolean ANGLER_SIMULATE_GRAVITY = false;
public static final int NEO_CURRENT_LIMIT = 20;
public static final double TURRET_LENGTH = 0.4;
public static final double TURRET_INERTIA = 0.5;
public static final double TURRET_GEARING = 45.0;
// angler (turret) PID
public static final double ANGLER_P = 0.7;
public static final double ANGLER_I = 0.000001;
public static final double ANGLER_D = 0.0;
public static final double ANGLER_FF = 0.0;
public static final double ANGLER_HOME_ROTATIONS = 0.0;
public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler
public static final double ANGLER_ENCODER_HIGH = 100; //Highest encoder position of Angler
public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler
public static final double ANGLER_ANGLE_HIGH = 45; //Highest angle position of Angler
public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState
public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState
public static final double ANGLER_LIMIT_SPEED = 0.2;
// turret (pan angle) PID
public static final double TURRET_P = .025;
public static final double TURRET_I = 0.00000;
public static final double TURRET_D = 0.0;
public static final double TURRET_FF = 0.0;
public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret
public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret
public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward
public static final double TURRET_LEFT_ANGLE = 95;
public static final double TURRET_RIGHT_ANGLE = -95;
public static final double TURRET_LIMIT_SPEED = 0.1;
//swerve config
public static final TelemetryVerbosity TELEMENTRY_VERBOSITY = TelemetryVerbosity.HIGH;
public static final boolean SET_HEADING_CORRECTION = false;
public static final boolean COSIN_COMPENSATOR = false;
public static final boolean USE_ANGULAR_VELOCITY_COMPENSATION_IN_TELEOP = true;
public static final boolean USE_ANGULAR_VELOCITY_COMPENSATION_IN_AUTO = true;
public static final double ANGULAR_VELOCITY_COEFFICENT = 0.1;
public static final boolean SET_MODULE_ENCODER_AUTO_SYNCHRONIZE = false;
public static final double SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND = 1;
// turret pan angle and launch angle calculations constants
public static final double GRAVITY = 9.81;
public static final double HUB_HEIGHT = 1.83;
public static final double SHOOTER_HEIGHT = 0.5;
public static final double BLUE_HUB_X_POSITION = 4.6256;
public static final double BLUE_HUB_Y_POSITION = 4.0345;
public static final double RED_HUB_X_POSITION = 11.9154;
public static final double RED_HUB_Y_POSITION = 4.0345;
public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
// Shift timings
public static final int SHIFT_1_START = 130;
public static final int SHIFT_2_START = 105;
public static final int SHIFT_3_START = 80;
public static final int SHIFT_4_START = 55;
public static final int ENDGAME_START = 30;
public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed)
public static final boolean ENABLE_VISION = true;
public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back)
}