Delete demo.py

parent d58f24f2
import sys
import numpy as np
import time
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = False
enable_log = True
scintillators_positions = np.array([224, 241, 250]) #In mm, relative to robot end (FRF)
# When the robot is manually jogged to the position depicted in Tools/calibration_setup.stl, the
# source is positionned at (10.00,0.02,-347.00) mm relative to the end of the robot.
source_position = np.array([201.700,110.595,139.742])+np.array([10,0.02,-347])
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
scintillators_positions = scintillators_positions,
source_position = source_position)
# Movements
robot.run('MovePose',[200,0,-75,0,0,0])
robot.run('SetCartLinVel',60)
robot.run('SetCartAngVel',30)
#Drawing a square
robot.run('MoveLinRelTRF',[0,-50,0,0,0,0])
robot.run('MoveLinRelTRF',[0,0,100,0,0,0])
robot.run('MoveLinRelTRF',[0,100,0,0,0,0])
robot.run('MoveLinRelTRF',[0,0,-100,0,0,0])
robot.run('MoveLinRelTRF',[0,-50,0,0,0,0])
robot.run('MoveLinRelTRF', [50,0,0,0,0,0])
#Drawing a cone
robot.run('MoveLinRelTRF',[0,0,0,0,-20,0])
robot.run('MoveLinRelWRF',[0,0,0,0,0,-60])
robot.run('MoveLinRelWRF',[0,0,0,0,0,120])
robot.run('MoveLinRelWRF',[0,0,0,0,0,-60])
robot.run('MoveLinRelTRF',[0,0,0,0,20,0])
# time.sleep(20)
robot.run('SetCartAngVel', 45)
robot.run('SetCartLinVel', 150)
robot.start_tracking()
# Approching the source in straight lines
number_of_straight_lines = 3
for i in range(number_of_straight_lines):
robot.move_position_relative_to_source(np.array([0,-185,20*i]))
robot.linear_sequence('y', 80, 10)
robot.linear_sequence('y', 40, 5, do_clear_buffer=False)
robot.linear_sequence('y', 20, 2, do_clear_buffer=False)
robot.linear_sequence('y', 10, 1, do_clear_buffer=False)
# Turning around the source
robot.move_position_relative_to_source(np.array([0,-40,30]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.turn_around_source(90,-5)
robot.turn_around_source(180,5, do_clear_buffer=False)
robot.turn_around_source(90,-5, do_clear_buffer=False)
# Approaching the source with an angle about x-axis
robot.move_position_relative_to_source(np.array([0,-185,0]))
robot.run('MoveLinRelTRF', [0,0,0,15,0,0])
robot.linear_sequence('y', 80, 10)
robot.linear_sequence('y', 40, 5, do_clear_buffer=False)
robot.linear_sequence('y', 20, 2, do_clear_buffer=False)
robot.linear_sequence('y', 10, 1, do_clear_buffer=False)
# Approaching the source with an angle about y-axis
robot.move_position_relative_to_source(np.array([0,-185,0]))
robot.run('MoveLinRelTRF', [0,0,0,0,10,0])
robot.linear_sequence('y', 80, 10)
robot.linear_sequence('y', 40, 5, do_clear_buffer=False)
robot.linear_sequence('y', 20, 2, do_clear_buffer=False)
robot.linear_sequence('y', 10, 1, do_clear_buffer=False)
robot.stop_tracking()
robot.run('MoveLinRelTRF', [0,-75,0,0,0,0])
# Deactivating robot
robot.deactivate()
print('Main program done')
sys.stdout.flush()
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment