Пример #1
0
def sendStopCommand(reason):

    # command 4
    msg = b'4' + b'\n'
    cartGlobal.log(f"Send stop, reason: {reason}")
    ser.write(msg)
    cartGlobal.setCartMoving(False)
Пример #2
0
    def moveCart(self):

        cartSpeed=int(self.sbSpeed.get())
        distance=int(self.sbDist.get())
        cartGlobal.log(f"moveCart, speed: {cartSpeed}, distance: {distance}")
        arduino.sendMoveCommand(self.direction, cartSpeed, distance)
        self.lblCommandValue.configure(text="Move")
        #self.lblMove.configure(text=str(speed))
        self.w.update_idletasks()
Пример #3
0
def estimateUntrackedMoveInPix(untrackedMoveTime):

    calculatedDxPix = _lastDxPixPerSec * untrackedMoveTime
    calculatedDyPix = _lastDyPixPerSec * untrackedMoveTime
    #log(f"calculated dx,dy, _lastDxPixPerSec: {_lastDxPixPerSec:.3f}, _lastDyPixPerSec: {_lastDyPixPerSec:.3f}, _lastTrackedMoveDuration: {_lastTrackedMoveDuration:.3f}")
    distancePix = np.hypot(calculatedDxPix, calculatedDyPix)
    speed = distancePix / untrackedMoveTime
    cartGlobal.log(f"untrackedMove duration: {untrackedMoveTime:.4f}, distancePix: {distancePix:.2f}, speed [pix/s]: {speed:.2f}")
    return calculatedDxPix, calculatedDyPix
Пример #4
0
    def startArduino(self):

        cartGlobal.log("try to open serial connection to arduino on cart (COM5)")
        arduino.initSerial("COM5")
        self.lblInfo.configure(text = "arduino connected, waiting for cart ready message", bg="white smoke", fg="orange")
        self.btnArduino.configure(state = "disabled")

        self.w.update_idletasks()
        self.w.after(2000, self.checkArduinoReady)
Пример #5
0
def trackCartMovements():

    global cap, camConnected, orb, bf

    cartGlobal.log("trackCartMovements started")

    if not camConnected:

        cap = cv2.VideoCapture(1)
        cap.set(cv2.CAP_PROP_FPS, 120)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
        cartGlobal.log(f"cam params fps: {cap.get(cv2.CAP_PROP_FPS)}, w: {cap.get(cv2.CAP_PROP_FRAME_WIDTH)}, h: {cap.get(cv2.CAP_PROP_FRAME_HEIGHT)}")

        time.sleep(0.5)         # allow cam to adjust to light

        # remove tracking jpg's from previous run
        for imgFile in glob.glob("C:/cartControl/floorImages/*.jpg"):
            os.remove(imgFile)

        for _ in range(2):      # first frame is black
            ret, img = cap.read()

        if ret:
            cartGlobal.log("cam connected")
            cv2.imshow("initial frame", img)
            cv2.waitKey(500)
            cartGlobal.saveImg(img, 0)
            cv2.destroyAllWindows()

            camConnected = True

            # Initiate ORB detector (fastThreshold has highest impact on number of candidate points seen)
            orb = cv2.ORB_create(nfeatures=250, edgeThreshold=8, patchSize=18, fastThreshold=6,  scoreType=cv2.ORB_FAST_SCORE)  # 2.5.2018
    
            # create BFMatcher object
            bf = cv2.BFMatcher()
    
        else:
            cartGlobal.log("no cam found")
            raise SystemExit()

    while True:

        if cartGlobal.isCartMoving():

            doOdometry()

            cartGlobal.log(f"odometry, cart stopped, moveDistance: {cartGlobal.currMoveDistance():.0f}")
Пример #6
0
    def exposed_heartBeat(self):
        global lastMessage

        # watchdog for incoming commands from navManager
        # stop cart if we do not get regular messages
        currTime = int(round(time.time() * 1000))
        cartGlobal.log(currTime - lastMessage)
        if currTime - lastMessage > TIMEOUT:
            if cartGlobal.isCartMoving:
                arduino.sendStopCommand("missing heartbeat from navManager")
        else:
            arduino.sendHeartbeat()
            cartGlobal.log("arduino Heartbeat sent")

        lastMessage = int(round(time.time() * 1000))
