-
Notifications
You must be signed in to change notification settings - Fork 11
/
Drivetrain.java
91 lines (77 loc) · 2.35 KB
/
Drivetrain.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
package com.spartronics4915.atlas.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.spartronics4915.atlas.RobotMap;
import com.spartronics4915.atlas.commands.StopCommand;
import com.spartronics4915.atlas.subsystems.SpartronicsSubsystem;
import com.spartronics4915.util.Rotation2d;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
/**
* The subsystem that controls the Drivetrain.
*
* A note on motor naming:
* We're doing port and starboard again. That's all that really matters here.s
* These directions are relative to the front of the robot, which removes as
* much possible ambiguity as you can with directions (in this context).
* Port and starboard refer respectively to left and right, relative to the
* front of the robot.
*/
public class Drivetrain extends SpartronicsSubsystem
{
private static Drivetrain sInstance = null;
// Motors
public SpeedController mLeftMotor;
public SpeedController mRightMotor;
//IMU
public PigeonIMU mIMU;
public static Drivetrain getInstance() {
if (sInstance == null)
{
sInstance = new Drivetrain();
}
return sInstance;
}
private Drivetrain()
{
try
{
// Initialize motors
mLeftMotor = new Victor(RobotMap.kLeftDriveMotorId);
mRightMotor = new Victor(RobotMap.kRightDriveMotorId);
//Initialize IMU
mIMU = new PigeonIMU(RobotMap.kDriveTrainIMUID);
// This needs to go at the end. We *don't* set
// mInitalized here (we only set it on faliure).
logInitialized(true);
}
catch (Exception e)
{
logException("Couldn't initialize Drivetrain", e);
logInitialized(false);
}
}
@Override
public void initDefaultCommand()
{
if (isInitialized())
{
setDefaultCommand(new StopCommand());
}
}
public void stop()
{
mLeftMotor.set(0);
mRightMotor.set(0);
}
public Rotation2d getIMUHeading()
{
double[] ypr = new double[3];
mIMU.getYawPitchRoll(ypr);
return Rotation2d.fromDegrees(ypr[0]);
}
public void driveOpenLoop(double left, double right)
{
mLeftMotor.set(left);
mRightMotor.set(right);
}
}