-
Notifications
You must be signed in to change notification settings - Fork 0
/
Constants.java
117 lines (92 loc) · 3.73 KB
/
Constants.java
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
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class ConversionConstants {
public static final double
GEAR_RATIO = 8.14 * 1.00415081 * Math.PI/2,
CTRE_TICKS = 2048,
CTRE_TICKS_PER_REV = CTRE_TICKS * GEAR_RATIO, // 26295
WHEEL_DIAMETER = 1.975*2, //inches
CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI,
INCHES_PER_TICK = CIRCUMFERENCE / CTRE_TICKS_PER_REV,
IPDS_TO_MPH = 0.568,
IPDS_TO_MEPS = 0.254,
METERS_PER_TICK = INCHES_PER_TICK / 37.3701,
CTRE_NATIVE_TO_MPH = INCHES_PER_TICK * IPDS_TO_MPH,
CTRE_NATIVE_TO_MPS = INCHES_PER_TICK * IPDS_TO_MEPS;
}
public static final class DriveConstants {
public static final int
FRONTRIGHT_PORT_DRIVE = 3,
FRONTRIGHT_PORT_ROTATE = 4,
FRONTRIGHT_PORT_CANCODER = 10,
FRONTRIGHT_OFFSET = 135,
FRONTLEFT_PORT_DRIVE = 1,
FRONTLEFT_PORT_ROTATE = 2,
FRONTLEFT_PORT_CANCODER = 9,
FRONTLEFT_OFFSET = 45,
BACKRIGHT_PORT_DRIVE = 5,
BACKRIGHT_PORT_ROTATE = 6,
BACKRIGHT_PORT_CANCODER = 11,
BACKRIGHT_OFFSET = -10,
BACKLEFT_PORT_DRIVE = 7,
BACKLEFT_PORT_ROTATE = 8,
BACKLEFT_PORT_CANCODER = 12,
BACKLEFT_OFFSET = -45;
public static final int
GYRO_PORT = 20;
public static final double
TRACKWIDTH_METERS = 0.375;
public final static SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(
new Translation2d(DriveConstants.TRACKWIDTH_METERS/2, DriveConstants.TRACKWIDTH_METERS/2), //frontleft
new Translation2d(DriveConstants.TRACKWIDTH_METERS/2, -DriveConstants.TRACKWIDTH_METERS/2), //frontright
new Translation2d(-DriveConstants.TRACKWIDTH_METERS/2, DriveConstants.TRACKWIDTH_METERS/2), //backleft
new Translation2d(-DriveConstants.TRACKWIDTH_METERS/2, -DriveConstants.TRACKWIDTH_METERS/2)); //backright
// Drive limiters
public static double
LIMIT_VX = 1.5,
LIMIT_VY = 1.5,
LIMIT_ROT = 4.0;
}
public static final class OIConstants {
public static final int
DRIVER_PORT = 0,
INTAKE_PORT = 1;
}
public static final class AutoConstants {
public static final double
MAX_SPEED_MPS = 3.0,
MAX_ACCELERATION_MPS = 2.0;
public static final Gains
THETA_CONTROLLER = new Gains(1, 0, 0, 0, 0, 1),
X_CONTROLLER = new Gains(1, 0, 0, 0, 0, 1),
Y_CONTROLLER = new Gains(1, 0, 0, 0, 0, 1);
public static final TrapezoidProfile.Constraints THETA_CONTROLLER_CONSTRAINTS =
new TrapezoidProfile.Constraints(
MAX_SPEED_MPS, MAX_ACCELERATION_MPS);
}
public static final class IntakeConstants {
}
public static final class PIDConstants {
public static final int
SLOT_IDX = 0,
PID_LOOP_IDX = 0,
TIMEOUT_MS = 30;
public static final Gains
DRIVE_GAINS_VELOCITY = new Gains(0.198, 0.00085, 4.0, 0, 0, 0.8),
DRIVE_GAINS_POSITION = new Gains(0.05, 0.00001, 0, 0, 0, 0.6);
}
}