def calculatePIDs(): global pitch_output, roll_output, yaw_output, thr, pids #PID CODE #print "PID _____ PITCH: " pitch_output = constrain(pids['PITCH'].update_pid_std(C_YPR['P'], IMU.Pitch, 10000),-250, 250) #print "PID _____ ROLL: " roll_output = constrain(pids['ROLL'].update_pid_std(C_YPR['R'], IMU.Roll, 10000),-250, 250) #print "PID _____ YAW: " yaw_output = constrain(pids['YAW'].update_pid_std(wrap_180(C_YPR['Y']), wrap_180(IMU.Yaw), 10000),-360, 360)
def test_go_left_off_top(self):
self.assertEqual(
constrain((8, 6), -2, self.bounds, False),
Coords(7, 8)
)
def test_negative_infinite_slope(self):
self.assertEqual(
constrain((8, 6), -float('inf'), self.bounds, True),
Coords(8, 0)
)
def test_go_left_off_left_positive_slope(self):
self.assertEqual(
constrain((8, 6), 1/4, self.bounds, False),
Coords(0, 4)
)
def test_go_left_off_left_negative_slope(self):
self.assertEqual(
constrain((4, 6), -1/4, self.bounds, False),
Coords(0, 7)
)
def test_go_left_off_bottom(self):
self.assertEqual(
constrain((8, 6), 2, self.bounds, False),
Coords(5, 0)
)
def test_go_right_off_bottom(self):
self.assertEqual(
constrain((8, 6), -2, self.bounds, True),
Coords(11, 0)
)
def test_go_right_off_right_negative_slope(self):
self.assertEqual(
constrain((8, 6), -1, self.bounds, True),
Coords(12, 2)
)
def test_go_right_off_right_positive_slope(self):
self.assertEqual(
constrain((8, 6), 1/4, self.bounds, True),
Coords(12, 7)
)
def test_go_right_off_top(self):
self.assertEqual(
constrain((8, 6), 2, self.bounds, True),
Coords(9, 8)
)
