44 lines
1.2 KiB
Python
Executable File
44 lines
1.2 KiB
Python
Executable File
import sys
|
|
import math
|
|
import hiwonder
|
|
import time
|
|
|
|
|
|
jetmax = hiwonder.JetMax()
|
|
sucker = hiwonder.Sucker()
|
|
target_positions = [(238, 20, 80),(238, 20, 123),(238, 20, 166)]
|
|
if __name__ == '__main__':
|
|
jetmax.go_home()
|
|
sucker.set_state(True)
|
|
time.sleep(0.5)
|
|
sucker.release(3)
|
|
time.sleep(2)
|
|
overlay = 0
|
|
|
|
while(overlay<3): # Pick up the block
|
|
hiwonder.pwm_servo1.set_position(90 , 0.1)
|
|
jetmax.set_position((0, 80, 120), 1)
|
|
time.sleep(1)
|
|
sucker.set_state(True) # Turn on the air pump
|
|
jetmax.set_position((0, 80, 85 - 5), 1)
|
|
time.sleep(1)
|
|
|
|
jetmax.set_position((0, 80, 180), 1)
|
|
time.sleep(1)
|
|
hiwonder.pwm_servo1.set_position(90, 0.1)
|
|
|
|
# Go to the target position
|
|
(x, y, z) = target_positions[overlay]
|
|
jetmax.set_position((x, y, 180), 1)
|
|
time.sleep(1)
|
|
jetmax.set_position((x, y, z), 1)
|
|
time.sleep(1)
|
|
|
|
# Put down the block
|
|
sucker.release(3) # Turn off the air pump
|
|
jetmax.set_position((x, y, 180), 1)
|
|
time.sleep(1)
|
|
jetmax.go_home()
|
|
time.sleep(1)
|
|
overlay = overlay+1
|