thymio.setBothSaveWaitUntilReadyAndReturnTime(speed, speed)
t0 = clock()
# while thymio.getLeft() != speed or thymio.getRight() != speed:
# print thymio.getLeft()
# print thymio.getRight()
camera.capture(stream, format="yuv", use_video_port = True)
image1 = emd.getReceptiveFields(stream.array, nOCellsX = 50, nOCellsY = 50)
stream.truncate(0)
#sleep(timeMeasure)
camera.capture(stream, format="yuv", use_video_port = True)
image2 = emd.getReceptiveFields(stream.array, nOCellsX = 50, nOCellsY = 50)
stream.truncate(0)
motionX = emd.getMotionX(image1, image2)
#TURN PHASE
emdsLeft = (-motionX[0:motionX.shape[0],0:motionX.shape[1]/2]).clip(min = 0)
emdsRight = motionX[0:motionX.shape[0],motionX.shape[1]/2:motionX.shape[1]].clip(min = 0)
meanLeft = np.mean(emdsLeft)
meanRight = np.mean(emdsRight)
adjustmentProportionLeft = meanLeft/(meanLeft+meanRight)
adjustmentProportionRight = meanRight/(meanLeft+meanRight)
t1 = clock()
thymio.setBothSaveWaitUntilReadyAndReturnTime(speed * adjustmentProportionLeft * 2, speed * adjustmentProportionRight * 2)
if __name__ == '__main__':
thymio.init()
#sock = _raspClient.initSocket('192.168.86.98')
with picamera.PiCamera() as camera:
camera.resolution = (50,50)
camera.framerate = 60
camera.hflip = True
camera.vflip = True
with picamera.array.PiYUVArray(camera) as stream:
camera.capture(stream, format="yuv")
delayed = emd.getReceptiveFields(stream.array, nOCellsX = 50, nOCellsY = 50)
stream.truncate(0)
for i in camera.capture_continuous(stream, format="yuv", use_video_port = True):
current = emd.getReceptiveFields(stream.array, nOCellsX = 50, nOCellsY = 50)
motionX = emd.getMotionX(delayed, current)
delayed = current
movementLeft = (-motionX[0:motionX.shape[0],0:motionX.shape[1]/2]).clip(min = 0)
movementRight = motionX[0:motionX.shape[0],motionX.shape[1]/2:motionX.shape[1]].clip(min = 0)
meanLeft = np.mean(movementLeft)
meanRight = np.mean(movementRight)
baseSpeed = 50
#turnSpeed = 100
_div.clear()
groundSensors = thymio.getGroundSensorR()
if groundSensors[0] < 100 or groundSensors[1] < 100:
print("Warning Thymio is about to fall from the table")
thymio.setRight(0)
