1818 * Tweak the PID gains accordingly.
1919"""
2020
21- import ctre
21+ import phoenix5
2222import wpilib
2323
2424
@@ -37,7 +37,7 @@ class Robot(wpilib.TimedRobot):
3737 kTimeoutMs = 10
3838
3939 def robotInit (self ):
40- self .talon = ctre .WPI_TalonSRX (3 )
40+ self .talon = phoenix5 .WPI_TalonSRX (3 )
4141 self .joy = wpilib .Joystick (0 )
4242
4343 self .loops = 0
@@ -46,7 +46,7 @@ def robotInit(self):
4646
4747 # choose the sensor and sensor direction
4848 self .talon .configSelectedFeedbackSensor (
49- ctre .FeedbackDevice .CTRE_MagEncoder_Relative ,
49+ phoenix5 .FeedbackDevice .CTRE_MagEncoder_Relative ,
5050 self .kPIDLoopIdx ,
5151 self .kTimeoutMs ,
5252 )
@@ -106,14 +106,14 @@ def teleopPeriodic(self):
106106
107107 # 10 Rotations * 4096 u/rev in either direction
108108 self .targetPos = leftYstick * 4096 * 10.0
109- self .talon .set (ctre .ControlMode .Position , self .targetPos )
109+ self .talon .set (phoenix5 .ControlMode .Position , self .targetPos )
110110
111111 # on button2 just straight drive
112112 if button2 :
113113 # Percent voltage mode
114- self .talon .set (ctre .ControlMode .PercentOutput , leftYstick )
114+ self .talon .set (phoenix5 .ControlMode .PercentOutput , leftYstick )
115115
116- if self .talon .getControlMode () == ctre .ControlMode .Position :
116+ if self .talon .getControlMode () == phoenix5 .ControlMode .Position :
117117 # append more signals to print when in speed mode.
118118 sb .append ("\t err: %s" % self .talon .getClosedLoopError (self .kPIDLoopIdx ))
119119 sb .append ("\t trg: %.3f" % self .targetPos )
0 commit comments