Example #1
0
def getout():
    leftLine = pi2go.irLeftLine()
    rightLine = pi2go.irRightLine()
    while (not (leftLine and rightLine)):
        pi2go.spinLeft(20)
        leftLine = pi2go.irLeftLine()
        rightLine = pi2go.irRightLine()
Example #2
0
def calibrate(action):
    """Calibrate the robot by performing the action until the left line sensor has changed 
    to dark three times.
    
    Returns time taken for action"""
    count = 0
    timestart = 0.0
    timeend = 0.0
    online = False
    
    # start the motor action, e.g. spinleft(100)
    action
    
    # act until see dark line
    while pi2go.irLeftLine():
        time.sleep(0.01)
    online = True
    timestart = time.time()
    while count < 2:
        if not pi2go.irLeftLine():
            if not online:
                online = True
                count += 1
        elif online:
            online = False
        time.sleep(0.01)
    timeend = time.time()    
    pi2go.stop()

    logging.debug('Time={}-{}={}'.format(timeend, timestart, (timeend-timestart)/2))
    return (timeend-timestart)/3
Example #3
0
def gotoline(speed, over=True):
    """Proceed forward until cross the line."""
    pi2go.goBoth(speed)
    while not pi2go.irLeftLine():
        time.sleep(0.1)
    if over:
        # get off the line
        while pi2go.irLeftLine():
            time.sleep(0.1)
def line(speed):
    left = pi2go.irLeftLine()
    right = pi2go.irRightLine()  
    if left == False and right == True:
        pi2go.forward(speed)
    elif right == True and left == True:
        pi2go.spinLeft(speed - 20) # speed-20 to smoothen the line follow
    elif left == True and right == False:
        pi2go.spinRight(speed - 20)  # speed-20 to smoothen the line follow
    elif left == False and right == False:
        pi2go.forward(speed)
Example #5
0
def linefollow(speed):    
    left = pi2go.irLeftLine()
    right = pi2go.irRightLine()
    if left == False and right == True:
        pi2go.forward(speed)
    elif right == True and left == True:
        pi2go.spinLeft(speed - 30)
    elif left == True and right == False:
        pi2go.spinRight(speed - 30)
    elif left == False and right == False:
        pi2go.forward(speed)
Example #6
0
 def displayStatus(self):
     left=pi2go.irLeft()
     right=pi2go.irRight()
     leftLine=pi2go.irLeftLine()
     rightLine=pi2go.irRightLine()
     distance=pi2go.getDistance()
     myscreen.addstr(2,30,"sonar "+str(distance))
     myscreen.addstr(3,30,"left sensor "+str(left))
     myscreen.addstr(4,30,"right sensor "+str(right))
     myscreen.addstr(5,30,"leftIR sensor "+str(leftLine))
     myscreen.addstr(6,30,"rightIR sensor "+str(rightLine))
     myscreen.refresh()
Example #7
0
 def displayStatus(self):
     left = pi2go.irLeft()
     right = pi2go.irRight()
     leftLine = pi2go.irLeftLine()
     rightLine = pi2go.irRightLine()
     distance = pi2go.getDistance()
     myscreen.addstr(2, 30, "sonar " + str(distance))
     myscreen.addstr(3, 30, "left sensor " + str(left))
     myscreen.addstr(4, 30, "right sensor " + str(right))
     myscreen.addstr(5, 30, "leftIR sensor " + str(leftLine))
     myscreen.addstr(6, 30, "rightIR sensor " + str(rightLine))
     myscreen.refresh()
