def handle_command(self, line): if line != "": sl = 0 sr = 0 if line == 'rdlt': answer = 'rdlt ' + str(sl + 100) + '\n' self.send(answer) print answer elif line == 'rdrt': answer = 'rdrt ' + str(sr + 100) + '\n' self.send(answer) print answer elif line == 'rdus': us = int(hcsr04.getDistance()) if us > 100: us = 100 answer = 'rdus ' + str(us) + '\n' self.send(answer) print answer else: c, p = line.split() print line.split() if c == 'lt': self.send('\n') print 'motor left ', p sl = int(p) - 100 pz.setMotor(0, sl) elif c == 'rt': self.send('\n') print 'motor right ', p sr = int(p) - 100 pz.setMotor(1, sr) else: self.send('unknown command\n') print 'Unknown command:', line
def texasRanger(): a = 0 # distance sum dummy i = 0 # measurement counter imax = 10 # total measurement number while i < imax: a += hcsr04.getDistance() i += 1 return a / imax
def run(): distance = int(hcsr04.getDistance()) while (distance > 10): distance = int(hcsr04.getDistance()) pz.stop() while True: distance = int(hcsr04.getDistance()) print "Distance:", distance distance2Pixel(distance) if distance < 10: pz.stop() time.sleep(1) while (distance < 18): distance = int(hcsr04.getDistance()) pz.spinLeft(speed) elif distance < 25: pz.forward(speed - 20) else: pz.forward(speed) time.sleep(0.1)
liftVal = 0 gripVal = 90 pz.setOutput(rot, rotVal) pz.setOutput(tilt, tiltVal) pz.setOutput(lift, liftVal) pz.setOutput(grip, gripVal) gripDistance = 3 # the distance to sensor below which the grabbing is commenced pz.setOutput(grip, gripMin) time.sleep(.5) import hcsr04 hcsr04.init() # main loop try: distance = int(hcsr04.getDistance()) print "object " + str(distance) + " far" while (distance > gripDistance) and (tiltVal > tiltMin): distance = int(hcsr04.getDistance()) # distance in cm print "object " + str(distance - gripDistance) + " too far" tiltVal = max(tiltMin, tiltVal - 5) # moves closer, but within the operational limit time.sleep(.5) # gives you time to evacuate, can have zero pz.setOutput(rot, rotVal) pz.setOutput(tilt, tiltVal) pz.setOutput(lift, liftVal) except KeyboardInterrupt: print()
i += 1 showFace(pairData, 255, 0, 255)# time.sleep(1) print 'Wiimote connected' wii.led = 1 showFace(pairData, 0, 255, 0) wii.rpt_mode = cwiid.RPT_BTN time.sleep(1) while True: buttons = wii.state['buttons'] if (buttons & cwiid.BTN_UP): # Forwards # stop all motors if too close to an object to prevent face plant if sonar.getDistance() < 10: pz.stop() showFace(oooohData, 0, 255, 0) else: time.sleep(button_delay) pz.forward(50) showFace(smileData, 255, 0, 0) elif (buttons & cwiid.BTN_DOWN): time.sleep(button_delay) pz.reverse(50) showFace(grimaceData, 255, 0, 255) elif (buttons & cwiid.BTN_LEFT): time.sleep(button_delay) pz.spinLeft(50) showFace(oooohData, 0, 255, 0) elif (buttons & cwiid.BTN_RIGHT):
#! /usr/bin/env python # # Basic test of HC-SR04 ultrasonic sensor on Picon Zero import hcsr04, time import piconzero as pz pz.setInputConfig(2, 0) pz.setInputConfig(3, 0) pz.init() hcsr04.init() try: while True: distance = int(hcsr04.getDistance()) print("Distance : "), distance if (((pz.readInput(2) != 1) or (pz.readInput(3) != 1)) and distance > 13): pz.reverse(60) else: pz.stop() time.sleep(0.2) pz.spinRight(50) time.sleep(0.2) pz.stop() break except KeyboardInterrupt: print finally: hcsr04.cleanup() pz.cleanup()
def automaze(): pz.stop() mylcd.lcd_display_string("Auto Maze ", 1) mylcd.lcd_display_string("Press E to End ", 2) time.sleep(2) MSPEED = 20 MTURN = 90 GO = 1 PREP = 1 STEP = 0 # start step count while PREP == 1: #get ready to go for event in get_key(): mylcd.lcd_display_string("Press G to GO ", 1) mylcd.lcd_display_string("Press E to End ", 2) if event.code == "KEY_G": PREP = 0 elif event.code == "KEY_E": GO = 0 PREP = 0 while GO == 1: #new simple quit - works perfectly if button.is_pressed: pz.stop() GO = 0 else: RIGHTIR = pz.readInput(0) #assign right IR to a variable LEFTIR = pz.readInput(1) #assign left IR to a variable RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) pz.forward(MSPEED) #steps to follow to complete the maze if RANGE < 25 and STEP == 0: #first right turn pz.stop() pz.spinRight(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 1 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable if RANGE < 25 and STEP == 1: #second right turn pz.stop() pz.spinRight(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 2 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable if RANGE < 25 and STEP == 2: #third right pz.stop() pz.spinRight(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 3 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable if RANGE < 25 and STEP == 3: #first left pz.stop() pz.spinLeft(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 4 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable if RANGE < 25 and STEP == 4: #second left pz.stop() pz.spinLeft(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 5 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable if RANGE < 25 and STEP == 5: #third left pz.stop() pz.spinLeft(MTURN) time.sleep(0.5) pz.stop() pz.forward(MSPEED) time.sleep(1) STEP = 6 mylcd.lcd_display_string("Range = %d %%" % RANGE, 1) mylcd.lcd_display_string("Step = %d %%" % STEP, 2) RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable #emergency wall avoidance protocol if LEFTIR == 0: pz.spinRight(100) time.sleep(0.01) elif RIGHTIR == 0: pz.spinLeft(100) time.sleep(0.01)
mylcd = I2C_LCD_driver.lcd() #assign LCD to variable for ease of use pz.init() #initiate hardware pz.setInputConfig(0, 0) #right IR sensor is input 0 and digital pz.setInputConfig(1, 0) #left IR sensor is input 1 and digital pz.setInputConfig(2, 0) #right line sensor is input 2 and digital pz.setInputConfig(3, 0) #left line is input 3 and digital RIGHTIR = pz.readInput(0) #assign right IR to a variable LEFTIR = pz.readInput(1) #assign left IR to a variable RIGHTLINE = pz.readInput(2) #assign right line sensor to a variable LEFTLINE = pz.readInput(3) #assign left line sensor to a variable hcsr04.init() #initiate hardware RANGE = hcsr04.getDistance() #assign HC-SR04 range to variable button = Button(22) #end of hardware setup #______________________________________________________________________________ #____________________________________________________________________________________ #functions for individual tasks def remotecontrol(): #works perfect pz.stop() mylcd.lcd_display_string("Remote Control ", 1) mylcd.lcd_display_string("Press E to End ", 2) time.sleep(2)
def getDistance(): return hcsr04.getDistance()
pz.stop() pts = [] row=2 scanWin.addstr(1,1,"Distance Map" ) scanWin.addstr(2,1, "Sonar : ir") for span in range( 39, 75, 3 ): irLine="" sonLine="" for stilt in range( 150, 30, -3 ): pz.setOutput (pan, span) pz.setOutput (tilt, stilt) time.sleep( 0.1) ir = pz.readInput(irSen) distance = int(hcsr04.getDistance()) #print "At pan,tilt: ",span,",",stilt,": ir Distance:", ir, " sonic distance: ", distance #volts=min(1,ir*0.0048828125); # // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3 volts=min(1,ir*0.002929688); # // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3 actdist=65*pow(volts, -1.10); # // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk #print "ir act distance: ",actdist # TODO: Save values and pos so build map # rescale reading to ascii value irLine = irLine + asciiSensor( 400, 400-min(400,ir) )
spinTime = 0.009 forwardTime = 0.0005 # stop variables sideStop = 21 frontStop = 100 # start recording ts = str(time()) cam.vflip = True cam.hflip = True cam.start_recording("/home/pi/qmpiwars/videos/straight-" + ts + ".h264") try: while True: distance = int(dist.getDistance()) LeftDist = sideDist.DistSensor(27, 22) #Left RightDist = sideDist.DistSensor(17, 18) #Right print "----------------------------------" print "Front Distance:", distance print "Left Distance:", LeftDist print "Right Distance:", RightDist print "----------------------------------" pz.forward(forwardSpeed) sleep(forwardTime) sideDist.neoPixelLight("forward") if(distance <= frontStop): # too close - STOP! print "Stop!" #pz.stop() sideDist.neoPixelLight("off") elif(LeftDist <= sideStop): # heading left - turn right
#! /usr/bin/env python # # Basic test of HC-SR04 ultrasonic sensor on Picon Zero import hcsr04, time hcsr04.init() try: while True: distance = int(hcsr04.getDistance()) print "Distance:", distance time.sleep(1) except KeyboardInterrupt: print finally: hcsr04.cleanup()
# distance variables leftMax = 12 frontStop = 25 rightMax = 12 # last known dist lastFront = 0 lastLeft = 0 lastRight = 0 # start camera # picamera start recording now... try: lastFront = int(dist.getDistance()) while lastFront > frontStop: # go forward to the first wall print "head to first wall" pz.forward(forwardSpeed) sleep(forwardTime) lastFront = int(dist.getDistance()) print str(lastFront) pz.stop() leftTurn = True # measure rhs dist currentRight = sideDist.DistSensor(17, 18) # assume this to be largest and smallest reading largeRight = currentRight smallRight = currentRight