<commit_before>#
# Copyright (c) 2009, Georgia Tech Research Corporation
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Georgia Tech Research Corporation nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#

#  \author <NAME> (Healthcare Robotics Lab, Georgia Tech.)

import roslib; roslib.load_manifest('pan_tilt_robotis')

import time
import sys, optparse

import numpy as np, math

import hrl_lib.util as ut
import robotis.robotis_servo as rs


class PanTilt():
    
    ## Assumes that both pan and tilt servos are controlled using the
    # same usb2dynamixel adaptor.
    # @param dev_name - name of serial device of the servo controller (e.g. '/dev/robot/servo0')
    # @param pan_id -  servo id for the pan Robotis servo.
    # @param pan_id -  servo id for the tilt Robotis servo.
    # @param baudrate - for the servo controller (usb2dynamixel)
    # @param pan_speed - max pan speed (radians/sec)
    # @param tilt_speed - max tilt speed (radians/sec)
    def __init__(self, dev_name, pan_id, tilt_id, baudrate=57600,
                 pan_speed = math.radians(90),
                 tilt_speed = math.radians(90)):
        self.pan_servo = rs.robotis_servo(dev_name,pan_id,baudrate,
                                          max_speed = pan_speed)
        self.tilt_servo = rs.robotis_servo(dev_name,tilt_id,baudrate,
                                           max_speed = tilt_speed)

    ## return (pan,tilt) angles in RADIANS.
    def get_pan_tilt(self):
        pan = self.pan_servo.read_angle()
        tilt = self.tilt_servo.read_angle()
        return pan,tilt

    ## set (pan,tilt) angles in RADIANS.
    # blocks until the pan and tilt angles are attained.
    # @param pan - pan angle (RADIANS)
    # @param tilt - tilt angle (RADIANS)
    def set_pan_tilt(self,pan,tilt):
        self.pan_servo.move_angle(pan,angvel=None,blocking=False)
        self.tilt_servo.move_angle(tilt,angvel=None,blocking=True)
        self.pan_servo.move_angle(pan,angvel=None,blocking=True)

    ## new pan,tilt = current pan,tilt + pan_d,tilt_d
    # blocks until the pan and tilt angles are attained.
    # @param pan - pan angle (RADIANS)
    # @param tilt - tilt angle (RADIANS)
    def set_pan_tilt_delta(self,pan_d,tilt_d):
        p,t = self.get_pan_tilt()
        self.set_pan_tilt(p+pan_d,t+tilt_d)


if __name__ == '__main__':

    p = optparse.OptionParser()
    p.add_option('-d', action='store', type='string', dest='servo_dev_name',
                 default='/dev/robot/pan_tilt0', help='servo device string. [default= /dev/robot/pan_tilt0]')
    p.add_option('--pan_id', action='store', type='int', dest='pan_id',
                 help='id of the pan servo',default=None)
    p.add_option('--tilt_id', action='store', type='int', dest='tilt_id',
                 help='id of the tilt servo',default=None)
    p.add_option('--pan', action='store', type='float', dest='pan',
                 help='pan angle (degrees).',default=None)
    p.add_option('--tilt', action='store', type='float', dest='tilt',
                 help='tilt angle (degrees).',default=None)

    opt, args = p.parse_args()

    servo_dev_name = opt.servo_dev_name
    pan_id = opt.pan_id
    tilt_id = opt.tilt_id

    pan = opt.pan
    tilt = opt.tilt

    if pan_id == None:
        print 'Please provide a pan_id'
        print 'Exiting...'
        sys.exit()
    if tilt_id == None:
        print 'Please provide a tilt_id'
        print 'Exiting...'
        sys.exit()
    if pan == None:
        print 'Please provide a pan (angle)'
        print 'Exiting...'
        sys.exit()
    if tilt == None:
        print 'Please provide a tilt (angle)'
        print 'Exiting...'
        sys.exit()

    ptu = PanTilt(servo_dev_name,pan_id,tilt_id)
    ptu.set_pan_tilt(math.radians(pan),math.radians(tilt))

<commit_msg>minor<commit_after>#
# Copyright (c) 2009, Georgia Tech Research Corporation
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Georgia Tech Research Corporation nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#

#  \author <NAME> (Healthcare Robotics Lab, Georgia Tech.)

