diff --git a/utils/control_util.py b/utils/control_util.py index e3eea70..2a1bd29 100644 --- a/utils/control_util.py +++ b/utils/control_util.py @@ -8,7 +8,7 @@ class ControlUtil: __fa:FrankaArm = None __ser: serial.Serial = None - cnt_rotation = 0 + curr_rotation = 0 BASE_TO_WORLD:np.ndarray = np.asarray([ [1, 0, 0, -0.61091665], @@ -64,13 +64,14 @@ class ControlUtil: "left": 1, "right": 0 } - ControlUtil.cnt_rotation += degree + delta_degree = degree - ControlUtil.curr_rotation + ControlUtil.curr_rotation += delta_degree print(f"Table rotated {ControlUtil.cnt_rotation} degree") if degree >= 0: - turn_angle = degree + turn_angle = delta_degree turn_direction = turn_directions["right"] else: - turn_angle = -degree + turn_angle = -delta_degree turn_direction = turn_directions["left"] write_len = ControlUtil.__ser.write(f"CT+TRUNSINGLE({turn_direction},{turn_angle});".encode('utf-8'))