Пример #7
0
    def exposed_startListening(self, logIP, logPort):
        '''
        a first call from navManager
        '''
        if not cartGlobal.standAloneMode:
            try:
                cartGlobal.navManager = rpyc.connect(logIP, logPort)
                cartGlobal.navManager.root.connectionStatus("cart", True)
                cartGlobal.log("logger connection established")

            except:
                print("cart - could not establish connection to logger")
                raise SystemExit()

        if not cartGlobal.standAloneMode:
            cartGlobal.navManager.root.processStatus("cart", True)
Пример #8
0
def sendRotateCommand(relAngle, speed):

    ROTATION_SPEED = 200

    cartGlobal.setMovementBlocked(False)
    cartGlobal.setTargetOrientation(relAngle)
    cartGlobal.setRequestedCartSpeed(speed)

    # command 2
    # msg = f"b,{cartGlobal.SPEED_FACTOR:07.4f},{cartGlobal.SPEED_EXPONENT:07.4f},{cartGlobal.SPEED_FACTOR_SIDEWAY:05.2f}"
    # ser.write(bytes(msg + '\n', 'ascii'))

    if relAngle < 0:  # rotate counterclock
        msg = f"2,{abs(relAngle):03.0f},{speed:03.0f}"
        cartGlobal.log(f"Send rotate counterclock {msg}")
        ser.write(bytes(msg + '\n', 'ascii'))

    # command 3
    if relAngle > 0:
        msg = f"3,{abs(relAngle):03.0f},{speed:.03f}"
        cartGlobal.log(f"Send rotate clockwise {msg}")
        ser.write(bytes(msg + '\n', 'ascii'))
Пример #9
0
def checkDecelerate():

    global _lastCheckDecelerate

    _lastCheckDecelerate = time.time()

    if cartGlobal.isCartMoving():

        cartSpeed = cartGlobal.getRequestedCartSpeed()
        remainingDist = cartGlobal.getRemainingDistance()
        #cartGlobal.log(f"check decelerate move, remainingDist: {int(remainingDist)}, currSpeed: {cartSpeed}")
        if cartSpeed * remainingDist / 80 < cartSpeed:
            cartSpeed -= 30
            cartGlobal.log(f"decelerate, remainingDist: {remainingDist:.0f}, new speed: {cartSpeed:.0f}")
            arduino.sendSpeedCommand(cartSpeed)    


    if cartGlobal.isCartRotating():

        #cartGlobal.log(f"check decelerate rotation, restRot: {restRot}, currSpeed: {cartSpeed}")
        if cartSpeed * cartGlobal.getRemainingRotation() / 5 < cartGlobal.getRequestedCartSpeed():
            cartSpeed -= 20
            arduino.sendSpeedCommand(cartSpeed)    
Пример #10
0
def initSerial(comPort):

    global ser

    cartGlobal.arduinoStatus = 0

    while True:
        try:
            ser = serial.Serial(comPort)
            # try to reset the arduino
            ser.setDTR(False)
            time.sleep(0.1)
            ser.setDTR(True)
            ser.baudrate = 115200
            ser.writeTimeout = 0
            cartGlobal.log("Serial comm to arduino established")
            return

        except:
            cartGlobal.log(
                f"exception on serial connect with {comPort} {sys.exc_info()[0]}"
            )
            time.sleep(3)
