StackBot/src/Sensor/scripts/stacking_main.py

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