Example #8
0
def mainLoop():
  global globalDistance, globalStop, state, finished
  global slowspeed, fastspeed
  while finished == False:
    if globalStop==1 or globalDistance<5:
      pi2go.stop()
    else:
      if state==1:  # Standard Line Follower
        if pi2go.irLeftLine() and pi2go.irRightLine():
          pi2go.forward(40)
        elif pi2go.irRightLine()==False:
          pi2go.spinRight(fastspeed)
        elif pi2go.irLeftLine()==False:
          pi2go.spinLeft(fastspeed)
      elif state==2:  # Obstacle avoider (reverses then spins when near object)
        if globalDistance>15:
          pi2go.forward(50)
        else:
          pi2go.reverse(30)
          time.sleep(0.5)
          pi2go.turnReverse(30,50)
          time.sleep(3)
      elif state==3:  # Obstacle avoider (spins when near object)
        if globalDistance>15:
          pi2go.forward(50)
        else:
          pi2go.spinLeft(50)
      elif state==4:  # Avoids objects using IR sensors only
        if pi2go.irAll()==False:
          pi2go.forward(50)
        else:
          ir=pi2go.irRight()
          pi2go.reverse(30)
          time.sleep(0.5)
          if ir:
            pi2go.turnReverse(50,30)
          else:
            pi2go.turnReverse(30,50)
          time.sleep(3)
Example #9
0
def pi2goMove():
    #print "pi2go Move"
    if not piStop:
        if pi2go.irLeftLine() == False:
            #print "Left"
            pi2go.turnForward(0.3*speed, speed)
        elif pi2go.irRightLine() == False:
            #print "Right"
            pi2go.turnForward(speed, 0.3*speed)
        else:
            #print "Straight"
            pi2go.forward(speed)
    else:
        pi2go.forward(0)
Example #10
0
def distance():
    stime = time.time()
    while True:
        # Defining the sensors
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()
        ntime = time.time()

        # timecheck for distance
        if ntime > (stime + 0.1):
            #print "%.5f" %time.time()
            dist = (int(pi2go.getDistance() * 10)) / 10.0
            #print "%.5f" %time.time()
            stime = ntime

    # distance groups
        if dist < 10:
            pi2go.setAllLEDs(2000, 0, 0)
        elif dist > 20:
            pi2go.setAllLEDs(0, 0, 2000)
        else:
            pi2go.setAllLEDs(0, 2000, 0)
Example #11
0
def distance():
    stime = time.time()
    while True:
        # Defining the sensors
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()
        ntime = time.time()

        # timecheck for distance
        if ntime > (stime + 0.1):
        #print "%.5f" %time.time()
            dist = (int(pi2go.getDistance()*10))/10.0
        #print "%.5f" %time.time()
            stime = ntime

      # distance groups
        if dist < 10:
            pi2go.setAllLEDs(2000, 0, 0)
        elif dist > 20:
            pi2go.setAllLEDs(0, 0, 2000)
        else:
            pi2go.setAllLEDs(0, 2000, 0)
Example #12
0
def calibrateRight():
    print "Calibrating right..."
    pi2go.setAllLEDs(0, 400, 0)
    pi2go.spinRight(20)

    leftLine = not pi2go.irLeftLine()
    rightLine = not pi2go.irRightLine()

    while (not leftLine and not rightLine):
        leftLine = not pi2go.irLeftLine()
        rightLine = not pi2go.irRightLine()

    if (rightLine):
        while (rightLine):
            leftLine = not pi2go.irLeftLine()
            rightLine = not pi2go.irRightLine()

        while (not leftLine):
            leftLine = not pi2go.irLeftLine()
            rightLine = not pi2go.irRightLine()

        pi2go.spinLeft(20)
        while (leftLine):
            leftLine = not pi2go.irLeftLine()

    else:
        pi2go.spinLeft(20)
        while (not rightLine):
            leftLine = not pi2go.irLeftLine()
            rightLine = not pi2go.irRightLine()

        pi2go.spinRight(20)
        while (rightLine):
            leftLine = not pi2go.irLeftLine()
            rightLine = not pi2go.irRightLine()

    pi2go.stop()

    pi2go.setAllLEDs(0, 0, 0)
Example #13
0
#tollgate 2, task 1a

import pi2go, time
import sys

pi2go.init()

speed = 60

try:

    while True:
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()
        if left == False and right == True:
            pi2go.forward(speed)
        elif left == True and right == True:
            pi2go.spinLeft(speed - 20)
        elif left == True and right == False:
            pi2go.spinRight(speed - 20)
        elif left == False and right == False:
            pi2go.forward(speed)

finally:
    pi2go.cleanup()
    sys.exit(0)
