-
Notifications
You must be signed in to change notification settings - Fork 0
/
Constants.java
163 lines (136 loc) · 7.62 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
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
// 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 java.lang.Math;
import com.revrobotics.CANSparkMax.IdleMode;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
/**
* 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 DriveConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds
public static final double kMaxSpeedMetersPerSecond = 4.8;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
// Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(21.5);
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = Units.inchesToMeters(21.5);
// Distance between front and back wheels on robot
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
// Angular offsets of the modules relative to the chassis in radians
public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
public static final double kFrontRightChassisAngularOffset = 0;
public static final double kBackLeftChassisAngularOffset = Math.PI;
public static final double kBackRightChassisAngularOffset = Math.PI / 2;
// SPARK MAX CAN IDs
public static final int kFrontLeftDrivingCanId = 3;
public static final int kFrontRightDrivingCanId = 5;
public static final int kRearLeftDrivingCanId = 7;
public static final int kRearRightDrivingCanId = 9;
public static final int kFrontLeftTurningCanId = 2;
public static final int kFrontRightTurningCanId = 4;
public static final int kRearLeftTurningCanId = 6;
public static final int kRearRightTurningCanId = 8;
public static final boolean kGyroReversed = true;
}
public static final class ModuleConstants {
// The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T.
// This changes the drive speed of the module (a pinion gear with more teeth will result in a
// robot that drives faster).
public static final int kDrivingMotorPinionTeeth = 13;
// Invert the turning encoder, since the output shaft rotates in the opposite direction of
// the steering motor in the MAXSwerve Module.
public static final boolean kTurningEncoderInverted = true;
// Calculations required for driving motor conversion factors and feed forward
public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
public static final double kWheelDiameterMeters = 0.0762;
public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
// 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
/ kDrivingMotorReduction;
public static final double kDrivingEncoderPositionFactor = (kWheelDiameterMeters * Math.PI)
/ kDrivingMotorReduction; // meters
public static final double kDrivingEncoderVelocityFactor = ((kWheelDiameterMeters * Math.PI)
/ kDrivingMotorReduction) / 60.0; // meters per second
public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians
public static final double kTurningEncoderVelocityFactor = (2 * Math.PI) / 60.0; // radians per second
public static final double kTurningEncoderPositionPIDMinInput = 0; // radians
public static final double kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor; // radians
public static final double kDrivingP = 0.04;
public static final double kDrivingI = 0;
public static final double kDrivingD = 0;
public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps;
public static final double kDrivingMinOutput = -1;
public static final double kDrivingMaxOutput = 1;
public static final double kTurningP = 1;
public static final double kTurningI = 0;
public static final double kTurningD = 0;
public static final double kTurningFF = 0;
public static final double kTurningMinOutput = -1;
public static final double kTurningMaxOutput = 1;
public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
public static final int kDrivingMotorCurrentLimit = 50; // amps
public static final int kTurningMotorCurrentLimit = 20; // amps
}
public static final class IntakeConstants {
public static final int intakeMotorID = 20;
public static final int intakeRoller1ID = 11;
public static final int intakeRoller2ID = 12;
public static final double kOpenSpeed = -0.2;
public static final double kCloseSpeed = 0.2;
public static final double kRollerSpeed = 0.25;
public static final int leftLimitSwitchChannel = 0;
public static final int middleLimitSwitchChannel = 1;
public static final int rightLimitSwitchChannel = 2;
public static final double kP = 2.5e-2;
public static final double kI = 1e-9;
public static final double kD = 0;
public static final double kMaxVelocityRadPerSecond = 2*Math.PI;
public static final double kMaxAccelerationRadPerSecSquared = Math.PI;
public static final double kEncoderDistancePerPulse = 0.1;
public static final double kArmOffsetRads = 0;
public static final double kSVolts = 0;
public static final double kGVolts = 0;
public static final double kVVoltSecondPerRad = 0;
public static final double kAVoltSecondSquaredPerRad = 0;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 1;
public static final double kPYController = 1;
public static final double kPThetaController = 1;
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
}
public static final class NeoMotorConstants {
public static final double kFreeSpeedRpm = 5676;
}
}