import time import hiwonder from utils.pixel_convert import pixel2robot jetmax = hiwonder.JetMax() sucker = hiwonder.Sucker() pre_x, pre_y, pre_z = 123, -195, 65 ori_x, ori_y, ori_z = 0, -212, 60 stack_id = 0 STACK_P_X, STACK_P_Y = -110, -8 STACK_ANGLE = 90 FIRST_Z = 75 PLACE_Z = -10 class Stackbot: def __init__(self): self.stack_xyz = [STACK_P_X, STACK_P_Y] self.ori_x, self.ori_y, self.ori_z = 0, -212, 60 self.first_z = 70 self.stack_pick_angle = 0 self.current_level = 0 def pick_stack(self): stack_xyz = [STACK_P_X, STACK_P_Y] jetmax.go_home(1) time.sleep(1) hiwonder.pwm_servo1.set_position(180, 0.1) time.sleep(0.5) jetmax.set_position((stack_xyz[0], stack_xyz[1], FIRST_Z + 100), 1) time.sleep(2) sucker.suck() jetmax.set_position((stack_xyz[0], stack_xyz[1], FIRST_Z - 8), 2) time.sleep(3) self.stack_pick_angle = hiwonder.serial_servo.read_position(1) jetmax.set_position((stack_xyz[0], stack_xyz[1], pre_z + 100), 1) time.sleep(1) jetmax.go_home(1) time.sleep(1) jetmax.go_home(1) time.sleep(1) def place_stack(self, x, y, theta, level): stack_xyz = (x, y, PLACE_Z + level * 12) jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 100), 1) time.sleep(1) current_angle = hiwonder.serial_servo.read_position(1) # hiwonder.pwm_servo1.set_position(180 + (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.1) current_angle = (current_angle-self.stack_pick_angle) * 0.25 current_stack_angle = current_angle + STACK_ANGLE if current_stack_angle > 180: current_stack_angle = current_stack_angle - 180 if current_stack_angle > theta: # print("current_stack_angle > theta") hiwonder.pwm_servo1.set_position((180 - (180 - (current_stack_angle - theta))), 0.5) # print(current_stack_angle - theta) else: # print("current_stack_angle < theta") hiwonder.pwm_servo1.set_position(180 - (theta- current_stack_angle), 0.5) time.sleep(1) # if stack_id == 6: # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 + 30, 0.5) # else: # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.5) # hiwonder.pwm_servo1.set_position(180 - theta, 0.5) # time.sleep(1) jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2]), 2) time.sleep(2) sucker.release() def place_stack_classic(self, px, py, theta, level): stack_xyz = pixel2robot(px, py) print(stack_xyz) stack_xyz = (stack_xyz[0], stack_xyz[1], PLACE_Z + level * 12) jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 100), 1) time.sleep(1) current_angle = hiwonder.serial_servo.read_position(1) # hiwonder.pwm_servo1.set_position(180 + (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.1) print(current_angle, self.stack_pick_angle) time.sleep(5) # if stack_id == 6: # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 + 30, 0.5) # else: # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.5) hiwonder.pwm_servo1.set_position(180 - theta, 0.5) time.sleep(1) jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 10), 2) time.sleep(2) sucker.release() if __name__ == '__main__': # jetmax.go_home(1) # time.sleep(1) # jetmax.set_position((ori_x, ori_y, 200), 1) hiwonder.pwm_servo1.set_position(180, 0.1) # pick_stack(1) sucker.release() stackbot = Stackbot() stackbot.pick_stack() stackbot.place_stack(-100, -100, 90, 10) # stackbot.pick_stack(2) # stackbot.place_stack(2, 172, 217, 150, 0) # stackbot.pick_stack(3) # stackbot.place_stack(3, 300, 430, 90, 0) # stackbot.pick_stack(4) # stackbot.place_stack(4, 274, 120, 90, 1) # stackbot.pick_stack(5) # stackbot.place_stack(5, 180, 373, 30, 1) # stackbot.pick_stack(6) # stackbot.place_stack(6, 412, 333, 150, 1) # while True: # ori_z += 16 # pick_stack(stack_id) # jetmax.set_position((ori_x, ori_y, ori_z), 1) # time.sleep(1) # sucker.release() # time.sleep(2) # stack_id += 1 # while True: # jetmax.set_position((cur_x-100, cur_y, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x+100, cur_y, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y-50, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y+50, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y, cur_z), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y, cur_z-100), 1) # time.sleep(2) # jetmax.set_position((cur_x, cur_y, cur_z), 0.5) # time.sleep(2)