Coverage for farmbot_sidecar_starter_pack/functions/movements.py: 100%

79 statements  

« prev     ^ index     » next       coverage.py v7.4.4, created at 2024-09-11 15:43 -0700

1""" 

2MovementControls class. 

3""" 

4 

5# └── functions/movements.py 

6# ├── [BROKER] get_xyz() 

7# ├── [BROKER] move() 

8# ├── [BROKER] set_home() 

9# ├── [BROKER] find_home() 

10# ├── [BROKER] find_axis_length() 

11# └── [BROKER] check_position() 

12 

13from .broker import BrokerConnect 

14from .information import Information 

15 

16AXES = ["x", "y", "z", "all"] 

17 

18 

19def validate_axis(axis): 

20 """Validate axis.""" 

21 if axis not in AXES: 

22 raise ValueError(f"Invalid axis: {axis} not in {AXES}") 

23 

24 

25class MovementControls(): 

26 """MovementControls class.""" 

27 

28 def __init__(self, state): 

29 self.broker = BrokerConnect(state) 

30 self.info = Information(state) 

31 self.state = state 

32 

33 def move(self, x=None, y=None, z=None, safe_z=None, speed=None): 

34 """Moves to the specified (x, y, z) coordinate.""" 

35 self.state.print_status(description=f"Moving to ({x}, {y}, {z}).") 

36 

37 def axis_overwrite(axis, value): 

38 return { 

39 "kind": "axis_overwrite", 

40 "args": { 

41 "axis": axis, 

42 "axis_operand": { 

43 "kind": "numeric", 

44 "args": { 

45 "number": value 

46 } 

47 } 

48 } 

49 } 

50 

51 def safe_z_body_item(): 

52 return { 

53 "kind": "safe_z", 

54 "args": {}, 

55 } 

56 

57 def speed_overwrite(axis, speed): 

58 return { 

59 "kind": "speed_overwrite", 

60 "args": { 

61 "axis": axis, 

62 "speed_setting": { 

63 "kind": "numeric", 

64 "args": { 

65 "number": speed, 

66 } 

67 } 

68 } 

69 } 

70 

71 move_message = { 

72 "kind": "move", 

73 "args": {}, 

74 "body": [], 

75 } 

76 

77 if x is not None: 

78 move_message["body"].append(axis_overwrite("x", x)) 

79 

80 if y is not None: 

81 move_message["body"].append(axis_overwrite("y", y)) 

82 

83 if z is not None: 

84 move_message["body"].append(axis_overwrite("z", z)) 

85 

86 if speed is not None: 

87 move_message["body"].append(speed_overwrite("x", speed)) 

88 move_message["body"].append(speed_overwrite("y", speed)) 

89 move_message["body"].append(speed_overwrite("z", speed)) 

90 

91 if safe_z is not None: 

92 move_message["body"].append(safe_z_body_item()) 

93 

94 self.broker.publish(move_message) 

95 

96 def set_home(self, axis="all"): 

97 """Sets the current position as the home position for a specific axis.""" 

98 self.state.print_status(description="Setting home position") 

99 

100 validate_axis(axis) 

101 

102 set_home_message = { 

103 "kind": "zero", 

104 "args": { 

105 "axis": axis 

106 } 

107 } 

108 self.broker.publish(set_home_message) 

109 

110 def find_home(self, axis="all", speed=100): 

111 """Moves the device to the home position for a specified axis.""" 

112 self.state.print_status(description="Finding home position") 

113 

114 validate_axis(axis) 

115 

116 if speed > 100 or speed < 1: 

117 error = "ERROR: Speed constrained to 1-100." 

118 self.state.print_status(description=error, update_only=True) 

119 self.state.error = error 

120 return 

121 

122 message = { 

123 "kind": "find_home", 

124 "args": { 

125 "axis": axis, 

126 "speed": speed 

127 } 

128 } 

129 self.broker.publish(message) 

130 

131 def find_axis_length(self, axis="all"): 

132 """Finds the length of a specified axis.""" 

133 self.state.print_status(description="Finding axis length") 

134 

135 validate_axis(axis) 

136 

137 find_axis_length_message = { 

138 "kind": "calibrate", 

139 "args": { 

140 "axis": axis 

141 } 

142 } 

143 

144 self.broker.publish(find_axis_length_message) 

145 

146 def get_xyz(self): 

147 """Returns the current (x, y, z) coordinates of the FarmBot.""" 

148 self.state.print_status(description="Getting current coordinates") 

149 

150 tree_data = self.info.read_status() 

151 if tree_data is None: 

152 error = "ERROR: No location data available." 

153 self.state.print_status(description=error, update_only=True) 

154 self.state.error = error 

155 return None 

156 position = tree_data["location_data"]["position"] 

157 

158 self.state.print_status( 

159 description=f"Current position: {position}.", 

160 update_only=True) 

161 return position 

162 

163 def check_position(self, coordinate, tolerance): 

164 """Verifies position of the FarmBot within specified tolerance range.""" 

165 

166 self.state.print_status( 

167 description=f"Checking if position is {coordinate} with tolerance: {tolerance}.") 

168 

169 actual_vals = self.get_xyz() 

170 

171 if actual_vals is None: 

172 return False 

173 

174 for axis in ['x', 'y', 'z']: 

175 user_value = coordinate[axis] 

176 actual_value = actual_vals[axis] 

177 if not actual_value - tolerance <= user_value <= actual_value + tolerance: 

178 description = "Farmbot is NOT at position." 

179 description += f"\n Current position: {actual_vals}." 

180 self.state.print_status( 

181 description=description, 

182 update_only=True) 

183 return False 

184 

185 self.state.print_status( 

186 description=f"Farmbot is at position: {actual_vals}.", 

187 update_only=True) 

188 return True