Example #14
0
import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
  print "Running on Pi2Go"
else:
  print "Running on Pi2Go-Lite"
count = 1

try:
    while True:
        print 'Count: ', count
        print 'Left:', pi2go.irLeft()
        if vsn == 1:
            print 'Centre:', pi2go.irCentre()
        print 'Right:', pi2go.irRight()
        print 'Line left', pi2go.irLeftLine()
        print 'Line right', pi2go.irRightLine()
        print
        count += 1
        time.sleep(1)
                          
except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
Example #15
0
        getout()
        calibrateRight()

        pi2go.setAllLEDs(0, 0, 0)
        pi2go.stop()
        baseSpeed = 30
        reassureTime = 1500

        lastTouchLeft = millis()
        lastTouchRight = millis()

        mode = "NORMAL"

        while True:
            leftLine = not pi2go.irLeftLine()
            rightLine = not pi2go.irRightLine()
            distance = pi2go.getDistance()

            if (distance < 1):
                print "Distance too low! Turning! ", distance
                pi2go.stop()

                for i in range(3):
                    pi2go.setAllLEDs(400, 0, 0)
                    time_.sleep(0.3)
                    pi2go.setAllLEDs(0, 0, 0)
                    time_.sleep(0.3)

                pi2go.spinLeft(20)
                while (not rightLine):
Example #16
0
#!/usr/bin/env python

import pi2go
import time
pi2go.init()
speed = 50
print "vorwaerts!"
pi2go.forward(speed)
print "fuer 2 Sekunden..."
time.sleep(2)
print "drehe links!"
pi2go.spinLeft(speed)
print "fuer 3 Sekunden..."
time.sleep(3)
print "stop"
pi2go.stop()
linkerLinienSensor = pi2go.irLeftLine()
rechterLinienSensor = pi2go.irRightLine()
print "Linker Linien-Sensor: ", linkerLinienSensor
print "Rechter Linien-Sensor: ", rechterLinienSensor
pi2go.cleanup()
Example #17
0
#!/usr/bin/env python
#Simply prints the state of the line follower sensors

# Must be run as root - sudo python .py

import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
    print "Running on Pi2Go"
else:
    print "Running on Pi2Go-Lite"

try:
    while True:
        print 'Left:', pi2go.irLeftLine()
        print 'Right:', pi2go.irRightLine()
        print
        time.sleep(1)

except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
Example #18
0
def line_follower():
    """
    State:
    L R
    1 1 - Both White  - Depends on P
    1 0 - Left White  - Turn Left
    0 1 - Right White - Turn Right
    0 0 - Both Black  - Go Forward

    P - previous State
    ------------------
    0 - Left
    1 - Right

    :return:
    """
    dist = 0
    speed = 70
    change = 20
    start = 0

    STATE = 00
    prev_STATE = 11
    STOP = False
    while True:
        #print "line follower %f" %time.time()
        # we don't need to start a thread if theres only one.....
        # print 'get dist: %f' % time.time()
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()
        # print 'get ir: %f' % time.time()
        if not left and not right:  # If both sensors are the on --> forward
            # pi2go.forward(speed)
            STATE = 00
        elif left and not right:  # If the left sensor is Off --> move right
            # pi2go.turnForward(speed+change, speed-change)
            STATE = 10
        elif right and not left:  # If the right sensor is off --> move left
            # pi2go.turnForward(speed - change, speed + change)
            STATE = 01
        else:  # stop
            # pi2go.stop()
            STATE = 11

        if time.time() - start > 0.2:
            dist = (int(pi2go.getDistance() * 10)) / 10.0
            #out_q.put(dist)
            start = time.time()

            if dist > 10:
                STOP = False
                STATE = 69
            else:
                STOP = True

        if STOP:
            pi2go.stop()
        if STATE == prev_STATE:
            pass
        elif STATE == 00:
            pi2go.forward(speed)
            # pi2go.stop()
        elif STATE == 10:
            pi2go.turnForward(speed + change, speed - change)
        elif STATE == 01:
            pi2go.turnForward(speed - change, speed + change)
        elif STATE == 11:
            # pi2go.forward(speed)
            pi2go.stop()

        prev_STATE = STATE