Пример #11
0
def sendMoveCommand(direction, speed, distanceMm):
    '''
    send move command to cart
    distance calculation is based on time travelled and requested speed
    if final orientation <> initial orientation send a rotate command at the end to end up in same orientation
    '''
    # conmmand 1,<dir>,<speed>,<distance>,<max duration>
    # e.g. forward, speed=180, distance=100 mm, max duration=5 s: 1,0,180,0100,0005
    distanceMmLimited = min(distanceMm,
                            2500)  # limit distance for single command
    cartGlobal.setMoveDirection(direction)

    cartGlobal.setCartMoveDistance(distanceMmLimited)
    cartGlobal.setRequestedCartSpeed(speed)
    moveDuration = ((distanceMmLimited / speed) * 800) + 1500
    if direction in [
            cartGlobal.Direction.LEFT.value, cartGlobal.Direction.RIGHT.value
    ]:
        moveDuration = int(moveDuration / cartGlobal.SPEED_FACTOR_SIDEWAY)
    if direction in [
            cartGlobal.Direction.BACK_DIAG_LEFT,
            cartGlobal.Direction.BACK_DIAG_RIGHT,
            cartGlobal.Direction.FOR_DIAG_LEFT,
            cartGlobal.Direction.FOR_DIAG_RIGHT
    ]:
        moveDuration = int(moveDuration / cartGlobal.SPEED_FACTOR_DIAGONAL)
    durationLimited = min(moveDuration,
                          9999)  # do not move more than 10 seconds

    cartGlobal.setMovementBlocked(False)
    cartGlobal.setCartMoving(True)

    moveMsg = f"1,{direction},{speed:03.0f},{distanceMmLimited:04.0f},{durationLimited:04.0f}"
    ser.write(bytes(moveMsg + "\n", 'ascii'))
    cartGlobal.log(
        f"Send move {moveMsg}, speed: {speed:.0f}, distance: {distanceMmLimited:.0f}, duration: {durationLimited:.0f}"
    )
