def error(self, lidar, zed, imu):
minIndex, minDistance = LidarHelper.shortestDistInRange(\
lidar, self.direction() * -(90 + 5), self.direction() * 45)
minAngle = LidarHelper.lidarIndexToAngle(minIndex)
if minDistance > 2:
return 0
offsetX = minDistance * math.cos(math.radians(minAngle + 90 * self.direction()))
return self.direction() * (offsetX - GOAL_X)
def findHoleAngle(self, lidar, minIndex, dir):
index = minIndex
try:
#TODO restict ranges a bit more
while lidar[index] < HOLE_DEPTH:
index += dir
startHoleIndex = index
while lidar[index] >= HOLE_DEPTH:
index += dir
endHoleIndex = index
holeIndex = self.mid(startHoleIndex, endHoleIndex)
return LidarHelper.lidarIndexToAngle(holeIndex)
except IndexError:
return -1 # It's not on this side
def errorAngle(self, minIndex):
minAngle = LidarHelper.lidarIndexToAngle(minIndex)
# print('minAngle = %f' % minAngle)
return (self.getGoalAngle() - minAngle) / abs(self.getMaxAngle() -
self.getMinAngle())