186 lines
5.3 KiB
Python
186 lines
5.3 KiB
Python
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)
|
|
|