Example #19
0
# lf.py
# slow line follower using pi2go library
# Author : Zachary Igielman

import sys, time
import pi2go

pi2go.init()

pwmMax = 4095

fast = 40

try:
       while True:
                  if pi2go.irLeftLine() and pi2go.irRightLine():
                          pi2go.forward(fast)
                          pi2go.setAllLEDs(pwmMax, pwmMax, pwmMax)  # Turn LEDs White for Forwards
                          print('straight')
                  elif pi2go.irRightLine()==False:
                          pi2go.spinRight(fast)
                          pi2go.setAllLEDs(pwmMax, 0, 0) # Turn LEDs Red for Right
                          print('right')
                  elif pi2go.irLeftLine()==False:
                          pi2go.spinLeft(fast)
                          pi2go.setAllLEDs(0, 0, pwmMax) # Turn LEDs Blue to Left
                          print('left')
except KeyboardInterrupt:
       pi2go.setAllLEDs (0, 0, 0)
       pi2go.cleanup()
       sys.exit()
Example #20
0
File: IRTest.py Project: bethias/pi
import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
    print "Running on Pi2Go"
else:
    print "Running on Pi2Go-Lite"
count = 1

try:
    while True:
        print 'Count: ', count
        print 'Left:', pi2go.irLeft()
        if vsn == 1:
            print 'Centre:', pi2go.irCentre()
        print 'Right:', pi2go.irRight()
        print 'Line left', pi2go.irLeftLine()
        print 'Line right', pi2go.irRightLine()
        print
        count += 1
        time.sleep(1)

except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
Example #21
0
#!/usr/bin/env python
#Simply prints the state of the line follower sensors

# Must be run as root - sudo python .py 

import time, pi2go

pi2go.init()

vsn = pi2go.version()
if vsn == 1:
  print "Running on Pi2Go"
else:
  print "Running on Pi2Go-Lite"


try:
    while True:
        print 'Left:', pi2go.irLeftLine()
        print 'Right:', pi2go.irRightLine()
        print
        time.sleep(1)
                          
except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
    def on_message(self, message):
        
        #Messages are of the form: "MessageType/Instruction" hence each message
        #from scratch needs to be separated into is consistuent parts.
        print message
        msg= message.split("/")
        
        #MOTOR FUNCTIONS
        if msg[0]== 'stop':
            pi2go.stop()
        
        elif msg[0]== 'forward':
            pi2go.forward(float(msg[1]))
        
        elif msg[0]== 'reverse':
            pi2go.reverse(float(msg[1]))
    
        elif msg[0]== 'spinLeft':
            pi2go.spinLeft(float(msg[1]))
        
        elif msg[0]== 'spinRight':
            pi2go.spinRight(float(msg[1]))
    
        elif msg[0]== 'turnForward':
            pi2go.turnForward(float(msg[1]), float(msg[2]))
        
        elif msg[0]== 'turnReverse':
            pi2go.turnReverse(float(msg[1]), float(msg[2]))
        
        elif msg[0]== 'goM':
            pi2go.go(float(msg[1]), float(msg[2]))
        
        elif msg[0]== 'go':
            pi2go.go(float(msg[1]))

        # SERVO FUNCTIONS
        #elif msg[0]== 'startServos':
            #pi2go.startServos()

        #elif msg[0]== 'stopServos':
            #pi2go.stopServos()

        #elif msg[0]== 'setServos':
            #pi2go.setServo(msg[1],float(msg[2]))


        # LED FUNCTIONS
        #elif msg[0]== 'setLED':
            #pi2go.setLED(msg[1], msg[2], msg[3], msg[4])

        #elif msg[0]== 'setAllLEDs':
            #pi2go.setAllLEDs(msg[1], msg[2], msg[3])

        elif msg[0]== 'LsetLED':
            pi2go.LsetLED(msg[1], msg[2])

        # IR FUNCTIONS
        elif msg[0]== 'irLeft':
            val = pi2go.irLeft()
            self.write_message(str(val))
        
        elif msg[0]== 'irRight':
            val = pi2go.irRight()
            self.write_message(str(val))
    
        elif msg[0]== 'irLeftLine':
            val =pi2go.irLeftLine()
            self.write_message(str(val))
        
        elif msg[0]== 'irRightLine':
            val= pi2go.irRightLine()
            self.write_message(str(val))

        # ULTRASONIC FUNCTION
        elif msg[0]== 'ultraSonic':
            val=pi2go.getDistance()
            self.write_message(str(val))
