[270, 90, 180, 180, 120],
[0, 90, 180, 180, 120],
[0, 115, 160, 180, 120],
[0, 45, 225, 180, 120],
[0, 5, 270, 180, 120],
[0, 45, 225, 180, 120],
[0, 90, 180, 180, 120],
]
# variables
arriveThreshold = 10
waitTime = 0
index = 0
# create Orion5 object
orion = orion5.Orion5()
# main loop
while True:
joints = path[index]
print("Moving to:", joints)
# keep writing the desired position until we arrive
while not arrived(joints, orion.getAllJointsPosition(), arriveThreshold):
orion.setAllJointsPosition(joints)
time.sleep(0.1)
# increment and wrap the current position within the sequence
index += 1
input(
'\nPosition claws around pinion gear\n80mm between inner faces\n40mm from centre of gear to claw face\npress enter when ready'
)
gotoPositionBlocking(o5, 355.0)
o5.claw.setVariable('misc variables', 'cwAngleLimit', 0)
o5.claw.setVariable('misc variables', 'ccwAngleLimit', 1087)
gotoPositionBlocking(o5, 120.0)
print('Waiting for Orion5 to be connected')
comport = waitForOrion5Forever()
orion = orion5.Orion5(mode='standalone',
serialName=comport,
useSimulator=False)
time.sleep(5)
a = input(
'Would you like to zero the existing offsets? (recommended for recalibrations) y/n\n> '
).lower()
if a == 'y':
applyOffsets(orion, [0, 0, 0, 0, 0])
try:
a = input('\nWould you like to eject claws? y/n\n> ').lower()
if a == 'y':
clawEject(orion)
import time
import orion5
from orion5.utils.general import waitForOrion5Forever
print('\nSearching for Orion5...')
comport = waitForOrion5Forever()
print('Found Orion5, serial port name:', comport)
orion = orion5.Orion5(mode='standalone', serialName=comport)
time.sleep(3)
def gotoPositionBlocking(pos):
orion.claw.setPosition(pos)
while abs(pos - orion.claw.getPosition()) > 2:
print('%.2f' % orion.claw.getPosition())
time.sleep(0.2)
try:
gotoPositionBlocking(359.0)
orion.claw.setVariable('misc variables', 'cwAngleLimit', 1060)
orion.claw.setVariable('misc variables', 'ccwAngleLimit', 1059)
gotoPositionBlocking(170.0)
orion.claw.setVariable('misc variables', 'cwAngleLimit', 0)
orion.claw.setVariable('misc variables', 'ccwAngleLimit', 1087)
'Set this flag to enter simulator mode first, and you control the serial connection from client side.'
)
args = parser.parse_args()
socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
socket_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
socket_server.bind((utils.SOCKET_HOST, utils.SOCKET_PORT))
socket_server.settimeout(None)
socket_server.listen(2)
print('\nOrion5 Server Started')
print('\nWaiting for Connections')
flag = Flag()
orion = orion5.Orion5(mode='standalone',
serialName=None,
useSimulator=args.simulator)
threads = []
running = True
try:
while running:
# check every second for new sockets trying to connect
ready, _, _ = select.select([socket_server], [], [], 0.25)
# if a new is connected
for socket in ready:
if socket is socket_server:
# if flag.check():
import time
import math
import random
import orion5
from orion5 import orion5_math
OUTER_RADIUS = 275.0
GRAB_BLOCK_RADIUS = 200.0
LOCALISE_RADIUS = 210.0
TABLE_HEIGHT = 20.0
CLAW_OPENED = 300
CLAW_CLOSED = 160
orion_object = orion5.Orion5()
def get_estop(orion):
return bool(orion.getVar(0, 'readConfig', 'Estop'))
def set_estop(orion, state):
if state:
state = 1
else:
state = 0
return orion.setVar(0, 'setConfig', 'Estop', state)
