Exemplo n.º 1
0
 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
Exemplo n.º 2
0
 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
Exemplo n.º 3
0
    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)
Exemplo n.º 4
0
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)
Exemplo n.º 5
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")
Exemplo n.º 6
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/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")
Exemplo n.º 7
0
    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)
Exemplo n.º 8
0
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)