def handle_write_zero_position(req):
"""
A handle for the service to be able to handle write_zero_position
:param req: The requester object which contains the input from the user
:type req: object
"""
lock.acquire()
response = as5048a.write_zero_position()
if as5048a.is_error:
errors = as5048a.clear_and_get_error()
lock.release()
message = """Failed to write zero position, the following errors were encountered:
Parity error: {}
Command invalid: {}
Framing error: {}""".format(**errors)
return TriggerResponse(False, message)
if as5048a.is_parity_mismatch:
lock.release()
message = "Failed to write zero position, the received package has a mismatched parity"
return TriggerResponse(False, message)
lock.release()
old = AS5048.calc_angle(response['old_zero_position'])
new = AS5048.calc_angle(response['new_zero_position'])
message = """Successfully wrote a new zero position
Old Zero Position: {} rad
New Zero Position: {} rad""".format(old, new)
return TriggerResponse(True, message)
def test_calc_angle(self):
self.assertEqual(AS5048.calc_angle(0), 0)
self.assertEqual(AS5048.calc_angle(0x1000), math.pi / 2)
self.assertEqual(AS5048.calc_angle(0x2000), math.pi)
self.assertEqual(AS5048.calc_angle(0x3000), math.pi * 3 / 2)
self.assertEqual(AS5048.calc_angle(0x4000), 2 * math.pi)