Пример #12
0
def doOdometry():

    global matchFailures, _lastDxPixPerSec, _lastDyPixPerSec

    saveFloorImages = True

    goodFrames = 0
    badFrames = 0
    frameNr = 0
    failures = np.zeros(10)

    cartGlobal.log("cart starts moving")

    # check for working cam and take a start image
    while True:

        # take a start image
        img1Timestamp = time.time()
        img2Timestamp = time.time()

        for _ in range(2):      # first frame is outdated
            ret, img2 = cap.read()

        if ret:
            if saveFloorImages:
                cartGlobal.saveImg(img2, frameNr)

            # use bw image for comparison
            img2bw = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

            # find relevant points in image
            kps2, des2 = orb.detectAndCompute(img2bw, None)

            _lastDxPixPerSec = 0
            _lastDyPixPerSec = 0

            break

        else:
            cartGlobal.log("cam problems, no start frame")


    frameSkipped = False

    # while cart is moving
    # take a ground pic and compare it with the previous one
    # find distance travelled in mm and update current position
    ########################################################
    while cartGlobal.isCartMoving():

        # check for problems with last frame comparison, compensate with last good move
        if frameSkipped:

            badFrames +=1

            # based on last good move and time since last eval update cart position
            duration = img2Timestamp - img1Timestamp
            dxPix, dyPix = estimateUntrackedMoveInPix(duration)

            # cartGlobal.log(f"  frame comparison failed with frame {frameNr}!, using last good dx/dy {dxPix:.3f},{dyPix:.3f}")
            cartGlobal.updateCartPositionInMm(dxPix, dyPix, duration)

        else:
            goodFrames += 1

        # check for slowing down
        checkDecelerate()

        # check for requested distance travelled
        if cartGlobal.checkMoveDistanceReached():

            arduino.sendStopCommand("distance reached")
            cartGlobal.log(f"move distance reached: target distance: {cartGlobal._moveDistanceRequested}, travelled distance: {cartGlobal.currMoveDistance()}, move time: {time.time() - cartGlobal._moveStartTime:.2f}")
            posX, posY = cartGlobal.getCartPosition()
            cartGlobal.log(f"current position {posX}/{posY}")

                 
        frameSkipped = True     # assume we have a problem detecting movement
        frameNr += 1
        cartGlobal.log("")
        cartGlobal.log(f"cart is moving, check progress, current frame: {frameNr}")
        #cartGlobal.setDistEvalTimestamp()

        # use last taken image as previous image
        img1bw = img2bw[:]
        kps1 = kps2[:]
        des1 = des2[:]
        img1Timestamp = img2Timestamp

        # get new image
        img2Timestamp = time.time()
        for _ in range(2):
            ret, img2 = cap.read()

        if ret:
            if saveFloorImages:
                cartGlobal.saveImg(img2, frameNr)
            img2bw = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
            kps2, des2 = orb.detectAndCompute(img2bw, None)

        else:
            #cartGlobal.log("cam problems, frame skipped")
            failures[0] += 1
            continue        # retry

        try:
            matches = bf.knnMatch(des1, des2, k=2)

        except:
            #cartGlobal.log("knnMatch failure")
            failures[1] += 1
            continue

        if not matches:
            #cartGlobal.log(f"No matches from bf.knnMach!")
            failures[2] += 1
            continue

        # get good matches (g_matches) as per Lowe's ratio 
        g_match = []

        if np.shape(matches)[1] != 2:
            #cartGlobal.log(f"no valid matches pairs found, frame: {frameNr}")
            failures[3] += 1
            continue

        for m, n in matches:
            if m.distance < 0.8 * n.distance:
                g_match.append(m)

        if len(g_match) < MIN_CANDIDATES:
            #cartGlobal.log(f"not enough matches pairs found, frame: {frameNr}")
            failures[4] += 1
            continue

        else:

            # get source and target points       
            src_pts = np.float32([ kps1[m.queryIdx].pt for m in g_match ]).reshape(-1,1,2)
            dst_pts = np.float32([ kps2[m.trainIdx].pt for m in g_match ]).reshape(-1,1,2)

            #cartGlobal.log(f"src/dst points {len(src_pts)}/{len(dst_pts)}")

            #_, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
            _, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
            matchesMask = mask.ravel().tolist()


            # try to find x/y distance moved
            src = [src_pts for (src_pts, matchesMask) in zip(src_pts, matchesMask) if matchesMask > 0]
            dst = [dst_pts for (dst_pts, matchesMask) in zip(dst_pts, matchesMask) if matchesMask > 0]

            #if len(src) < MIN_CANDIDATES + 5:
            if len(src) < MIN_CANDIDATES:
                #cartGlobal.log(f"not enough matches after findHomography, frame: {frameNr}")
                failures[5] += 1
                continue

            # use only similar angle/distance values
            if not similarAngleDistance(src, dst):
                #cartGlobal.log(f"not enough similar angles/distancies found, frame: {frameNr}")
                failures[6] += 1
                continue

            #try:
            xSum, ySum = 0, 0
            numCompare = min(len(src), MIN_CANDIDATES)

            for i in range(numCompare):
                xSum += src[i][0][0]-dst[i][0][0]
                ySum += src[i][0][1]-dst[i][0][1]

            if numCompare:
                dxPix = xSum / numCompare
                dyPix = ySum / numCompare

                duration = img2Timestamp - img1Timestamp
                distance = np.hypot(dxPix, dyPix)

                _lastDxPixPerSec = dxPix / (duration)
                _lastDyPixPerSec = dyPix / (duration)

                #cartGlobal.log(f"dxPix {dxPix:.4f}, dyPix {dyPix:.4f}, duration: {img2Timestamp-img1Timestamp:.4f}, speed: {distance/duration:.4f}")

                frameSkipped = False
                cartGlobal.updateCartPositionInMm(dxPix, dyPix, duration)

            else:
                #cartGlobal.log(f"no src points")
                failures[7] += 1

        

        #cartGlobal.log(f"time used for frame comparison {time.time()-start:.4f}")

    # move done
    cartGlobal.log(f"frames: {goodFrames+badFrames}, bad frames: {badFrames}")
    if failures[0] > 0:
        cartGlobal.log(f"cam problems, frame skipped: {failures[0]:.0f}")
    if failures[1] > 0:
        cartGlobal.log(f"knnMatch failure: {failures[1]:.0f}")
    if failures[2] > 0:
        cartGlobal.log(f"No matches from bf.knnMach: {failures[2]:.0f}")
    if failures[3] > 0:
        cartGlobal.log(f"no valid matches pairs found, {failures[3]:.0f}")
    if failures[4] > 0:
        cartGlobal.log(f"not enough matches pairs found: {failures[4]:.0f}")
    if failures[5] > 0:
        cartGlobal.log(f"not enough matches after findHomography: {failures[5]:.0f}")
    if failures[6] > 0:
        cartGlobal.log(f"not enough similar angles/distancies found: {failures[6]:.0f}")
    if failures[7] > 0:
        cartGlobal.log(f"no src points: {failures[7]:.0f}")
