def __init__(self):
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0., 0.], 0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.
self.offset_counter_iteration = 100
def __init__(self):
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0.,0.],0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading =0.
self.offset_counter_iteration = 100
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0., 0.], 0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] #Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0,
0]) #Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write(
"- Check if eBot is on, paired and connected. If not, power up and try again. \n"
)
#sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
class eBot:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0., 0.], 0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] #Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0,
0]) #Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write(
"- Check if eBot is on, paired and connected. If not, power up and try again. \n"
)
#sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if (self.serialReady):
#self.cmdEnable(0)
self.stopmoving()
self.sendOutputs()
self.sendPacket([CMD.CLOSE])
# self.flushSerialPort()
try:
self.port.close()
except:
pass
app.soar.removeFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
#if self.serialReady:
#self.cmdEnable(0)
#self.stopmoving()
#self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
from soar.serial import Serial
self.connect(Serial)
self.open()
#self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
def getOpenPorts(self):
"""
This Function Returns a list of tuples with the port number and its description. Used for Windows only
"""
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
#maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
#return ports
#ports = getOpenPorts()
devicePorts = []
for port in ports:
#Just because it is formatted that way...
if 'BthModem' in port[1][8:] or 'VCP' in port[1][
8:] or 'ProlificSerial' in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def connect(self, Serial):
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
#usbSerial = glob.glob('/dev/ttyUSB*')
ports = glob.glob('/dev/rfcomm*')
elif sys.platform == "darwin":
ports = glob.glob('/dev/tty.eBot*')
#usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
#ports = ['COM' + str(i + 1) for i in range(256)]
#EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
for port in ports:
try:
s = Serial(port, baudRate, timeout=1, writeTimeout=1)
s._timeout = 1.0
s._writeTimeout = 1.0
#try:
# s.open()
#except:
# continue
line = "aaaa"
while line[:2] != "eB":
if (s.inWaiting() > 0):
line = s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if (line[:2] == "eB"):
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 1.0
self.port._writeTimeout = 1.0
self.port.flushInput()
self.port.flushOutput()
if (line[:2] == "eB"):
break
#s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if (connect == 0):
try:
self.port.close()
except:
pass
#sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
# Open connection to robot
def open(self):
numAttempts = 3
timeout = 5.0
startt = time()
while (self.connectionState != STATE_READY):
if (self.connectionState == STATE_NO_SYNC):
self.sendPacket([CMD_SYNC_SEND.CHECK1])
elif (self.connectionState == STATE_SYNC):
self.sendPacket([CMD_SYNC_SEND.CHECK2])
self.connectionState = STATE_READY
break
#elif (self.connectionState == STATE_READY):
# pass
try:
pkt = self.recvPacket()
except:
if (self.connectionState == STATE_NO_SYNC
and numAttempts >= 0):
numAttempts -= 1
else:
self.port.close()
sys.stderr.write(
"Could not open serial port. Is robot turned on and connected?\n"
)
raise Exception("No Robot Found")
continue
if (pkt[3] == CMD_SYNC_RECV.CHECK1):
self.connectionState = STATE_SYNC
# elif (pkt[3]==CMD_SYNC_RECV.CHECK2):
# self.connectionState = STATE_READY
if (time() - startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
sleep(.01)
try:
self.port.write('A')
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write(
"Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n"
)
#changed = [0, 0, 0, 0, 0, 0]
#while 0 in changed:
# #self.cmdPulse()
# self.sipRecv()
# self.storedsonars.get()
# changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))]
self.serialReady = True
# turn on io packets
#self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0])
def getPose(self):
return self.odpose.get()
def initGlobals(self):
self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write(
"Trying to set maximum translational velocity too high for real robot\n"
)
if maxRotVel > 1.75:
sys.stderr.write(
"Trying to set maximum rotational velocity too high for real robot\n"
)
def enableTeleportation(self, prob, pose):
sys.stderr.write(
"Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if (not self.asynchronous):
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v, w)
sleep(dt)
self.motorOutput(0, 0)
def sendOutputs(self):
data = [CMD.RECV]
v = int(self.trans.get() * 1000.0)
if (v >= 0):
#data = [abs(v) & 0xFF, v >> 8, 0x01]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x01)
else:
#data = [abs(v) & 0xFF, v >> 8, 0x10]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x10)
rv = int(self.rot.get() * 220 / pi) # 180*1.5
if (rv >= 0):
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x01)
else:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x10)
self.sipSend(data)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get() * 1000.0))
self.cmdRvel(int(self.rot.get() * 180 / pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [
ArcosCmd.DIGOUT,
ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get() * 25.5))) & 0xFF),
0xFF
]
self.sipSend(data)
# update sonars, odometry and stalled
def read(self):
self.incoming = None
if self.offset_counter > 1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting() != 0:
self.incoming = self.port.readline().rstrip('\n')
self.offset_counter += 1
try:
self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \
self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \
self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \
self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \
self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";")
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
print "eBot.read(): bad formatted data received"
print self.incoming
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = (self.Ax + self.Ax_offset)
self.Ay_offset = (self.Ay + self.Ay_offset)
self.Az_offset = (self.Az + self.Az_offset)
self.Gx_offset = (self.Gx + self.Gx_offset)
self.Gy_offset = (self.Gy + self.Gy_offset)
self.Gz_offset = (self.Gz + self.Gz_offset)
if self.offset_counter == self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset / self.offset_counter_iteration
self.Ay_offset = self.Ay_offset / self.offset_counter_iteration
self.Az_offset = self.Az_offset / self.offset_counter_iteration
self.Gx_offset = self.Gx_offset / self.offset_counter_iteration
self.Gy_offset = self.Gy_offset / self.offset_counter_iteration
self.Gz_offset = self.Gz_offset / self.offset_counter_iteration
if self.offset_counter > self.offset_counter_iteration:
sampling_time = (self.time_stamp -
self.prev_time_stamp) / 1000.
if sampling_time > 0:
if abs(self.Gz -
self.Gz_offset) > 50: # to remove the noise
self.heading = self.heading + sampling_time * (
self.Gz - self.Gz_offset
) # the integration to get the heading
self.heading_scaled = self.heading / 130.5 #128.7
self.heading_scaled = self.heading_scaled
self.pos_values[0],self.pos_values[1],self.pos_values[2] = \
self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time)
return self.incoming
def update(self, dummyparameter=0):
if (self.serialReady):
#self.sendPacket([CMD.RECV])
#self.sendMotorCmd()
#self.sendDigitalOutCmd()
# temp = self.port.inWaiting()
self.sendOutputs()
#sleep(.005)
#self.sendPacket([CMD.SEND])
self.read()
sleep(.005)
self.parseData()
self.from_update = 1
#if (self.sipRecv() > 0):
# self.zero_recv_cnt = 0
#else:
# self.zero_recv_cnt += 1
# if (self.zero_recv_cnt > 400):
# self.serialReady = False
# app.soar.closeEBot()
# sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self, data):
c = 0
i = 3
n = data[2] - ord('0') - 2
while (n > 1):
c += (data[i] << 8) | (data[i + 1])
c = c & 0xFFFF
n -= 2
i += 2
if (n > 0):
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if (self.asynchronous):
pass
# self.stopSerialThread()
else:
#self.sendMotorCmd()
self.sendOutputs()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self, data):
#pkt = [0x3C,0x3C,len(data)+2]
pkt = [0x3C, 0x3C, len(data)]
for d in data:
pkt.append(d)
#pkt.append(0)
#pkt.append(0)
#chk = self.calcChecksum(pkt)
#pkt[len(pkt)-2] = (chk>>8)
#pkt[len(pkt)-1] = (chk&0xFF)
s = reduce(lambda x, y: x + chr(y), pkt, "")
try:
self.port.write(s)
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write(
"Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n"
)
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0, 0, 0]
while (1):
cnt = 0
tstart = time()
while (time() - tstart) < timeout:
if (self.port.inWaiting() > 0):
data[2] = ord(self.port.read())
break
if (time() - tstart) > timeout:
raise Exception("Read timeout")
if (data[0] == 0x3E and data[1] == 0x3E):
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2] - ord('0')):
data.append(ord(self.port.read()))
#if self.from_update:
#crc = self.calcChecksum(data)
#if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8):
# self.port.flushInput()
# raise Exception("Checksum failure")
#if self.port.inWaiting() > 0:
# self.port.flushInput()
#self.from_update=0
return data
# Send a packet
def sipSend(self, data):
if self.serialReady:
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
if self.serialReady:
while (self.port.inWaiting() > 0):
try:
recv = self.recvPacket()
except:
print 'no recv packet'
#continue
break
iters += 1
self.parseData(recv)
# 0x3s means sip packet
#if (recv[3]&0xF0)==0x30:
# self.parseSip(recv)
# 0xF0 means io packet
#elif recv[3]==0xF0:
# self.parseIO(recv)
return iters
def parseData(self):
# parse all data
stall = 0
bump = 0
sonars = [-1, -1, -1, -1, -1, -1]
#Right Sonar
sonars[4] = float(self.Ultrasonic_rear_right)
#if -0.5<self.prev_sonar[4]- sonars[4]<0.5:
# sonars[4] = self.prev_sonar4
#else:
# self.prev_sonar4=sonars[4]
#print 'Sonar 1: ', sonars[0]
#Right Front Sonar
sonars[3] = float(self.Ultrasonic_right)
#print 'Sonar 2: ', sonars[1]
#Front Sonar
sonars[2] = float(self.Ultrasonic_front)
#print 'Sonar 3: ', sonars[2]
#Left Front Sonar
sonars[1] = float(self.Ultrasonic_left)
#print 'Sonar 4: ', sonars[3
#Left Sonar
sonars[0] = float(self.Ultrasonic_rear_left)
#print 'Sonar 5: ', sonars[4]
#Back Sonar
sonars[5] = float(self.Ultrasonic_back)
#print 'Sonar 6: ', sonars[5]
self.pos_values[0], self.pos_values[1] = self.EKF.get_position()
self.pos_values[2] = self.EKF.get_heading()
self.pos_values[2] = self.pos_values[2] % (2 * pi)
if self.pos_values[2] > 2 * pi:
self.pos_values[2] -= 2 * pi
elif self.pos_values[2] < 0:
self.pos_values[2] += 2 * pi
self.odpose.set(
(self.pos_values[0], self.pos_values[1], self.pos_values[2]))
self.stalled.set((stall != 0x00) or (bump != 0x00))
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i] != -1:
storedsonars[i] = sonars[i] * METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self, data):
self.sipSend(data)
self.sipRecv()
# def cmdPulse(self):
# self.sipSendReceive([ArcosCmd.PULSE])
# def cmdPulseSend(self):
# self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self, v):
#self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0])
pass
def cmdVel(self, v):
absv = int(abs(v))
if (v >= 0):
data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8]
else:
data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
# def cmdVel2(self,l,r):
# self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)])
def cmdRvel(self, rv):
absrv = abs(rv)
if (rv >= 0):
data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8]
else:
data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self, note, duration):
data = [ArcosCmd.SAY, ArgType.ARGSTR]
for d in ("%s,%s" % (note, duration)):
data.append(ord(d))
data.append(0)
class eBot():
def __init__(self):
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0., 0.], 0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.
self.offset_counter_iteration = 100
def destroy(self):
"""
Destructor function for eBot class.
"""
self.disconnect()
self.sonarValues = None
self.port = None
self.serialReady = None
def getOpenPorts(self):
"""
Windows only function: Obtains a list of tuples with eBot-relevant port number and description.
:rtype: list
:return: devicePorts: list of port numbers and descriptions of relevant serial devices.
"""
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
#maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
#return ports
#ports = getOpenPorts()
devicePorts = []
for port in ports:
#Just because it is formatted that way...
if 'BthModem' in port[1][8:] or 'VCP' in port[1][
8:] or 'ProlificSerial' in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def open(self):
"""
Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to.
:raise Exception: No eBot found
"""
self.connect()
def connect(self):
"""
Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to.
:raise Exception: No eBot found
"""
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
#usbSerial = glob.glob('/dev/ttyUSB*')
ports = glob.glob('/dev/rfcomm*')
#print "Support for this OS is under development."
elif sys.platform == "darwin":
ports = glob.glob('/dev/tty.eBo*')
#usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
#ports = ['COM' + str(i + 1) for i in range(256)]
#EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
line = "a"
print "connecting",
for port in ports:
try:
print ".",
if (line[:2] == "eB"):
break
s = Serial(port, baudRate, timeout=5.0, writeTimeout=5.0)
s._timeout = 5.0
s._writeTimeout = 5.0
#try:
# s.open()
#except:
# continue
while (line[:2] != "eB"):
if (s.inWaiting() > 0):
line = s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if (line[:2] == "eB"):
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 5.0
self.port._writeTimeout = 5.0
self.port.flushInput()
self.port.flushOutput()
break
#s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if (connect == 0):
try:
self.port.close()
except:
pass
#sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
print "Connection Error", "No eBot found. Please reconnect and try again.",
#import ctypes # An included library with Python install.
#ctypes.windll.user32.MessageBoxA(0, "Your text", "Your title", 1)
raise Exception("No eBot found")
sleep(.01)
try:
self.port.write('<<1E')
sleep(0.4)
line = self.port.readline()
if (line != ">>1B\n" and line != ">>1B" and line != ">>\n"
and line != ">>"):
self.lostConnection()
self.port.write("<<1O")
sleep(0.4)
self.port.write("F")
sleep(0.2)
self.port.flushInput()
self.port.flushOutput()
print "connected"
self.receive_thread = Thread(target=self.recieve_background)
self.thread_flag = 1
self.receive_thread.start()
self.offset_counter = 0
except:
print "COM Error", "Robot connection lost..."
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
sys.stderr.write("Robot turned off or no longer connected.\n")
sleep(7)
self.serialReady = True
def read(self):
self.incoming = None
if self.offset_counter > 1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting() != 0:
self.incoming = self.port.readline().rstrip('\n')
self.offset_counter += 1
try:
self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \
self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \
self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \
self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \
self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";")
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
#print "eBot.read(): bad formatted data received"
#print self.incoming
pass
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = (self.Ax + self.Ax_offset)
self.Ay_offset = (self.Ay + self.Ay_offset)
self.Az_offset = (self.Az + self.Az_offset)
self.Gx_offset = (self.Gx + self.Gx_offset)
self.Gy_offset = (self.Gy + self.Gy_offset)
self.Gz_offset = (self.Gz + self.Gz_offset)
if self.offset_counter == self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset / (
self.offset_counter_iteration - 2)
self.Ay_offset = self.Ay_offset / (
self.offset_counter_iteration - 2)
self.Az_offset = self.Az_offset / (
self.offset_counter_iteration - 2)
self.Gx_offset = self.Gx_offset / (
self.offset_counter_iteration - 2)
self.Gy_offset = self.Gy_offset / (
self.offset_counter_iteration - 2)
self.Gz_offset = self.Gz_offset / (
self.offset_counter_iteration - 2)
if self.offset_counter > self.offset_counter_iteration:
sampling_time = (self.time_stamp -
self.prev_time_stamp) / 1000.
if sampling_time > 0:
if abs(self.Gz -
self.Gz_offset) > 50: # to remove the noise
self.heading = self.heading + sampling_time * (
self.Gz - self.Gz_offset
) # the integration to get the heading
self.heading_scaled = self.heading / 130.5 #128.7
self.pos_values[0],self.pos_values[1],self.pos_values[2] = \
self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time)
## self.pos_values[2] = degrees(self.pos_values[2])
## self.pos_values[2] = self.pos_values[2] % 360
## if self.pos_values[2]>180:
## self.pos_values[2]-=360
## elif self.pos_values[2]<-180:
## self.pos_values[2]+=360
return self.incoming
def recieve_background(self):
while self.thread_flag:
self.read()
sleep(0.005)
return
def close(self):
"""
Close BLE connection with eBot.
"""
self.disconnect()
#TODO: add disconnect feedback to robot
def disconnect(self):
"""
Close BLE connection with eBot.
"""
#self.receive_thread.
sleep(0.05)
self.thread_flag = 0
sleep(0.05)
if self.serialReady:
try:
self.port.close()
print "Successful", "eBot successfully disconnected."
except:
self.lostConnection()
def sonars(self):
"""
Retrieves and returns all six ultrasonic sensor values from the eBot in meters.
:rtype: list
:return: sonarValues
"""
self.sonarValues[3] = float(self.Ultrasonic_rear_right) / 1000
self.sonarValues[4] = float(self.Ultrasonic_right) / 1000
self.sonarValues[2] = float(self.Ultrasonic_front) / 1000
self.sonarValues[1] = float(self.Ultrasonic_left) / 1000
self.sonarValues[0] = float(self.Ultrasonic_rear_left) / 1000
self.sonarValues[5] = float(self.Ultrasonic_back) / 1000
return self.sonarValues
def calibration_values(self):
"""
Retrieves and returns the calibration values of the eBot.
:rtype: list
:return: all_Values (calibration values)
"""
if self.serialReady:
try:
self.port.write("2C")
except:
self.lostConnection()
line = self.port.readline()
values = line.split(";")
while len(values) < 10:
if self.serialReady:
try:
self.port.write("2C")
except:
self.lostConnection()
line = self.port.readline()
values = line.split(";")
self.all_Values[0] = float(values[0])
self.all_Values[1] = float(values[1])
self.all_Values[2] = float(values[2])
self.all_Values[3] = float(values[3])
self.all_Values[8] = float(values[4]) / 1000
self.all_Values[7] = float(values[5]) / 1000
self.all_Values[6] = float(values[6]) / 1000
self.all_Values[5] = float(values[7]) / 1000
self.all_Values[4] = float(values[8]) / 1000
self.all_Values[9] = float(values[9]) / 1000
return self.all_Values
def halt(self):
"""
Halts the eBot, turns the motors and LEDs off.
"""
if self.serialReady:
try:
self.port.write("2H")
except:
self.lostConnection()
sleep(0.05)
def led(self, bool):
"""
Controls the state of the LED on the eBot.
:param bool: Defines whether the LED should turn ON (1) or OFF (0)
"""
if (bool == 1):
self.led_on()
elif (bool == 0):
self.led_off()
else:
self.led_off()
sleep(0.05)
def led_on(self):
"""
Turns the LED on the eBot ON.
"""
if self.serialReady:
try:
self.port.write("2L")
except:
self.lostConnection()
sleep(0.05)
def led_off(self):
"""
Turns the LED on the eBot OFF.
"""
if self.serialReady:
try:
self.port.write("2l")
except:
self.lostConnection()
sleep(0.05)
def light(self):
"""
Retrieves and returns a list of tuples with the light index. 0 index is front and 1st index is top LDR readings.
:rtype: list
:return: ldrvalue: LDR Readings
"""
self.ldrvalue[0] = float(self.LDR_front)
self.ldrvalue[1] = float(self.LDR_top)
return self.ldrvalue
#Double check true vs. false
def obstacle(self):
"""
Tells whether or not there is an obstacle less than 250 mm away from the front of the eBot.
:rtype: bool
:return: True if obstacle exists
"""
if self.Ultrasonic_front > 250:
return False
else:
return True
#TODO: implement x, y, z returns and a seperate odometry function
def acceleration(self):
"""
Retrieves and returns accelerometer values; absolute values of X,Y and theta coordinates of robot with reference
to starting position.
:rtype: list
:return: acc_values: Accelerometer values
"""
self.acc_values[0] = float(self.Ax - self.Ax_offset)
self.acc_values[1] = float(self.Ay - self.Ay_offset)
self.acc_values[2] = float(self.Az - self.Az_offset)
self.acc_values[3] = float(self.Gx - self.Gx_offset)
self.acc_values[4] = float(self.Gy - self.Gy_offset)
self.acc_values[5] = float(self.Gz - self.Gz_offset)
return self.acc_values
def odometry(self):
"""
Retrieves and returns the odometry values of the eBot as a Pose object with respect to the robot initial position.
Pose.x: x coordinate in meters
Pose.y: y coordinate in meters
Pose.theta: rotation in radians
:rtype: Pose object
:return: Pose: Represent the x, y, theta pose of an object in 2D space
"""
self.pos_values[0], self.pos_values[1] = self.EKF.get_position()
self.pos_values[2] = self.EKF.get_heading()
self.pos_values[2] = self.pos_values[2] % (2 * pi)
if self.pos_values[2] > 2 * pi:
self.pos_values[2] -= 2 * pi
elif self.pos_values[2] < 0:
self.pos_values[2] += 2 * pi
pose = (self.pos_values[0], self.pos_values[1], self.pos_values[2])
return util.valueListToPose(pose)
#TODO: implement temperature feedback from MPU6050 IC
def temperature(self):
"""
Retrieves and returns temperature reading from the eBot.
:rtype: int
:return: Temperature value.
"""
return int(self.tempreture_sensor)
def power(self):
"""
:return:
"""
self.p_value[0] = float(self.voltage)
self.p_value[1] = float(self.current)
return self.p_value
def imperial_march(self):
"""
"""
if self.serialReady:
try:
self.port.write("2b")
except:
self.lostConnection()
def buzzer(self, btime, bfreq):
"""
Plays the buzzer for given time at given frequency.
:param btime: Time in Seconds
:param bfreq: Frequency in Hertz
"""
buzzer_time = int(btime)
buzzer_frequency = int(bfreq)
bt1 = str(buzzer_time)
bf1 = str(buzzer_frequency)
str_len = len(bt1) + len(bf1) + 2
str_len = str_len + 48
myvalue = chr(str_len) + 'B' + bt1 + ';' + bf1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
sleep(buzzer_time / 1000)
def port_name(self):
"""
Returns port name of currently connected eBot.
:return: port: Port name
"""
return self.port
def port_close(self):
"""
Closes the COM port that corresponds to the eBot object.
:raise Exception: Could not close COM port
"""
try:
self.port.close()
except:
self.serialReady = False
raise Exception("Could not close COM port.")
#TODO: Add com port argument functionality
def port_open(self):
"""
Still under development, currently just calls connect
"""
self.connect()
def wheels(self, LS, RS):
"""
Controls the speed of the wheels of the robot according to the specified values
:param LS: Speed of left motor
:param RS: Speed of right motor
"""
if LS > 1:
LS = 1
elif LS < -1:
LS = -1
if RS > 1:
RS = 1
elif RS < -1:
RS = -1
Left_speed = int((LS + 2) * 100)
Right_speed = int((RS + 2) * 100)
LS1 = str(Left_speed)
RS1 = str(Right_speed)
myvalue = '8' + 'w' + LS1 + ';' + RS1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
# class ebot_f:
# def __init__(self):
sleep(0.05)
def wheel_calibrate(self, LS, RS):
"""
Controls the speed of the wheels of the robot according to the specified values
:param LS: Speed of left motor
:param RS: Speed of right motor
"""
if LS > 9999:
LS = 9999
elif LS < 1:
LS = 1
if RS > 9999:
RS = 9999
elif RS < 1:
RS = 1
LS1 = str(LS).zfill(4)
RS1 = str(RS).zfill(4)
myvalue = ':' + 'c' + LS1 + ';' + RS1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
# class ebot_f:
# def __init__(self)
def lostConnection(self):
"""
Handler for the case that the computer loses connection with the eBot.
:raise Exception: Robot Connection Lost
"""
try:
self.port.close()
except:
pass
self.serialReady = False
print "COM Error", "Robot connection lost..."
raise Exception("Robot Connection Lost")
class eBot():
def __init__(self):
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0.,0.],0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading =0.
self.offset_counter_iteration = 100
def destroy(self):
"""
Destructor function for eBot class.
"""
self.disconnect()
self.sonarValues = None
self.port = None
self.serialReady = None
def getOpenPorts(self):
"""
Windows only function: Obtains a list of tuples with eBot-relevant port number and description.
:rtype: list
:return: devicePorts: list of port numbers and descriptions of relevant serial devices.
"""
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
#maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
#return ports
#ports = getOpenPorts()
devicePorts = []
for port in ports:
#Just because it is formatted that way...
if 'BthModem' in port[1][8:] or 'VCP' in port[1][8:] or 'ProlificSerial' in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def open(self):
"""
Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to.
:raise Exception: No eBot found
"""
self.connect()
def connect(self):
"""
Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to.
:raise Exception: No eBot found
"""
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
#usbSerial = glob.glob('/dev/ttyUSB*')
ports = glob.glob('/dev/tty.eBo*')
#print "Support for this OS is under development."
elif sys.platform == "darwin":
ports = glob.glob('/dev/tty.eBo*')
#usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
#ports = ['COM' + str(i + 1) for i in range(256)]
#EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
line = "a"
print "connecting",
for port in ports:
try:
print ".",
if (line[:2] == "eB"):
break
s = Serial(port, baudRate, timeout=5.0, writeTimeout=5.0)
s._timeout = 5.0
s._writeTimeout = 5.0
#try:
# s.open()
#except:
# continue
while (line[:2] != "eB"):
if (s.inWaiting()>0):
line=s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if (line[:2] == "eB"):
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 5.0
self.port._writeTimeout = 5.0
self.port.flushInput()
self.port.flushOutput()
break
#s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if (connect == 0):
try:
self.port.close()
except:
pass
#sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
print "Connection Error", "No eBot found. Please reconnect and try again.",
#import ctypes # An included library with Python install.
#ctypes.windll.user32.MessageBoxA(0, "Your text", "Your title", 1)
raise Exception("No eBot found")
sleep(.01)
try:
self.port.write('<<1E')
sleep(0.4)
line = self.port.readline()
if (line != ">>1B\n" and line != ">>1B" and line != ">>\n" and line != ">>"):
self.lostConnection()
self.port.write("<<1O")
sleep(0.4)
self.port.write("F")
sleep(0.2)
self.port.flushInput()
self.port.flushOutput()
print "connected"
self.receive_thread = Thread(target= self.recieve_background)
self.thread_flag =1
self.receive_thread.start()
self.offset_counter =0
except:
print "COM Error", "Robot connection lost..."
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
sys.stderr.write("Robot turned off or no longer connected.\n")
sleep(7)
self.serialReady = True
def read(self):
self.incoming=None
if self.offset_counter>1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting()!=0:
self.incoming=self.port.readline().rstrip('\n')
self.offset_counter += 1
try:
self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \
self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \
self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \
self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \
self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";")
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
print "eBot.read(): bad formatted data received"
print self.incoming
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if self.offset_counter>2 and self.offset_counter<self.offset_counter_iteration:# we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = (self.Ax+self.Ax_offset)
self.Ay_offset = (self.Ay+self.Ay_offset)
self.Az_offset = (self.Az+self.Az_offset)
self.Gx_offset = (self.Gx+self.Gx_offset)
self.Gy_offset = (self.Gy+self.Gy_offset)
self.Gz_offset = (self.Gz+self.Gz_offset)
if self.offset_counter == self.offset_counter_iteration:# we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset/(self.offset_counter_iteration-2)
self.Ay_offset = self.Ay_offset/(self.offset_counter_iteration-2)
self.Az_offset = self.Az_offset/(self.offset_counter_iteration-2)
self.Gx_offset = self.Gx_offset/(self.offset_counter_iteration-2)
self.Gy_offset = self.Gy_offset/(self.offset_counter_iteration-2)
self.Gz_offset = self.Gz_offset/(self.offset_counter_iteration-2)
if self.offset_counter>self.offset_counter_iteration:
sampling_time = (self.time_stamp-self.prev_time_stamp)/1000.
if sampling_time>0:
if abs(self.Gz-self.Gz_offset)>50: # to remove the noise
self.heading = self.heading+sampling_time*(self.Gz-self.Gz_offset) # the integration to get the heading
self.heading_scaled = self.heading/130.5 #128.7
self.pos_values[0],self.pos_values[1],self.pos_values[2] = \
self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time)
self.pos_values[2] = degrees(self.pos_values[2])
self.pos_values[2] = self.pos_values[2] % 360
if self.pos_values[2]>180:
self.pos_values[2]-=360
elif self.pos_values[2]<-180:
self.pos_values[2]+=360
return self.incoming
def recieve_background(self):
while self.thread_flag:
self.read()
sleep(0.005)
return
def close(self):
"""
Close BLE connection with eBot.
"""
self.disconnect()
#TODO: add disconnect feedback to robot
def disconnect(self):
"""
Close BLE connection with eBot.
"""
#self.receive_thread.
sleep(0.05)
self.thread_flag = 0
sleep(0.05)
if self.serialReady:
try:
self.port.close()
print "Successful", "eBot successfully disconnected."
except:
self.lostConnection()
def robot_uS(self):
"""
Retrieves and returns all six ultrasonic sensor values from the eBot in meters.
:rtype: list
:return: sonarValues
"""
self.sonarValues[4] = float(self.Ultrasonic_rear_right) / 1000
self.sonarValues[3] = float(self.Ultrasonic_right) / 1000
self.sonarValues[2] = float(self.Ultrasonic_front) / 1000
self.sonarValues[1] = float(self.Ultrasonic_left) / 1000
self.sonarValues[0] = float(self.Ultrasonic_rear_left) / 1000
self.sonarValues[5] = float(self.Ultrasonic_back) / 1000
return self.sonarValues
def calibration_values(self):
"""
Retrieves and returns the calibration values of the eBot.
:rtype: list
:return: all_Values (calibration values)
"""
if self.serialReady:
try:
self.port.write("2C")
except:
self.lostConnection()
line = self.port.readline()
values = line.split(";")
while len(values) < 10:
if self.serialReady:
try:
self.port.write("2C")
except:
self.lostConnection()
line = self.port.readline()
values = line.split(";")
self.all_Values[0] = float(values[0])
self.all_Values[1] = float(values[1])
self.all_Values[2] = float(values[2])
self.all_Values[3] = float(values[3])
self.all_Values[8] = float(values[4]) / 1000
self.all_Values[7] = float(values[5]) / 1000
self.all_Values[6] = float(values[6]) / 1000
self.all_Values[5] = float(values[7]) / 1000
self.all_Values[4] = float(values[8]) / 1000
self.all_Values[9] = float(values[9]) / 1000
return self.all_Values
def halt(self):
"""
Halts the eBot, turns the motors and LEDs off.
"""
if self.serialReady:
try:
self.port.write("2H")
except:
self.lostConnection()
sleep(0.05)
def led(self, bool):
"""
Controls the state of the LED on the eBot.
:param bool: Defines whether the LED should turn ON (1) or OFF (0)
"""
if (bool == 1):
self.led_on()
elif (bool == 0):
self.led_off()
else:
self.led_off()
sleep(0.05)
def led_on(self):
"""
Turns the LED on the eBot ON.
"""
if self.serialReady:
try:
self.port.write("2L")
except:
self.lostConnection()
sleep(0.05)
def led_off(self):
"""
Turns the LED on the eBot OFF.
"""
if self.serialReady:
try:
self.port.write("2l")
except:
self.lostConnection()
sleep(0.05)
def light(self):
"""
Retrieves and returns a list of tuples with the light index. 0 index is front and 1st index is top LDR readings.
:rtype: list
:return: ldrvalue: LDR Readings
"""
self.ldrvalue[0] = float(self.LDR_front)
self.ldrvalue[1] = float(self.LDR_top)
return self.ldrvalue
#Double check true vs. false
def obstacle(self):
"""
Tells whether or not there is an obstacle less than 250 mm away from the front of the eBot.
:rtype: bool
:return: True if obstacle exists
"""
if self.Ultrasonic_front>250:
return False
else:
return True
#TODO: implement x, y, z returns and a seperate odometry function
def acceleration(self):
"""
Retrieves and returns accelerometer values; absolute values of X,Y and theta coordinates of robot with reference
to starting position.
:rtype: list
:return: acc_values: Accelerometer values
"""
self.acc_values[0] = float(self.Ax-self.Ax_offset)
self.acc_values[1] = float(self.Ay-self.Ay_offset)
self.acc_values[2] = float(self.Az-self.Az_offset)
self.acc_values[3] = float(self.Gx-self.Gx_offset)
self.acc_values[4] = float(self.Gy-self.Gy_offset)
self.acc_values[5] = float(self.Gz-self.Gz_offset)
return self.acc_values
def position(self):
"""
Retrieves and returns position values of the eBot.
:rtype: list
:return: pos_values: X,Y,Z position values
"""
#self.pos_values[0] , self.pos_values[1] = self.EKF.get_position()
#self.pos_values[2] = self.EKF.get_heading()
#self.pos_values[2] = 0
return self.pos_values[0],self.pos_values[1],self.pos_values[2]
#TODO: implement temperature feedback from MPU6050 IC
def temperature(self):
"""
Retrieves and returns temperature reading from the eBot.
:rtype: int
:return: Temperature value.
"""
return int(self.tempreture_sensor)
def power(self):
"""
:return:
"""
self.p_value[0] = float(self.voltage)
self.p_value[1] = float(self.current)
return self.p_value
def imperial_march(self):
"""
"""
if self.serialReady:
try:
self.port.write("2b")
except:
self.lostConnection()
def buzzer(self, btime, bfreq):
"""
Plays the buzzer for given time at given frequency.
:param btime: Time in Seconds
:param bfreq: Frequency in Hertz
"""
buzzer_time = int(btime)
buzzer_frequency = int(bfreq)
bt1 = str(buzzer_time)
bf1 = str(buzzer_frequency)
str_len = len(bt1) + len(bf1)+2
str_len =str_len + 48
myvalue = chr(str_len) + 'B' + bt1 + ';' + bf1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
sleep(buzzer_time/1000)
def port_name(self):
"""
Returns port name of currently connected eBot.
:return: port: Port name
"""
return self.port
def port_close(self):
"""
Closes the COM port that corresponds to the eBot object.
:raise Exception: Could not close COM port
"""
try:
self.port.close()
except:
self.serialReady = False
raise Exception("Could not close COM port.")
#TODO: Add com port argument functionality
def port_open(self):
"""
Still under development, currently just calls connect
"""
self.connect()
def wheels(self, LS, RS):
"""
Controls the speed of the wheels of the robot according to the specified values
:param LS: Speed of left motor
:param RS: Speed of right motor
"""
if LS > 1:
LS = 1
elif LS < -1:
LS = -1
if RS > 1:
RS = 1
elif RS < -1:
RS = -1
Left_speed = int((LS + 2) * 100)
Right_speed = int((RS + 2) * 100)
LS1 = str(Left_speed)
RS1 = str(Right_speed)
myvalue = '8' + 'w' + LS1 + ';' + RS1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
# class ebot_f:
# def __init__(self):
sleep(0.05)
def wheel_calibrate(self, LS, RS):
"""
Controls the speed of the wheels of the robot according to the specified values
:param LS: Speed of left motor
:param RS: Speed of right motor
"""
if LS > 9999:
LS = 9999
elif LS < 1:
LS = 1
if RS > 9999:
RS = 9999
elif RS < 1:
RS = 1
LS1 = str(LS).zfill(4)
RS1 = str(RS).zfill(4)
myvalue = ':' + 'c' + LS1 + ';' + RS1
if self.serialReady:
try:
self.port.write(myvalue)
except:
self.lostConnection()
# class ebot_f:
# def __init__(self)
def lostConnection(self):
"""
Handler for the case that the computer loses connection with the eBot.
:raise Exception: Robot Connection Lost
"""
try:
self.port.close()
except:
pass
self.serialReady = False
print "COM Error", "Robot connection lost..."
raise Exception("Robot Connection Lost")
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0.0, 0.0], 0.0)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.0
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] # Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0]) # Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities,
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get,
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write("- Check if eBot is on, paired and connected. If not, power up and try again. \n")
# sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet((self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
class eBot:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0.0, 0.0], 0.0)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.0
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] # Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0]) # Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities,
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get,
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write("- Check if eBot is on, paired and connected. If not, power up and try again. \n")
# sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet((self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if self.serialReady:
# self.cmdEnable(0)
self.stopmoving()
self.sendOutputs()
self.sendPacket([CMD.CLOSE])
# self.flushSerialPort()
try:
self.port.close()
except:
pass
app.soar.removeFlowTriplet((self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
# if self.serialReady:
# self.cmdEnable(0)
# self.stopmoving()
# self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
from soar.serial import Serial
self.connect(Serial)
self.open()
# self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
def getOpenPorts(self):
"""
This Function Returns a list of tuples with the port number and its description. Used for Windows only
"""
path = "HARDWARE\\DEVICEMAP\\SERIALCOMM"
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
# maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
# return ports
# ports = getOpenPorts()
devicePorts = []
for port in ports:
# Just because it is formatted that way...
if "BthModem" in port[1][8:] or "VCP" in port[1][8:] or "ProlificSerial" in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def connect(self, Serial):
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
# usbSerial = glob.glob('/dev/ttyUSB*')
print "Support for this OS is under development."
elif sys.platform == "darwin":
ports = glob.glob("/dev/tty.eBot*")
# usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
# ports = ['COM' + str(i + 1) for i in range(256)]
# EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
for port in ports:
try:
s = Serial(port, baudRate, timeout=1, writeTimeout=1)
s._timeout = 1.0
s._writeTimeout = 1.0
# try:
# s.open()
# except:
# continue
line = "aaaa"
while line[:2] != "eB":
if s.inWaiting() > 0:
line = s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if line[:2] == "eB":
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 1.0
self.port._writeTimeout = 1.0
self.port.flushInput()
self.port.flushOutput()
if line[:2] == "eB":
break
# s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if connect == 0:
try:
self.port.close()
except:
pass
# sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
# Open connection to robot
def open(self):
numAttempts = 3
timeout = 5.0
startt = time()
while self.connectionState != STATE_READY:
if self.connectionState == STATE_NO_SYNC:
self.sendPacket([CMD_SYNC_SEND.CHECK1])
elif self.connectionState == STATE_SYNC:
self.sendPacket([CMD_SYNC_SEND.CHECK2])
self.connectionState = STATE_READY
break
# elif (self.connectionState == STATE_READY):
# pass
try:
pkt = self.recvPacket()
except:
if self.connectionState == STATE_NO_SYNC and numAttempts >= 0:
numAttempts -= 1
else:
self.port.close()
sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
continue
if pkt[3] == CMD_SYNC_RECV.CHECK1:
self.connectionState = STATE_SYNC
# elif (pkt[3]==CMD_SYNC_RECV.CHECK2):
# self.connectionState = STATE_READY
if (time() - startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
sleep(0.01)
try:
self.port.write("A")
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# changed = [0, 0, 0, 0, 0, 0]
# while 0 in changed:
# #self.cmdPulse()
# self.sipRecv()
# self.storedsonars.get()
# changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))]
self.serialReady = True
# turn on io packets
# self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0])
def getPose(self):
return self.odpose.get()
def initGlobals(self):
self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write("Trying to set maximum translational velocity too high for real robot\n")
if maxRotVel > 1.75:
sys.stderr.write("Trying to set maximum rotational velocity too high for real robot\n")
def enableTeleportation(self, prob, pose):
sys.stderr.write("Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if not self.asynchronous:
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v, w)
sleep(dt)
self.motorOutput(0, 0)
def sendOutputs(self):
data = [CMD.RECV]
v = int(self.trans.get() * 1000.0)
if v >= 0:
# data = [abs(v) & 0xFF, v >> 8, 0x01]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x01)
else:
# data = [abs(v) & 0xFF, v >> 8, 0x10]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x10)
rv = int(self.rot.get() * 220 / pi) # 180*1.5
if rv >= 0:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x01)
else:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x10)
self.sipSend(data)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get() * 1000.0))
self.cmdRvel(int(self.rot.get() * 180 / pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [
ArcosCmd.DIGOUT,
ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get() * 25.5))) & 0xFF),
0xFF,
]
self.sipSend(data)
# update sonars, odometry and stalled
def read(self):
self.incoming = None
if self.offset_counter > 1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting() != 0:
self.incoming = self.port.readline().rstrip("\n")
self.offset_counter += 1
try:
self.time_stamp, self.Ax, self.Ay, self.Az, self.Gx, self.Gy, self.Gz, self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front, self.Ultrasonic_left, self.Ultrasonic_rear_left, self.Ultrasonic_back, self.encoder_right, self.encoder_left, self.LDR_top, self.LDR_front, self.tempreture_sensor, self.voltage, self.current = self.incoming.split(
";"
)
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
print "eBot.read(): bad formatted data received"
print self.incoming
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if (
self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration
): # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax + self.Ax_offset
self.Ay_offset = self.Ay + self.Ay_offset
self.Az_offset = self.Az + self.Az_offset
self.Gx_offset = self.Gx + self.Gx_offset
self.Gy_offset = self.Gy + self.Gy_offset
self.Gz_offset = self.Gz + self.Gz_offset
if self.offset_counter == self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset / self.offset_counter_iteration
self.Ay_offset = self.Ay_offset / self.offset_counter_iteration
self.Az_offset = self.Az_offset / self.offset_counter_iteration
self.Gx_offset = self.Gx_offset / self.offset_counter_iteration
self.Gy_offset = self.Gy_offset / self.offset_counter_iteration
self.Gz_offset = self.Gz_offset / self.offset_counter_iteration
if self.offset_counter > self.offset_counter_iteration:
sampling_time = (self.time_stamp - self.prev_time_stamp) / 1000.0
if sampling_time > 0:
if abs(self.Gz - self.Gz_offset) > 50: # to remove the noise
self.heading = self.heading + sampling_time * (
self.Gz - self.Gz_offset
) # the integration to get the heading
self.heading_scaled = self.heading / 130.5 # 128.7
self.heading_scaled = self.heading_scaled
self.pos_values[0], self.pos_values[1], self.pos_values[2] = self.EKF.update_state(
[self.heading_scaled * pi / 180.0, self.encoder_right / 1000.0, self.encoder_left / 1000.0],
sampling_time,
)
return self.incoming
def update(self, dummyparameter=0):
if self.serialReady:
# self.sendPacket([CMD.RECV])
# self.sendMotorCmd()
# self.sendDigitalOutCmd()
# temp = self.port.inWaiting()
self.sendOutputs()
# sleep(.005)
# self.sendPacket([CMD.SEND])
self.read()
sleep(0.005)
self.parseData()
self.from_update = 1
# if (self.sipRecv() > 0):
# self.zero_recv_cnt = 0
# else:
# self.zero_recv_cnt += 1
# if (self.zero_recv_cnt > 400):
# self.serialReady = False
# app.soar.closeEBot()
# sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self, data):
c = 0
i = 3
n = data[2] - ord("0") - 2
while n > 1:
c += (data[i] << 8) | (data[i + 1])
c = c & 0xFFFF
n -= 2
i += 2
if n > 0:
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if self.asynchronous:
pass
# self.stopSerialThread()
else:
# self.sendMotorCmd()
self.sendOutputs()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self, data):
# pkt = [0x3C,0x3C,len(data)+2]
pkt = [0x3C, 0x3C, len(data)]
for d in data:
pkt.append(d)
# pkt.append(0)
# pkt.append(0)
# chk = self.calcChecksum(pkt)
# pkt[len(pkt)-2] = (chk>>8)
# pkt[len(pkt)-1] = (chk&0xFF)
s = reduce(lambda x, y: x + chr(y), pkt, "")
try:
self.port.write(s)
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0, 0, 0]
while 1:
cnt = 0
tstart = time()
while (time() - tstart) < timeout:
if self.port.inWaiting() > 0:
data[2] = ord(self.port.read())
break
if (time() - tstart) > timeout:
raise Exception("Read timeout")
if data[0] == 0x3E and data[1] == 0x3E:
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2] - ord("0")):
data.append(ord(self.port.read()))
# if self.from_update:
# crc = self.calcChecksum(data)
# if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8):
# self.port.flushInput()
# raise Exception("Checksum failure")
# if self.port.inWaiting() > 0:
# self.port.flushInput()
# self.from_update=0
return data
# Send a packet
def sipSend(self, data):
if self.serialReady:
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
if self.serialReady:
while self.port.inWaiting() > 0:
try:
recv = self.recvPacket()
except:
print "no recv packet"
# continue
break
iters += 1
self.parseData(recv)
# 0x3s means sip packet
# if (recv[3]&0xF0)==0x30:
# self.parseSip(recv)
# 0xF0 means io packet
# elif recv[3]==0xF0:
# self.parseIO(recv)
return iters
def parseData(self):
# parse all data
stall = 0
bump = 0
sonars = [-1, -1, -1, -1, -1, -1]
# Right Sonar
sonars[4] = float(self.Ultrasonic_rear_right)
# if -0.5<self.prev_sonar[4]- sonars[4]<0.5:
# sonars[4] = self.prev_sonar4
# else:
# self.prev_sonar4=sonars[4]
# print 'Sonar 1: ', sonars[0]
# Right Front Sonar
sonars[3] = float(self.Ultrasonic_right)
# print 'Sonar 2: ', sonars[1]
# Front Sonar
sonars[2] = float(self.Ultrasonic_front)
# print 'Sonar 3: ', sonars[2]
# Left Front Sonar
sonars[1] = float(self.Ultrasonic_left)
# print 'Sonar 4: ', sonars[3
# Left Sonar
sonars[0] = float(self.Ultrasonic_rear_left)
# print 'Sonar 5: ', sonars[4]
# Back Sonar
sonars[5] = float(self.Ultrasonic_back)
# print 'Sonar 6: ', sonars[5]
self.pos_values[0], self.pos_values[1] = self.EKF.get_position()
self.pos_values[2] = self.EKF.get_heading()
self.pos_values[2] = self.pos_values[2] % (2 * pi)
if self.pos_values[2] > 2 * pi:
self.pos_values[2] -= 2 * pi
elif self.pos_values[2] < 0:
self.pos_values[2] += 2 * pi
self.odpose.set((self.pos_values[0], self.pos_values[1], self.pos_values[2]))
self.stalled.set((stall != 0x00) or (bump != 0x00))
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i] != -1:
storedsonars[i] = sonars[i] * METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self, data):
self.sipSend(data)
self.sipRecv()
# def cmdPulse(self):
# self.sipSendReceive([ArcosCmd.PULSE])
# def cmdPulseSend(self):
# self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self, v):
# self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0])
pass
def cmdVel(self, v):
absv = int(abs(v))
if v >= 0:
data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8]
else:
data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
# def cmdVel2(self,l,r):
# self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)])
def cmdRvel(self, rv):
absrv = abs(rv)
if rv >= 0:
data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8]
else:
data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self, note, duration):
data = [ArcosCmd.SAY, ArgType.ARGSTR]
for d in "%s,%s" % (note, duration):
data.append(ord(d))
data.append(0)