def GetData():
BFRMR1serialport.sendserial([255, 255, GETDATA, 0, 0, 0, 0, 0]) # send command to get sensor data from robot
while True:
a = BFRMR1serialport.getserial(10) # wait here until data is received to confirm command complete
if a is not None:
break
return a # return data packet
def RobotMove(direction, encodercount, speed, SonarThreshold, IRThreshold):
# 3.4 degrees = 1 encoder count for a turn
BFRMR1serialport.sendserial(
[255, 255, direction, encodercount, speed, SonarThreshold, IRThreshold, 0]
) # send command to move head
while True:
a = BFRMR1serialport.getserial(10) # wait here until data is received to confirm command complete
if a is not None:
break
return a # return data packet
def SonarScan(Tilt, StartAngle, EndAngle, Steps, Speed):
print "Sonar Scan"
ServoStart = int(128 - (StartAngle * 1.45)) # convert from angle to value between 0 and 255
ServoEnd = int(128 - (EndAngle * 1.45)) # convert from angle to value between 0 and 255
ServoTilt = int(128 + (Tilt * 1.45)) # convert from angle to value between 0 and 255
BFRMR1serialport.sendserial(
[255, 255, SONARSCAN, ServoTilt, ServoStart, ServoEnd, Steps, Speed]
) # send command to start sonar scan
while True:
a = BFRMR1serialport.getserial(Steps + 2) # wait here until data is received to confirm command complete
if a is not None:
break
return a # return data packet
def HeadMove(pan, tilt, speed):
panservovalue = int(128 - (pan * 1.45)) # convert from angle to value between 0 and 255
tiltservovalue = int(128 + (tilt * 1.45))
if speed <= 10:
BFRMR1serialport.sendserial(
[255, 255, HEADMOVE, panservovalue, tiltservovalue, speed, 0, 0]
) # send command to move head
while True:
a = BFRMR1serialport.getserial(10) # wait here until data is received to confirm command complete
if a is not None:
break
return a # return data packet
else:
print "Speed out of range. Value should be between 0 and 10"
def PlayTone(toneselection):
BFRMR1serialport.sendserial([255, 255, PLAYTONE, toneselection, 0, 0, 0, 0]) # send command to play a tone
return
SonarData = SonarScan(-10, -80, 80, 30, 9)
print SonarData
for x in range (0,3):
Result = ScanForTarget("HOME")
if Result == -1:
print "No Target found from scan"
#turn 120 degrees to the left
RobotData = RobotMove(ROBOTLEFT,int(120/TurnRatio),3, 0, 255) #no sonar or IR threshold so move always completes
else:
print "Target Aquired"
TargetAquired = True
break
if TargetAquired is True:
result = MoveToTarget("HOME")
if result == 1:
Run = False
print "Press button 0 to start"
else:
print "No target in view"
#Move to a different location and scan again here
Run = True
print "Press button 0 to start"
while True:
ScanGround()
"""
BFRMR1serialport.closeserial()
