def autoPilotUSon(self):
        actualIndex = int(self.ultrasonic.steps / 2)
        degreePerStep = 180.0 / (self.ultrasonic.steps-1)
        while True:
            dists = self.ultrasonic.findBestWay()
            print(dists)
            maxIndex = dists.index(max(dists))
            steps = abs(90.0 - maxIndex * degreePerStep) / degreePerStep + 1
            # if distance is more than 500cm, the measurement is probably wrong -> stop
            if dists[maxIndex] > self.motor.DIST_PER_SEC / 2:# and dists[maxIndex] < 500:
                if maxIndex == int(self.ultrasonic.steps / 2):
                    # straight forward
                    self.motor.forward()
                elif maxIndex < int(self.ultrasonic.steps / 2):
                    # turn right
                    self.motor.forwardLeft()
                    time.sleep(self.motor.SEC_PER_TURN / 360.0 * degreePerStep * steps)
                    self.motor.forward()
                elif maxIndex > int(self.ultrasonic.steps / 2):
                    # turn left
                    self.motor.forwardRight()
                    time.sleep(self.motor.SEC_PER_TURN / 360.0 * degreePerStep * steps)
                    self.motor.forward()
                actualIndex = maxIndex
            else:
                print(dists[maxIndex], self.motor.DIST_PER_SEC)
                self.motor.stop()
                return