-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathDriveTrain.java
162 lines (112 loc) · 5.3 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
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
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc.team4206.robot.subsystems;
import org.usfirst.frc.team4206.robot.Robot;
import org.usfirst.frc.team4206.robot.commands.PlayerDrive;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
* An example subsystem. You can replace me with your own Subsystem.
*/
@SuppressWarnings("unused")
public class DriveTrain extends Subsystem {
//Declaring Talons + DiffDrive
static WPI_TalonSRX leftDriveMaster;
static WPI_TalonSRX leftDriveSlave;
static WPI_TalonSRX rightDriveMaster;
static WPI_TalonSRX rightDriveSlave;
static DifferentialDrive drive;
double speed;
double rotation;
int armPIDSlot;
int armPIDLoop;
int armMMTimeout;
public DriveTrain() {
/* ----------BASIC DRIVETRAIN STUFF ----------*/
leftDriveMaster = new WPI_TalonSRX(2);
leftDriveSlave = new WPI_TalonSRX(3);
rightDriveMaster = new WPI_TalonSRX(4);
rightDriveSlave = new WPI_TalonSRX(5);
leftDriveSlave.follow(leftDriveMaster);
rightDriveSlave.follow(rightDriveMaster);
armPIDSlot = 0;
armPIDLoop = 0;
armMMTimeout = 10;
drive = new DifferentialDrive(leftDriveMaster, rightDriveMaster);
leftDriveMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
leftDriveMaster.setSensorPhase(true);
rightDriveMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
rightDriveMaster.setSensorPhase(true);
/* ---------- MOTION PROFILING ---------- */
leftDriveMaster.setInverted(false);
rightDriveMaster.setInverted(false);
/* Set relevant frame periods to be at least as fast as periodic rate */
leftDriveMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, armMMTimeout);
leftDriveMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, armMMTimeout);
/* set the peak and nominal outputs */
leftDriveMaster.configNominalOutputForward(0, armMMTimeout);
leftDriveMaster.configNominalOutputReverse(0, armMMTimeout);
leftDriveMaster.configPeakOutputForward(0.75, armMMTimeout);
leftDriveMaster.configPeakOutputReverse(-0.75, armMMTimeout);
/* set closed loop gains in slot0 - see documentation */
leftDriveMaster.selectProfileSlot(armPIDSlot, armPIDLoop);
leftDriveMaster.config_kF((int) .07, 0.2, armMMTimeout);
leftDriveMaster.config_kP(2, 0.2, armMMTimeout);
leftDriveMaster.config_kI(0, 0, armMMTimeout);
leftDriveMaster.config_kD(0, 0, armMMTimeout);
/* set acceleration and vcruise velocity - see documentation */
leftDriveMaster.configMotionCruiseVelocity(70, armMMTimeout);
leftDriveMaster.configMotionAcceleration(50, armMMTimeout);
leftDriveMaster.setSensorPhase(true);
leftDriveMaster.setInverted(false);
/* Set relevant frame periods to be at least as fast as periodic rate */
rightDriveMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, armMMTimeout);
rightDriveMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, armMMTimeout);
/* set the peak and nominal outputs */
rightDriveMaster.configNominalOutputForward(0, armMMTimeout);
rightDriveMaster.configNominalOutputReverse(0, armMMTimeout);
rightDriveMaster.configPeakOutputForward(0.5, armMMTimeout);
rightDriveMaster.configPeakOutputReverse(-0.5, armMMTimeout);
/* set closed loop gains in slot0 - see documentation */
rightDriveMaster.selectProfileSlot(armPIDSlot, armPIDLoop);
rightDriveMaster.config_kF((int) .07, 0.2, armMMTimeout);
rightDriveMaster.config_kP(2, 0.2, armMMTimeout);
rightDriveMaster.config_kI(0, 0, armMMTimeout);
rightDriveMaster.config_kD(0, 0, armMMTimeout);
/* set acceleration and vcruise velocity - see documentation */
rightDriveMaster.configMotionCruiseVelocity(70, armMMTimeout);
rightDriveMaster.configMotionAcceleration(50, armMMTimeout);
}
public void ArcadeDrive(double speed, double rotation) {
drive.arcadeDrive(speed, rotation);
drive.setSafetyEnabled(false);
}
public void ChezyDrive(double speed, double rotation, boolean quickturn) {
drive.curvatureDrive(-speed, rotation, quickturn);
}
public void ZeroEncoders() {
leftDriveMaster.setSelectedSensorPosition(0, 0, 0);
rightDriveMaster.setSelectedSensorPosition(0, 0, 0);
}
public double LookEncoders() {
return
leftDriveMaster.getSelectedSensorPosition(0);
}
public void MotionMagic(double left, double right) {
leftDriveMaster.set(ControlMode.MotionMagic, left);
rightDriveMaster.set(ControlMode.MotionMagic, right);
}
/* ----------- MOTION PROFILING COMMANNDS -----------*/
//Use this to buffer the profiles to the motor controllers
public void initDefaultCommand() {
setDefaultCommand(new PlayerDrive());
}
}