import QboCmd
import serial
port = '/dev/serial0'
ser = serial.Serial(port,
baudrate=115200,
bytesize=serial.EIGHTBITS,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE,
rtscts=False,
dsrdtr=False,
timeout=0)
HeadServo = QboCmd.Controller(ser)
# Open serial port
ser = serial.Serial(port,
baudrate=115200,
bytesize=serial.EIGHTBITS,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE,
rtscts=False,
dsrdtr=False,
timeout=0)
print "Open serial port sucessfully."
print(ser.name)
except:
print "Error opening serial port."
sys.exit()
QBO = QboCmd.Controller(ser)
QBO.SetServo(1, Xcoor, 100)
QBO.SetServo(2, Ycoor, 100)
QBO.SetNoseColor(0) #Off QBO nose brigth
webcam = cv2.VideoCapture(0)
#cv2.imshow("test",webcam) # Get ready to start getting images from the webcam
webcam.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,
320) # I have found this to be about the highest-
webcam.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,
240) # resolution you'll want to attempt on the pi
frontalface = cv2.CascadeClassifier(
"haarcascade_frontalface_alt2.xml") # frontal face pattern detection
profileface = cv2.CascadeClassifier(
parser.add_argument(
"-d",
"--device",
help="Servo number ID = 1:Left-Right servo 2:Up-Down Servo",
type=int)
parser.add_argument(
"-c",
"--command",
help=
"Command: SET_SERVO_ID, SET_SERVO_CW_LIM, SET_SERVO_CCW_LIM, SET_USB2SERVO_FWD, SET_SERVO_ENABLE"
)
parser.add_argument("param", help="Command parameter", type=int)
args = parser.parse_args()
comPort = serial.Serial('/dev/serial0', 115200, timeout=0)
HeadCtrl = QboCmd.Controller(comPort)
def ChangeDeviceID(device, cmd, value):
HeadCtrl.GetHeadCmd("SET_SERVO_LED", [device, 1])
time.sleep(0.1)
Id = HeadCtrl.GetHeadCmd("GET_SERVO_BYTE_REG", [device, 3])
print "Present ID", Id
time.sleep(0.1)
HeadCtrl.GetHeadCmd(cmd, [device, value])
print cmd, [device, value]
time.sleep(0.1)
newId = HeadCtrl.GetHeadCmd("GET_SERVO_BYTE_REG", [value, 3])
print "New ID", newId
time.sleep(.5)
HeadCtrl.GetHeadCmd("SET_SERVO_LED", [value, 0])