def setUp(self): self.driver_obj = Driver()
class TestDriverCalculations(unittest.TestCase): # sign conventions (revised): # axis: x axis is parallel to goal, y axis is to- the left when facing # the goal direction, z-axis is oriented up # positive heading error - rotated counter clockwise from goal # positve offset error - positive y-axis, def setUp(self): self.driver_obj = Driver() def test_zero(self): heading = 0 offset = 0 adjusted_heading = self.driver_obj.calc_adjusted_heading(heading, offset) self.assertTrue(is_close(adjusted_heading, 0.0)) def test_pos_heading(self): heading = 1.0 offset = 0 adjusted_heading = self.driver_obj.calc_adjusted_heading(heading, offset) self.assertTrue(is_close(adjusted_heading, heading)) def test_neg_heading(self): heading = -1.0 offset = 0 adjusted_heading = self.driver_obj.calc_adjusted_heading(heading, offset) self.assertTrue(is_close(adjusted_heading, heading)) def test_pure_offset1(self): heading = 0 offset = .5 adjusted_heading = self.driver_obj.calc_adjusted_heading(heading, offset) self.assertTrue(adjusted_heading > 0.0) self.assertTrue(is_close(adjusted_heading, .75*math.pi/2, 4)) def test_pure_offset2(self): heading = 0 offset = -.5 adjusted_heading = self.driver_obj.calc_adjusted_heading(heading, offset) self.assertTrue(adjusted_heading < 0.0) self.assertTrue(is_close(adjusted_heading, -.75*math.pi/2, 4)) def test_angular_vel1(self): adjusted_heading = 0 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(is_close(ang_vel, 0.0)) def test_angular_vel2(self): adjusted_heading = 0.5 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel < 0.0) self.assertTrue(is_close(ang_vel, -0.5)) def test_angular_vel3(self): adjusted_heading = -0.5 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel > 0.0) self.assertTrue(is_close(ang_vel, 0.5)) def test_angular_vel4(self): adjusted_heading = 0.25 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel < 0.0) self.assertTrue(is_close(ang_vel, -0.4333), 3) def test_angular_vel5(self): adjusted_heading = -0.25 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel > 0.0) self.assertTrue(is_close(ang_vel, 0.4333), 3) def test_angular_vel6(self): adjusted_heading = 100.0 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel < 0.0) self.assertTrue(is_close(ang_vel, -self.driver_obj.max_omega)) def test_angular_vel7(self): adjusted_heading = -100.0 ang_vel = self.driver_obj.calc_angular_velocity(adjusted_heading) self.assertTrue(ang_vel > 0.0) self.assertTrue(is_close(ang_vel, self.driver_obj.max_omega)) def test_linear_veloicty1(self): along = 0.0 off = 0.0 ang_vel = 0.0 goal_vel = 0.0 lin_vel = self.driver_obj.calc_linear_velocity(along, off, ang_vel, goal_vel) self.assertTrue(is_close(lin_vel, 0.0)) def test_linear_veloicty2(self): along = 0.0 off = 0.0 ang_vel = 0.0 goal_vel = self.driver_obj.max_v lin_vel = self.driver_obj.calc_linear_velocity(along, off, ang_vel, goal_vel) self.assertTrue(is_close(lin_vel, self.driver_obj.max_v)) def test_linear_veloicty3(self): along = 0.0 off = 0.0 ang_vel = 0.0 goal_vel = -self.driver_obj.max_v lin_vel = self.driver_obj.calc_linear_velocity(along, off, ang_vel, goal_vel) self.assertTrue(is_close(lin_vel, -self.driver_obj.max_v)) def test_linear_veloicty4(self): along = 0.5 off = 0.0 ang_vel = 0.0 goal_vel = 0.0 lin_vel = self.driver_obj.calc_linear_velocity(along, off, ang_vel, goal_vel) self.assertTrue(is_close(lin_vel, -0.5)) def test_linear_veloicty5(self): along = -0.5 off = 0.0 ang_vel = 0.0 goal_vel = 0.0 lin_vel = self.driver_obj.calc_linear_velocity(along, off, ang_vel, goal_vel) self.assertTrue(is_close(lin_vel, 0.5))