Пример #13
0
 def exposed_rotateRelative(self, angle, speed):
     if abs(angle) > 0:
         cartGlobal.log(f"exposed_rotateRelative, angle: {angle:.1f}")
         arduino.sendRotateCommand(int(angle), int(speed))
         gui.controller.updateTargetRotation(cartGlobal.getOrientation() +
                                             int(angle))
Пример #14
0
 def exposed_move(self, direction, speed, distance=10):
     cartGlobal.log(
         f"exposed_move, direction: {direction}, speed: {speed}, distance: {distance}"
     )
     arduino.sendMoveCommand(direction, speed, distance)
Пример #15
0
 def navigateTo(self):
     start=time.time()
     #PathFinder.analyze((int(self.sbX.get()),int(self.sbY.get())))
     cartGlobal.log(f"analyzed in: {time.time() - start} seconds")
     self.w.update_idletasks()
Пример #16
0
 def exposed_stop(self):
     cartGlobal.log("exposed_stop")
     gui.controller.stopCart()
Пример #17
0
    def showNewDistances(self):

        #cartGlobal.log("showNewDistancies")

        cartControl.obstacleInfo=[]

        r = cartControl.NUM_DISTANCE_SENSORS

        for i in range(r):
            s=cartControl.distanceSensors[i]
            d=cartControl.distanceList[i]
            sensor=sensors[i]

            # check for new data
            if s['newValuesShown']:
                continue

            cartControl.setSensorDataShown(i, True)
            #cartGlobal.log(f"show new sensor data {i}")

            # clear area
            col="white"
            if sensor.id < 4:
                self.canvas.create_rectangle(sensor.x1 - 20, sensor.y1 - 10, sensor.x2 + 20, sensor.y, fill=col, outline=col)
            if sensor.id > 5:
                self.canvas.create_rectangle(sensor.x1 - 20, sensor.y, sensor.x2 + 20, sensor.y + 40, fill=col, outline=col)
            if sensor.id == 4:
                self.canvas.create_rectangle(sensor.x1 - 20, sensor.y - 40, sensor.x, sensor.y + 40, fill=col, outline=col)
            if sensor.id == 5:
                self.canvas.create_rectangle(sensor.x, sensor.y - 40, sensor.x + 20, sensor.y + 40, fill=col, outline=col)

            # show valid range of distance measures
            self.canvas.create_arc(sensor.x1, sensor.y1, sensor.x2, sensor.y2,
                start=sensor.arcFrom, extent=sensor.arcLength, style=tk.ARC, width=sensor.arcWidth, outline="snow3")

            if s.get('range') == 'long': 
                lengthFactor=LINE_SCALE_LONG
                minRange=cartGlobal.LONG_RANGE_MIN
                maxRange=cartGlobal.LONG_RANGE_MAX
            else: 
                lengthFactor=LINE_SCALE_SHORT
                minRange=cartGlobal.SHORT_RANGE_MIN
                maxRange=cartGlobal.SHORT_RANGE_MAX

            for k in range(cartControl.NUM_MEASUREMENTS_PER_SCAN):

                # line color depends on distance value

                age=time.time() - s.get('timestamp')

                # limit line length
                if d[k] < 40:
                    lineLength=d[k]
                else:
                    lineLength=40

                if lineLength < minRange or lineLength > maxRange:
                    cartControl.obstacleInfo.append({'direction': s.get('direction'), 'position': s.get('position')})

                    if age > 2:
                        col="coral1"
                        lineWidth = 2
                    else:
                        col="red"
                        lineWidth = 2
                else:
                    if age > 2:
                        col="paleGreen"
                        lineWidth = 1
                    else:
                        col="green"
                        lineWidth = 1

                # Angle = value index * 15 + const + start rotation of arc
                xOffset=np.cos(np.radians(k * 15 + 15 + s.get('rotation'))) * lineLength * lengthFactor
                yOffset=np.sin(np.radians(k * 15 + 15 + s.get('rotation'))) * lineLength * lengthFactor
                    
                try:
                    self.canvas.create_line(sensor.x, sensor.y, sensor.x + xOffset, sensor.y + yOffset, fill=col, width=lineWidth)
                except:
                    cartGlobal.log("ERROR: sensor.x,.y: " + str(sensor.x) + ", " + str(sensor.y))
