def request_all_sensor(self):
self.sci.flash_input()
requestBytes = [142, 100]
self.sci.send(requestBytes)
data = self.sci.read(80)
return Sensor.gen_from_bytes(data)
def request_all_sensor(self):
self.sci.flash_input()
requestBytes = [142, 100]
self.sci.send(requestBytes)
data = self.sci.read(80)
return Sensor.gen_from_bytes(data)
def run(self):
while(self.running):
self._request_sensor()
self.data = self.sci.read(80)
self.sensor = Sensor.gen_from_bytes(self.data)
if self.prevSensor :
eventList = self.sensor.diff(self.prevSensor)
if (eventList and len(eventList)>0):
self._raise_event(eventList)
# 現状Create2からは正しいDistance値が取得できないため，encoderから計算する
leftDiff = self.sensor.encoderCountsLeft - self.prevSensor.encoderCountsLeft
rightDiff = self.sensor.encoderCountsRight - self.prevSensor.encoderCountsRight
# overflow check
if(leftDiff<-10000):
leftDiff += 65536
elif(leftDiff>10000):
leftDiff -= 65536
if(rightDiff<-10000):
rightDiff += 65536
elif(rightDiff>10000):
rightDiff -= 65536
self.leftEncoder += leftDiff
self.rightEncoder += rightDiff
self.totalDistance += (leftDiff + rightDiff)/2 * ENC_TO_DISTANCE
l = leftDiff * ENC_TO_DISTANCE
r = rightDiff * ENC_TO_DISTANCE
angleDiff = -(l-r) * 180/(WHEEL_BASE*2)/math.pi
self.totalAngle += angleDiff
# check reachDistance Event
if(self.nextDistance):
if(self.nextDistanceCompare):
if(self.totalDistance>self.nextDistance):
self.nextDistance=None
self.nextDistanceCompare=None
self._raise_event([Event.reachDistance])
#print "raise reachDistance1"
else:
if(self.totalDistance<self.nextDistance):
self.nextDistance=None
self.nextDistanceCompare=None
self._raise_event([Event.reachDistance])
#print "raise reachDistance2"
# check reachAngle Event
if(self.nextAngle):
if(self.nextAngleCompare):
if(self.totalAngle>self.nextAngle):
self.nextAngle=None
self.nextAngleCompare=None
self._raise_event([Event.reachAngle])
else:
if(self.totalAngle<self.nextAngle):
self.nextAngle=None
self.nextAngleCompare=None
self._raise_event([Event.reachAngle])
self.prevSensor = self.sensor
time.sleep(self.interval/1000)
def run(self):
while (self.running):
self._request_sensor()
self.data = self.sci.read(80)
self.sensor = Sensor.gen_from_bytes(self.data)
if self.prevSensor:
eventList = self.sensor.diff(self.prevSensor)
if (eventList and len(eventList) > 0):
self._raise_event(eventList)
# 現状Create2からは正しいDistance値が取得できないため，encoderから計算する
leftDiff = self.sensor.encoderCountsLeft - self.prevSensor.encoderCountsLeft
rightDiff = self.sensor.encoderCountsRight - self.prevSensor.encoderCountsRight
# overflow check
if (leftDiff < -10000):
leftDiff += 65536
elif (leftDiff > 10000):
leftDiff -= 65536
if (rightDiff < -10000):
rightDiff += 65536
elif (rightDiff > 10000):
rightDiff -= 65536
self.leftEncoder += leftDiff
self.rightEncoder += rightDiff
self.totalDistance += (leftDiff +
rightDiff) / 2 * ENC_TO_DISTANCE
l = leftDiff * ENC_TO_DISTANCE
r = rightDiff * ENC_TO_DISTANCE
angleDiff = -(l - r) * 180 / (WHEEL_BASE * 2) / math.pi
self.totalAngle += angleDiff
# check reachDistance Event
if (self.nextDistance):
if (self.nextDistanceCompare):
if (self.totalDistance > self.nextDistance):
self.nextDistance = None
self.nextDistanceCompare = None
self._raise_event([Event.reachDistance])
#print "raise reachDistance1"
else:
if (self.totalDistance < self.nextDistance):
self.nextDistance = None
self.nextDistanceCompare = None
self._raise_event([Event.reachDistance])
#print "raise reachDistance2"
# check reachAngle Event
if (self.nextAngle):
if (self.nextAngleCompare):
if (self.totalAngle > self.nextAngle):
self.nextAngle = None
self.nextAngleCompare = None
self._raise_event([Event.reachAngle])
else:
if (self.totalAngle < self.nextAngle):
self.nextAngle = None
self.nextAngleCompare = None
self._raise_event([Event.reachAngle])
self.prevSensor = self.sensor
time.sleep(self.interval / 1000)
