11
11
import com .ctre .phoenix .motorcontrol .DemandType ;
12
12
import com .ctre .phoenix .motorcontrol .FeedbackDevice ;
13
13
import com .ctre .phoenix .motorcontrol .NeutralMode ;
14
+ import com .ctre .phoenix .motorcontrol .can .VictorSPX ;
14
15
15
16
import org .mayheminc .robot2020 .Constants ;
16
17
import org .mayheminc .util .MayhemTalonSRX ;
17
18
import org .mayheminc .util .PidTunerObject ;
18
19
20
+ import edu .wpi .first .wpilibj .Victor ;
19
21
import edu .wpi .first .wpilibj .smartdashboard .SmartDashboard ;
20
22
import edu .wpi .first .wpilibj2 .command .SubsystemBase ;
21
23
22
24
public class Intake extends SubsystemBase implements PidTunerObject {
23
25
24
26
private final int PIVOT_CLOSE_ENOUGH = 50 ;
25
- private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX (Constants .Talon .INTAKE_ROLLERS );
27
+ private final VictorSPX rollerTalon = new VictorSPX (Constants .Talon .INTAKE_ROLLERS );
26
28
private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX (Constants .Talon .INTAKE_PIVOT );
27
29
private final int PIVOT_ZERO_POSITION = 900 ;
28
30
public final double PIVOT_UP = 900.0 ;
@@ -44,8 +46,10 @@ enum PivotMode {
44
46
*/
45
47
public Intake () {
46
48
rollerTalon .setNeutralMode (NeutralMode .Coast );
47
- rollerTalon .configNominalOutputVoltage (+0.0f , -0.0f );
48
- rollerTalon .configPeakOutputVoltage (+12.0 , -12.0 );
49
+ rollerTalon .configNominalOutputForward (+0.0f );
50
+ rollerTalon .configNominalOutputReverse (0.0 );
51
+ rollerTalon .configPeakOutputForward (+12.0 );
52
+ rollerTalon .configPeakOutputReverse (-12.0 );
49
53
50
54
configPivotMotor (pivotTalon );
51
55
}
@@ -93,12 +97,10 @@ public void zero() {
93
97
*/
94
98
public void setRollers (double power ) {
95
99
rollerTalon .set (ControlMode .PercentOutput , power );
96
-
97
100
}
98
101
99
102
public void setPivot (Double b ) {
100
103
m_targetPosition = b ;
101
- // pivotTalon.set(ControlMode.Position, b);
102
104
103
105
mode = PivotMode .PID_MODE ;
104
106
isMoving = true ;
@@ -159,7 +161,7 @@ public void updateSmartDashBoard() {
159
161
160
162
SmartDashboard .putBoolean ("Intake Is Moving" , isMoving );
161
163
SmartDashboard .putBoolean ("Intake PID Mode" , (mode == PivotMode .PID_MODE ));
162
- SmartDashboard .putNumber ("Intake Rollers" , rollerTalon .getMotorOutputVoltage ());
164
+ SmartDashboard .putNumber ("Intake Rollers" , rollerTalon .getMotorOutputPercent ());
163
165
}
164
166
165
167
public void setPivotVBus (double VBus ) {
0 commit comments