def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(
0.8,
0.0,
1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0,
self.bno055,
self.heading_hold_pid_output)
"""self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)"""
self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-0.2, 0.2)
#self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
self.joystick_rate = 0.3
def test_range_finder_syntax():
""" Super basic tests that do nothing but check for syntactical errors"""
rf = RangeFinder(0)
rf.range_finder_counter = MagicMock()
pid_value = rf.pidGet()
dist = rf.getDistance()