import roslib; roslib.load_manifest('pan_tilt_robotis')

import time
import sys, optparse

import numpy as np, math

import hrl_lib.util as ut
import robotis.robotis_servo as rs


class PanTilt():
    
    ## Assumes that both pan and tilt servos are controlled using the
    # same usb2dynamixel adaptor.
    # @param dev_name - name of serial device of the servo controller (e.g. '/dev/robot/servo0')
    # @param pan_id -  servo id for the pan Robotis servo.
    # @param pan_id -  servo id for the tilt Robotis servo.
    # @param baudrate - for the servo controller (usb2dynamixel)
    # @param pan_speed - max pan speed (radians/sec)
    # @param tilt_speed - max tilt speed (radians/sec)
    def __init__(self, dev_name, pan_id, tilt_id, baudrate=57600,
                 pan_speed = math.radians(90),
                 tilt_speed = math.radians(90)):
        self.pan_servo = rs.robotis_servo(dev_name,pan_id,baudrate,
                                          max_speed = pan_speed)
        self.tilt_servo = rs.robotis_servo(dev_name,tilt_id,baudrate,
                                           max_speed = tilt_speed)

    ## return (pan,tilt) angles in RADIANS.
    def get_pan_tilt(self):
        pan = self.pan_servo.read_angle()
        tilt = self.tilt_servo.read_angle()
        return pan,tilt

    ## set (pan,tilt) angles in RADIANS.
    # blocks until the pan and tilt angles are attained.
    # @param pan - pan angle (RADIANS)
    # @param tilt - tilt angle (RADIANS)
    def set_pan_tilt(self,pan,tilt):
        self.pan_servo.move_angle(pan,angvel=None,blocking=False)
        self.tilt_servo.move_angle(tilt,angvel=None,blocking=True)
        self.pan_servo.move_angle(pan,angvel=None,blocking=True)

    ## new pan,tilt = current pan,tilt + pan_d,tilt_d
    # blocks until the pan and tilt angles are attained.
    # @param pan - pan angle (RADIANS)
    # @param tilt - tilt angle (RADIANS)
    def set_pan_tilt_delta(self,pan_d,tilt_d):
        p,t = self.get_pan_tilt()
        self.set_pan_tilt(p+pan_d,t+tilt_d)


if __name__ == '__main__':

    p = optparse.OptionParser()
    p.add_option('-d', action='store', type='string', dest='servo_dev_name',
                 default='/dev/robot/pan_tilt0', help='servo device string. [default= /dev/robot/pan_tilt0]')
    p.add_option('--pan_id', action='store', type='int', dest='pan_id',
                 help='id of the pan servo',default=None)
    p.add_option('--tilt_id', action='store', type='int', dest='tilt_id',
                 help='id of the tilt servo',default=None)
    p.add_option('--pan', action='store', type='float', dest='pan',
                 help='pan angle (degrees).',default=None)
    p.add_option('--tilt', action='store', type='float', dest='tilt',
                 help='tilt angle (degrees).',default=None)

    opt, args = p.parse_args()

    servo_dev_name = opt.servo_dev_name
    pan_id = opt.pan_id
    tilt_id = opt.tilt_id

    pan = opt.pan
    tilt = opt.tilt

    if pan_id == None:
        print 'Please provide a pan_id'
        print 'Exiting...'
        sys.exit()
    if tilt_id == None:
        print 'Please provide a tilt_id'
        print 'Exiting...'
        sys.exit()
    if pan == None:
        print 'Please provide a pan (angle)'
        print 'Exiting...'
        sys.exit()
    if tilt == None:
        print 'Please provide a tilt (angle)'
        print 'Exiting...'
        sys.exit()

    ptu = PanTilt(servo_dev_name,pan_id,tilt_id)
    ptu.set_pan_tilt(math.radians(pan),math.radians(tilt))

# For EL-E:
# python pan_tilt.py -d /dev/robot/servos_pan_tilt_hat  --pan_id=6 --tilt_id=18 --pan=0 --tilt=0



{'fraction_non_alphanumeric': 0.0992089964738397, 'fraction_numerical': 0.003907366815972553, 'mean_word_length': 2.987082066869301, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '251020', 'n_tokens_mistral': 3407, 'n_tokens_neox': 3228, 'n_words': 1036}