diff --git a/software/raspberry pi/hexapod.py b/software/raspberry pi/hexapod.py index a73206f..6a34d33 100644 --- a/software/raspberry pi/hexapod.py +++ b/software/raspberry pi/hexapod.py @@ -124,6 +124,7 @@ class Hexapod(Thread): # self.leg_5.reset(True) self.standby_coordinate = self.calculate_standby_coordinate(60, 75) + self.laydown_coordinate = self.calculate_laydown_coordinate(self.standby_coordinate) self.forward_path = gen_forward_path(self.standby_coordinate) self.backward_path = gen_backward_path(self.standby_coordinate) self.fastforward_path = gen_fastforward_path(self.standby_coordinate) @@ -144,7 +145,8 @@ class Hexapod(Thread): self.current_motion = None - self.standby() + # self.standby() + self.laydown() time.sleep(1) # self.leg_0.set_angle(1, 30) @@ -194,10 +196,10 @@ class Hexapod(Thread): np.sin(j3_rad) return standby_coordinate - def calculate_sit_coordinate(self, standby_coordinate): - sit_coordinate = np.zeros_like(standby_coordinate) - sit_coordinate[:, 0:2] = standby_coordinate[:, 0:2] - return sit_coordinate + def calculate_laydown_coordinate(self, standby_coordinate): + laydown_coordinate = np.zeros_like(standby_coordinate) + laydown_coordinate[:, 0:2] = standby_coordinate[:, 0:2] + return laydown_coordinate def move(self, path): for p_idx in range(0, np.shape(path)[0]): @@ -284,6 +286,19 @@ class Hexapod(Thread): self.leg_2.move_junctions(angles[2, :]) self.leg_3.move_junctions(angles[3, :]) + def laydown(self): + dest = self.laydown_coordinate + angles = self.inverse_kinematics(dest) + + self.leg_0.move_junctions(angles[0, :]) + self.leg_5.move_junctions(angles[5, :]) + + self.leg_1.move_junctions(angles[1, :]) + self.leg_4.move_junctions(angles[4, :]) + + self.leg_2.move_junctions(angles[2, :]) + self.leg_3.move_junctions(angles[3, :]) + def inverse_kinematics(self, dest): temp_dest = dest-self.mount_position local_dest = np.zeros_like(dest)