def simpleNudgeTest(): print "sending start" cl.flush() cl.start(13) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" delay(9000) print "nudging" cl.flush() cl.nudge(cl.M1) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" delay(3000) print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!"
def simpleNudgeTest(): print "sending start" cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" delay( 9000 ) print "nudging" cl.flush() cl.nudge( cl.M1 ) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" delay( 3000 ) print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!"
def mainLoop(): vc = VideoCapture(outfile="sample_video/nudge_movement_raw.avi") lastNudge = 0 intersect = None print "startup loop" while vc.frameBuf.size() < 5 and vc.captureFrame(): frame, intersect, horz = vc.findLines() vc.drawGrid(frame) vc.writeFrame() if intersect: vc.saveFrameToBuf() print "sending start" cl.flush() cl.start(13) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" # t0 = time.time() while vc.captureFrame() and vc.frameCount < 150: frame, intersect, horz = vc.findLines() nudgeMotor = None nudgeTime = None mustStop = False for line in horz: # print "horz:", line if line[0] > vc.height - 50 and line[1] == 255: print "can't continue, gotta stop" mustStop = True break if mustStop: vc.drawGrid(frame) vc.writeFrame() vc.saveFrameToBuf() break if intersect and vc.frameCount >= (lastNudge + 15): if intersect > (vc.width / 2 + 60): print "BIGGER NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 7 circle(frame, (20, vc.height - 20), 6, (0, 0, 255), 2) elif intersect > (vc.width / 2 + 30): print "BIG NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 4 circle(frame, (20, vc.height - 20), 4, (0, 0, 255), 2) elif intersect > (vc.width / 2 + 15): print "NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 2 circle(frame, (20, vc.height - 20), 2, (0, 0, 255), 2) elif intersect < (vc.width / 2 - 60): print "BIGGER NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 7 circle(frame, (20, vc.height - 20), 6, (0, 255, 0), 2) elif intersect < (vc.width / 2 - 30): print "BIG NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 4 circle(frame, (20, vc.height - 20), 4, (0, 255, 0), 2) elif intersect < (vc.width / 2 - 15): print "NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 2 circle(frame, (20, vc.height - 20), 2, (0, 255, 0), 2) if nudgeMotor and nudgeTime: cl.flush() cl.nudge(nudgeMotor, nudgeTime) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" lastNudge = vc.frameCount vc.drawGrid(frame) vc.writeFrame() vc.saveFrameToBuf() print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!" # get a few final frames print "get a few more frames" marker = vc.frameCount + 10 while vc.captureFrame() and vc.frameCount < marker: frame, intersect, horz = vc.findLines() vc.drawGrid(frame) vc.writeFrame(frame) vc.saveFrameToBuf()
def mainLoop(): vc = VideoCapture( outfile="sample_video/nudge_movement_raw.avi" ) lastNudge = 0 intersect = None print "startup loop" while vc.frameBuf.size() < 5 and vc.captureFrame(): frame, intersect, horz = vc.findLines() vc.drawGrid( frame ) vc.writeFrame( ) if intersect: vc.saveFrameToBuf() print "sending start" cl.flush() cl.start( 13 ) test = cl.readAndCheck() if not test: print "start not received D=" else: print "start received!" # t0 = time.time() while vc.captureFrame() and vc.frameCount < 150: frame, intersect, horz = vc.findLines() nudgeMotor = None nudgeTime = None mustStop = False for line in horz: # print "horz:", line if line[0] > vc.height-50 and line[1] == 255: print "can't continue, gotta stop" mustStop = True break if mustStop: vc.drawGrid( frame ) vc.writeFrame( ) vc.saveFrameToBuf() break if intersect and vc.frameCount >= (lastNudge+15): if intersect > (vc.width/2 + 60): print "BIGGER NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 7 circle( frame, (20, vc.height-20), 6, (0,0,255), 2 ) elif intersect > (vc.width/2 + 30): print "BIG NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,0,255), 2 ) elif intersect > (vc.width/2 + 15): print "NUDGE M2!!" nudgeMotor = cl.M2 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,0,255), 2 ) elif intersect < (vc.width/2 - 60): print "BIGGER NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 7 circle( frame, (20, vc.height-20), 6, (0,255,0), 2 ) elif intersect < (vc.width/2 - 30): print "BIG NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,255,0), 2 ) elif intersect < (vc.width/2 - 15): print "NUDGE M1!!" nudgeMotor = cl.M1 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,255,0), 2 ) if nudgeMotor and nudgeTime: cl.flush() cl.nudge( nudgeMotor, nudgeTime ) test = cl.readAndCheck() if not test: print "nudge not received D=" else: print "nudge received!" lastNudge = vc.frameCount vc.drawGrid( frame ) vc.writeFrame( ) vc.saveFrameToBuf() print "sending stop" cl.flush() cl.stop() if not cl.readAndCheck(): print "stop not received D=" else: print "stop received!" # get a few final frames print "get a few more frames" marker = vc.frameCount + 10 while vc.captureFrame() and vc.frameCount < marker: frame, intersect, horz = vc.findLines() vc.drawGrid( frame ) vc.writeFrame( frame ) vc.saveFrameToBuf()
def forwardMovement(): global nextTurn lastNudge = 0 intersect = None while vc.captureFrame(): lineFrame, intersect, horz = vc.findLines() if vc.checkReset(): Log.info("resetting....") return False # vc.frameBuf.printHorzDiff() # if nextTurn == None: # next = vc.findShapes() cornerFrame = vc.trackCorners() vc.drawGrid(lineFrame) vc.writeFrame(lineFrame) vc.saveFrameToBuf() frame = lineFrame nudgeMotor = None nudgeTime = None if vc.lastFlowPnts is not None: pnts = vc.lastFlowPnts # Log.debug( 3*vc.height/4 ) # Log.debug( pnt ) if len(pnts) == 2 and abs(pnts[0][0][1] - pnts[1][0][1]) < 10: # condition where there are only 2 dots on the same horz # we don't wanna stop on the farthest tracked point pass elif pnts[0][0][1] > (3 * vc.height / 4) - 25: Log.debug("can't continue, gotta stop - corner") return True for line in horz: if line[0] > vc.height - 80 and line[1] == 255: Log.debug("can't continue, gotta stop - line") return True if intersect and vc.frameCount >= (lastNudge + 10): if intersect > (vc.width / 2 + 60): nudgeMotor = cl.M2 nudgeTime = 6 circle(frame, (20, vc.height - 20), 6, (0, 0, 255), 2) elif intersect > (vc.width / 2 + 25): nudgeMotor = cl.M2 nudgeTime = 4 circle(frame, (20, vc.height - 20), 4, (0, 0, 255), 2) elif intersect > (vc.width / 2 + 15): nudgeMotor = cl.M2 nudgeTime = 2 circle(frame, (20, vc.height - 20), 2, (0, 0, 255), 2) elif intersect < (vc.width / 2 - 60): nudgeMotor = cl.M1 nudgeTime = 6 circle(frame, (20, vc.height - 20), 6, (0, 255, 0), 2) elif intersect < (vc.width / 2 - 25): nudgeMotor = cl.M1 nudgeTime = 4 circle(frame, (20, vc.height - 20), 4, (0, 255, 0), 2) elif intersect < (vc.width / 2 - 15): nudgeMotor = cl.M1 nudgeTime = 2 circle(frame, (20, vc.height - 20), 2, (0, 255, 0), 2) if nudgeMotor and nudgeTime: cl.flush() cl.nudge(nudgeMotor, nudgeTime) test = cl.readAndCheck() if not test: Log.warning("nudge %s, %d not received D=" % (nudgeMotor, nudgeTime)) else: Log.info("nudge %s, %d received!" % (nudgeMotor, nudgeTime)) lastNudge = vc.frameCount
def forwardMovement(): global nextTurn lastNudge = 0 intersect = None while vc.captureFrame(): lineFrame, intersect, horz = vc.findLines() if vc.checkReset(): Log.info( "resetting...." ) return False # vc.frameBuf.printHorzDiff() # if nextTurn == None: # next = vc.findShapes() cornerFrame = vc.trackCorners() vc.drawGrid( lineFrame ) vc.writeFrame( lineFrame ) vc.saveFrameToBuf() frame = lineFrame nudgeMotor = None nudgeTime = None if vc.lastFlowPnts is not None: pnts = vc.lastFlowPnts # Log.debug( 3*vc.height/4 ) # Log.debug( pnt ) if len(pnts) == 2 and abs(pnts[0][0][1] - pnts[1][0][1]) < 10: # condition where there are only 2 dots on the same horz # we don't wanna stop on the farthest tracked point pass elif pnts[0][0][1] > (3*vc.height/4)-25: Log.debug( "can't continue, gotta stop - corner" ) return True for line in horz: if line[0] > vc.height-80 and line[1] == 255: Log.debug( "can't continue, gotta stop - line" ) return True if intersect and vc.frameCount >= (lastNudge+10): if intersect > (vc.width/2 + 60): nudgeMotor = cl.M2 nudgeTime = 6 circle( frame, (20, vc.height-20), 6, (0,0,255), 2 ) elif intersect > (vc.width/2 + 25): nudgeMotor = cl.M2 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,0,255), 2 ) elif intersect > (vc.width/2 + 15): nudgeMotor = cl.M2 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,0,255), 2 ) elif intersect < (vc.width/2 - 60): nudgeMotor = cl.M1 nudgeTime = 6 circle( frame, (20, vc.height-20), 6, (0,255,0), 2 ) elif intersect < (vc.width/2 - 25): nudgeMotor = cl.M1 nudgeTime = 4 circle( frame, (20, vc.height-20), 4, (0,255,0), 2 ) elif intersect < (vc.width/2 - 15): nudgeMotor = cl.M1 nudgeTime = 2 circle( frame, (20, vc.height-20), 2, (0,255,0), 2 ) if nudgeMotor and nudgeTime: cl.flush() cl.nudge( nudgeMotor, nudgeTime ) test = cl.readAndCheck() if not test: Log.warning( "nudge %s, %d not received D=" % (nudgeMotor, nudgeTime) ) else: Log.info( "nudge %s, %d received!" % (nudgeMotor, nudgeTime) ) lastNudge = vc.frameCount