Example #23
0
import pi2go, time

# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
import threading

# main loop
speed = 30
pi2go.init()

try:
    while True:
        if (pi2go.irLeftLine() == True and pi2go.irRightLine() == True):
            pi2go.go(speed * 0.94, speed)
        elif (pi2go.irLeftLine() == False):
            pi2go.spinLeft(speed)
        elif (pi2go.irRightLine() == False):
            pi2go.spinRight(speed)
        elif (pi2go.irLeftLine() == False and pi2go.irRightLine() == False):
            pi2go.stop()
        #print(pi2go.irLeftLine())
        #print(pi2go.irRightLine())
        #print('#######')
        #time.sleep(3)

finally:
    pi2go.cleanup()
      pi2go.go(movingSpeed, movingSpeed)
      
  # Turn right
  if (turnRight):
      # Turn right until right sensor doesn't recognize the line anymore 
      pi2go.go(30, -30)
      if (pi2go.irRightLine()):
          goForward = True
          turnLeft = False
          turnRight = False        
          turnAround = False
  # Turn left
  if (turnLeft):
      # Turn left until left sensor doesn't recognize the line anymore 
      pi2go.go(-30, 30)
      if (pi2go.irLeftLine()):
          goForward = True
          turnLeft = False
          turnRight = False       
          turnAround = False
          
  # Turn around
  while turnAround:     
      # Turn right fast until the both sensor detects a line    
      pi2go.go(70, -70)
 
      if (not pi2go.irRightLine()):
          rightSensorFoundTurnLine = True;
     
      if (not pi2go.irLeftLine() and rightSensorFoundTurnLine):
          # Both sensor detected a line, which means that we are over turned.
Example #25
0
if vsn == 1:
  print "Running on Pi2Go"
else:
  print "Running on Pi2Go-Lite"


try:
    while True:
	print 'obstacle'
	print 'irLeft', GPIO.input(11)
	print pi2go.irLeft()
	print 'irRight', GPIO.input(7)
	print pi2go.irRight()
	print 'irCentre', GPIO.input(13)
	print pi2go.irCentre()
	print 'line'
	print 'LeftLine', GPIO.input(12)
	print pi2go.irLeftLine()
	print 'RightLine', GPIO.input(15)
	print pi2go.irRightLine()
        #print 'Left:', pi2go.irLeftLine()
        #print 'Right:', pi2go.irRightLine()
	print
        time.sleep(5)
                          
except KeyboardInterrupt:
    print

finally:
    pi2go.cleanup()
Example #26
0
def line_follower():
    """
    State:
    L R
    1 1 - Both White  - Depends on P
    1 0 - Left White  - Turn Left
    0 1 - Right White - Turn Right
    0 0 - Both Black  - Go Forward

    P - previous State
    ------------------
    0 - Left
    1 - Right

    :return:
    """
    dist = 0
    speed = 70
    change = 20
    start = 0

    STATE = 00
    prev_STATE = 11
    STOP = False
    stop = False
    prev_stop = True
    while True:
        #print "line follower %f" %time.time()
        # print 'get dist: %f' % time.time()
        left = pi2go.irLeftLine()
        right = pi2go.irRightLine()
        # print 'get ir: %f' % time.time()
        if not left and not right:    # If both sensors are the on --> forward
            STATE = 00
        elif left and not right:          # If the left sensor is Off --> move right
            STATE = 10
        elif right and not left:         # If the right sensor is off --> move left
            STATE = 01
        else:
            STATE = 11

        if time.time() - start > 0.15:
            dist = (int(pi2go.getDistance()*10))/10.0
            print dist
            while dist < 25:
                dist = (int(pi2go.getDistance()*10))/10.0
                pi2go.stop()
                STATE = 69
                time.sleep(0.15)
                if stop == False:
                    pi2go.setAllLEDs(4095,0,0)
                stop = True
            start = time.time()
        stop = False
        if stop == prev_stop:
            pass
        elif stop == False:
		    pi2go.setAllLEDs(0,4095,0)
        
		
        
        if STATE == prev_STATE:
            pass
        elif STATE == 00:
            pi2go.forward(speed)
        elif STATE == 10:
            pi2go.turnForward(speed + change, speed - change)
        elif STATE == 01:
            pi2go.turnForward(speed - change, speed + change)
        elif STATE == 11:
            pi2go.stop()
        prev_stop = stop
        prev_STATE = STATE