Пример #18
0
def readMessages():

    import cartControl

    global ser

    while ser is None:
        time.sleep(0.1)

    while ser.is_open:

        if ser.inWaiting() > 0:
            #cartGlobal.log(f"inWaiting > 0")
            recvB = ser.readline(120)
            try:
                recv = recvB.decode()
            except:
                cartGlobal.log(f"problem with decoding cart msg '{recvB}'")

            #cartGlobal.log(f"line read {recv}")
            msgID = recvB[0:3].decode()

            #cartGlobal.log("in read message: ", msgID)

            if msgID == "!A0":  #"cart ready"
                cartGlobal.arduinoStatus = 1
                cartGlobal.log("cart ready")
                sendConfigValues()

            elif msgID == "!A1":  # distance values from obstacle sensors
                #!A1,<sensorID>,<ANZ_MESSUNGEN_PRO_SCAN>,[ANZ_MESSUNGEN_PRO_SCAN<value>,]
                messageItems = [
                    int(e) if e.isdigit() else e for e in recv.split(',')
                ]
                cartControl.updateDistances(messageItems[1:])
                #gui.controller.showDistances(messageItems[1])      test, could be responsible for delays in message read?

            elif msgID == "!A2":  #"obstacle:":
                #!A2,<shortest distance>,<farthest distance>,<first sensorId with exceeding range>
                try:
                    distanceShort = float(recv.split(",")[1])
                    distanceLong = float(recv.split(",")[2])
                    sensorID = int(recv.split(",")[3])
                except:
                    cartGlobal.log(
                        f"exception with message: '{recv[:-2]}', error in message structure"
                    )
                    distanceShort = -1
                    distanceLong = -1
                cartGlobal.setMovementBlocked(True)
                if distanceShort < distanceLong:
                    distance = distanceShort
                else:
                    distance = distanceLong
                gui.controller.updateDistanceSensorObstacle(distance, sensorID)
                gui.controller.updateDistanceSensorAbyss("", -1)

            elif msgID == "!A3":  #"abyss:":
                #!A2,<farthest distance>,<farthest distance>,<first sensorId with exceeding range>
                try:
                    distanceShort = float(recv.split(",")[1])
                    distanceLong = float(recv.split(",")[2])
                    sensorID = int(recv.split(",")[3])
                except:
                    cartGlobal.log(
                        f"exception with message: '{recv[:-2]}', error in message structure"
                    )
                    distanceShort = -1
                    distanceLong = -1

                if distanceShort > distanceLong:
                    distance = distanceShort
                else:
                    distance = distanceLong

                cartGlobal.setMovementBlocked(True)
                gui.controller.updateDistanceSensorAbyss(distance, sensorID)
                gui.controller.updateDistanceSensorObstacle("", -1)

            elif msgID == "!A4":  # free path after move blocked
                cartGlobal.setMovementBlocked(False)
                gui.controller.updateDistanceSensorAbyss("", -1)
                gui.controller.updateDistanceSensorObstacle("", -1)

            elif msgID == "!A5":  #"cart stopped":
                # !A5,<orientation>,<distance moved>
                orientation = round(float(recv.split(",")[1]))
                distance = round(float(recv.split(",")[2]))
                direction = int(recv.split(",")[3])

                cartGlobal.setOrientation(
                    orientation)  # delays with A9 occurred
                cartGlobal.calculateNewCartPosition(orientation, distance,
                                                    direction)
                cartGlobal.setMovementBlocked(False)
                cartGlobal.setCartRotating(False)
                cartGlobal.setCartMoving(False)
                cartGlobal.log("<-A " + recv[:-2])  # stop and reason
                cartGlobal.log("!A5 cart stopped received from Arduino")

                # persist position
                # do this on the cart as cart might be moved locally not through navManager
                cartGlobal.saveCartLocation()

            elif msgID == "!Aa":  # cart position update:
                # !Aa,<orientation>,<distance moved>
                orientation = round(float(recv.split(",")[1]))
                distance = round(float(recv.split(",")[2]))
                direction = int(recv.split(",")[3])
                cartGlobal.setOrientation(orientation)
                if distance > 0:
                    cartGlobal.calculateNewCartPosition(
                        orientation, distance, direction)
                cartGlobal.log(f"!Aa, distance: {distance}")

            elif msgID == "!A6":  # cart docked
                # Arduino triggers the relais for loading batteries through the docking contacts
                # gui update by heartbeat logic
                cartGlobal.log("cart docked")
                if cartGlobal.navManager is not None:
                    cartGlobal.navManager.root.cartDocked(True)

            elif msgID == "!A7":  # cart docked
                # Arduino releases the relais for loading batteries through the docking contacts
                cartGlobal.log("cart undocked")
                if cartGlobal.navManager is not None:
                    cartGlobal.navManager.root.cartDocked(False)

            elif msgID == "!A8":  #12 V supply, measured value
                cartGlobal.log(f"{recv[:-2]} received")
                try:
                    newVoltage12V = round(float(recv.split(",")[1]))
                except:
                    cartGlobal.log(
                        "!A8 not able to retrieve voltage value {recv[:-2]}")
                    newVoltage12V = cartGlobal.getVoltage12V()

                # test for change in Voltage
                if abs(newVoltage12V - cartGlobal.getVoltage12V()) > 500:
                    cartGlobal.setVoltage12V(newVoltage12V)
                    cartGlobal.log(
                        f"new 12V voltage read [mV]: {newVoltage12V}")

            elif msgID == "!A9":  # orientation x:":
                #!A9,<orientation>
                try:
                    newOrientation = round(float(recv.split(",")[1]))
                except:
                    cartGlobal.log(
                        "!A9 not able to retrieve orientation {recv[:-2]}")
                    newOrientation = cartGlobal.getOrientation()

                # test for change of orientation
                if cartGlobal.getOrientation() != newOrientation:
                    cartGlobal.setOrientation(newOrientation)
                    #cartGlobal.log(f"new cart orientation: {newOrientation}")

                    # if cart is in rotation no blocking exists
                    if cartGlobal.getMovementBlocked()[0]:
                        cartGlobal.setMovementBlocked(False)

                # no new orientation, check for blocked movement and stop cart
                else:
                    blocked, startTime = cartGlobal.getMovementBlocked()
                    if blocked:
                        if time.time() - startTime > 3:
                            if cartGlobal.isCartMoving():
                                #cartGlobal.log("cart stopped by MovementBlocked")
                                sendStopCommand("blocked movement")
                                cartGlobal.setMovementBlocked(False)

            else:
                try:
                    cartGlobal.log("<-A " + recv[:-2])
                except:
                    cartGlobal.log(
                        f"Unexpected error on reading messages: {sys.exc_info()[0]}"
                    )

            #cartGlobal.log("msg processed")
        time.sleep(0.001)  # give other threads a chance
Пример #19
0
def sendReadSensorCommand(sensorID):

    msg = b'7' + bytes(str(sensorID), 'ascii') + b'\n'
    cartGlobal.log("Send readSensor " + str(msg))
    ser.write(msg)
Пример #20
0
    def exposed_getCartInfo(self):
        posX, posY = cartGlobal.getCartPosition()
        return cartGlobal.getOrientation(
        ), posX, posY, cartGlobal.isCartMoving(), cartGlobal.isCartRotating()

    def exposed_getBatteryStatus(self):
        return cartGlobal.getBatteryStatus()

    def exposed_isCartMoving(self):
        return cartGlobal.isCartMoving()

    def exposed_isCartRotating(self):
        return cartGlobal.isCartRotating()


if __name__ == '__main__':

    windowName = "marvin//cartControl"
    os.system("title " + windowName)
    hwnd = win32gui.FindWindow(None, windowName)
    win32gui.MoveWindow(hwnd, 280, 0, 1200, 400, True)

    print("startup cart")
    cartInit()

    if not cartGlobal.standAloneMode:

        cartGlobal.log("wait for messages from navManager ...")
        t = ThreadedServer(cartCommands, port=CART_PORT)
        t.start()