Example #27
0
import pi2go
import time

pi2go.init()

# Here we set the speed to 60 out of 100 - feel free to change!
speed = 40
change = 25

try:

  stime = time.time()
  while True:
    # Defining the sensors
    left = pi2go.irLeftLine()
    right = pi2go.irRightLine()   
    ntime = time.time()

    # timecheck for distance         
    if ntime > (stime + 0.1):
      #print "%.5f" %time.time()
      dist = (int(pi2go.getDistance()*10))/10.0
      #print "%.5f" %time.time()
      stime = ntime
      
      # distance groups 
      if dist < 10: 
        pi2go.setAllLEDs(2000, 0, 0)
      elif dist > 20:
        pi2go.setAllLEDs(0, 0, 2000)
Example #28
0
def follow_line():
    """Follow a black line on a white background"""
    #SIM_PULSE = 0.035
    TURN_RATE = 50
    STEP_RATE = 1 #0.5
    SPEED_LINE = 35  
    logging.debug('Turn-rate={} Step-rate={}'.format(TURN_RATE, STEP_RATE))
    # start speed
    speed = SPEED_LINE
    logging.debug('speed={}'.format(speed)) 
    # start with no turn-rate
    turn = 0.0
    # count steps on this turn-rate; use to accelerate
    step = 1
    logging.debug('turn={} step={}'.format(turn, step))
    
    while not pi2go.getSwitch():  #action
        #logging.debug('time0={}'.format(time.time()))
        # turn left if left sensor detects dark line
        if not pi2go.irLeftLine():
            #logging.debug('Left')
            if turn > 0:
                step = 0
            #turn = -TURN_RATE
            #turn = max(turn - TURN_RATE, -100)
            speed = TURN_RATE-20
            turn = -TURN_RATE
        elif not pi2go.irRightLine():
            #logging.debug('     Right')
            if turn < 0:
                step = 0
            #turn = TURN_RATE
            #turn = min(turn + TURN_RATE, 100)
            speed = TURN_RATE-20
            turn = TURN_RATE
        else:
            # no change for now as line is between sensors
            #logging.debug('  None  ')
            speed = SPEED_LINE
            turn = 0
            #pass
        
        # accelerate
        step += 1
        #step = min(step + STEP_RATE, 100)
        #logging.debug("turn={} step={}".format(turn, step))
        
        leftspeed = max(min(speed + turn + step * STEP_RATE * my_sign(turn), 100), -100)
        rightspeed = max(min(speed - turn + step * STEP_RATE * my_sign(-turn), 100), -100)
        #logging.debug("turn={} step={} lspeed={} rspeed={}".format(turn, step, leftspeed, rightspeed))
        #logging.debug("lspeed={} rspeed={}".format(leftspeed, rightspeed))
        #pi2go.stop()
        #time.sleep(0.5)
        pi2go.go(leftspeed, rightspeed)
        
        # Ignore the time delay to sample as fast as possible.
        #logging.debug('time1={}'.format(time.time()))
        #time.sleep(SIM_PULSE)
        
        # IGNORE button code for now.
        #if getButton[0] = 1:
        #   # button is pressed to wait for release to flip action state
        #   while getButton[0] =1:
        #       time (0.1)
        #   action = start-stop(action)
    pi2go.stop()