Python sonarSpin示例

示例#1

文件： testSonar.py 项目： sooty1892/lego_storm

from robot import robot

robot= robot(0,0,0)
while True:
	rounds = float(input("Enter number of angle : "))
	robot.sonarSpin(rounds)

robot.terminate()

示例#2

文件： wallFollowTest.py 项目： sooty1892/lego_storm

import sys
import time
sys.path.insert(0, '../')
from robot import robot

robot = robot(0, 0, 0, True, True)
robot.enableSonar()

# (273,21,-90), (525,21,-90), (21,21,-90)

#robot.findDistance(63)
while True:
    robot.sonarSpin(110)
    robot.followWallBackwards(400, 25)
    time.sleep(2)
    robot.sonarSpin(-40)
    robot.followWallLeft(400, 25)
    time.sleep(2)
    robot.sonar_reset()

示例#3

文件： challenge.py 项目： dsl12/lego_storm

def routineRight():
    robot.sonarSpin(angle)
    robot.followWallBackwards(half/2, diswall)
    robot.sonarSpin(-(90+angle))
    distance = half_fromwall - robot.getSonarSingle()
    robot.sonarSpin(90+angle)
    robot.followWallBackwards(distance, diswall)
    robot.turnRightDeg(90)
    #turn sonar to wall and take measurement
    robot.sonarSpin(-(90+angle)) # i am looking at small wall
    runToWallAt21() 
    print "!! I AM AT POINT 2 !!"
    time.sleep(1)

    robot.forward(-42)
    robot.turnLeftDeg(90)
    robot.sonarSpin(90+angle)# turn back to look at left wall

    robot.followWallBackwards(half-earlystop, diswall)
    robot.sonarSpin(90-angle) # look at right wall 
    runToWallAt21(-1)

    robot.turnLeftDeg(90)
    distance = robot.getSonarSingle() - 21 + 2 
    robot.sonarSpin(-(90-angle)) #turn back for wall following

    robot.followWallBackwards(distance, diswall)
    print "!! I AM AT POINT 3 !!"
    time.sleep(1)

    robot.sonarSpin(-(angle*2))
    robot.followWallLeft(small,diswall)
    robot.turnRightDeg(90)
    robot.followWallLeft(full-earlystop, diswall)
    
    robot.sonarSpin(-(90-angle)) # look at right wall 
    runToWallAt21()

    robot.turnRightDeg(90)
    distance = robot.getSonarSingle() - 21
    robot.sonarSpin(90-angle) #turn back for wall following
    
    robot.followWallLeft(distance, diswall)
    print "!! I AM AT POINT 1 !!"

示例#4

文件： challenge.py 项目： dsl12/lego_storm

def routineLeft():
    robot.sonarSpin(-angle)
    robot.followWallLeft(half/2, diswall)
    robot.sonarSpin(90+angle)
    distance = half_fromwall - robot.getSonarSingle()
    robot.sonarSpin(-(90+angle))
    robot.followWallLeft(distance,diswall)
    robot.turnRightDeg(90)

    robot.sonarSpin(-(90-angle)) # 
    runToWallAt21()
    # robot.forward(42)
    print "!! I AM AT POINT 2 !!"
    time.sleep(1)

    robot.forward(-42)
    robot.turnLeftDeg(90)
    robot.sonarSpin(90-angle)

    robot.followWallLeft(half-earlystop, diswall)
    robot.sonarSpin(-(90-angle)) # look at right wall 
    runToWallAt21()
    robot.turnRightDeg(90)
    distance = robot.getSonarSingle() - 21
    robot.sonarSpin((90-angle)) #turn back for wall following
    
    robot.followWallLeft(distance, diswall)
    print "!! I AM AT POINT 1 !!"
    time.sleep(1)

    robot.sonarSpin(2*angle)
    robot.followWallBackwards(small,diswall)
    robot.turnLeftDeg(90)
    robot.followWallBackwards(full-earlystop, diswall)

    robot.sonarSpin((90-angle)) # look at right wall 
    runToWallAt21(-1)

    robot.turnLeftDeg(90)   
    distance = robot.getSonarSingle() - 21 + 2 
    robot.sonarSpin(-(90-angle)) #turn back for wall following

    robot.followWallBackwards(distance, diswall)
    print "!! I AM AT POINT 3 !!"

示例#5

文件： challenge.py 项目： dsl12/lego_storm

fastParams.pidParameters.maxOutput = 255
fastParams.pidParameters.k_p = 270
fastParams.pidParameters.k_i = 400
fastParams.pidParameters.k_d = 160

# (273,21,-90), (525,21,-90), (21,21,-90)

startTime = datetime.now()
measurements = robot.turnSonarTakingMeasurements()

# Change sonar to move FASTAAAAAAAAAAAAR!
robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams)

# place.get_Loc(measurements)

robot.sonarSpin(-90)
toturn = robot.getMeanAngle(measurements) -13
toturn = toturn if toturn <= 180 else toturn - 360 

robot.turnLeftDeg(toturn)

robot.forward(disout)




#### ***Decide location
sonarRight = robot.getSonarMeasurements(1)[0]


robot.sonarSpin(-180)

示例#6

文件： wallFollowTest.py 项目： dsl12/lego_storm

import sys
import time
sys.path.insert(0, '../')
from robot import robot

robot = robot(0, 0, 0, True, True)
robot.enableSonar()

# (273,21,-90), (525,21,-90), (21,21,-90)



#robot.findDistance(63)
while True:
    robot.sonarSpin(110)
    robot.followWallBackwards(400,25)
    time.sleep(2)
    robot.sonarSpin(-40)
    robot.followWallLeft(400,25)
    time.sleep(2)
    robot.sonar_reset();

示例#7

def routineLeft():
    robot.sonarSpin(-angle)
    robot.followWallLeft(half / 2, diswall)
    robot.sonarSpin(90 + angle)
    distance = half_fromwall - robot.getSonarSingle()
    robot.sonarSpin(-(90 + angle))
    robot.followWallLeft(distance, diswall)
    robot.turnRightDeg(90)

    robot.sonarSpin(-(90 - angle))  #
    runToWallAt21()
    # robot.forward(42)
    print "!! I AM AT POINT 2 !!"
    time.sleep(1)

    robot.forward(-42)
    robot.turnLeftDeg(90)
    robot.sonarSpin(90 - angle)

    robot.followWallLeft(half - earlystop, diswall)
    robot.sonarSpin(-(90 - angle))  # look at right wall
    runToWallAt21()
    robot.turnRightDeg(90)
    distance = robot.getSonarSingle() - 21
    robot.sonarSpin((90 - angle))  #turn back for wall following

    robot.followWallLeft(distance, diswall)
    print "!! I AM AT POINT 1 !!"
    time.sleep(1)

    robot.sonarSpin(2 * angle)
    robot.followWallBackwards(small, diswall)
    robot.turnLeftDeg(90)
    robot.followWallBackwards(full - earlystop, diswall)

    robot.sonarSpin((90 - angle))  # look at right wall
    runToWallAt21(-1)

    robot.turnLeftDeg(90)
    distance = robot.getSonarSingle() - 21 + 2
    robot.sonarSpin(-(90 - angle))  #turn back for wall following

    robot.followWallBackwards(distance, diswall)
    print "!! I AM AT POINT 3 !!"

示例#8

def routineRight():
    robot.sonarSpin(angle)
    robot.followWallBackwards(half / 2, diswall)
    robot.sonarSpin(-(90 + angle))
    distance = half_fromwall - robot.getSonarSingle()
    robot.sonarSpin(90 + angle)
    robot.followWallBackwards(distance, diswall)
    robot.turnRightDeg(90)
    #turn sonar to wall and take measurement
    robot.sonarSpin(-(90 + angle))  # i am looking at small wall
    runToWallAt21()
    print "!! I AM AT POINT 2 !!"
    time.sleep(1)

    robot.forward(-42)
    robot.turnLeftDeg(90)
    robot.sonarSpin(90 + angle)  # turn back to look at left wall

    robot.followWallBackwards(half - earlystop, diswall)
    robot.sonarSpin(90 - angle)  # look at right wall
    runToWallAt21(-1)

    robot.turnLeftDeg(90)
    distance = robot.getSonarSingle() - 21 + 2
    robot.sonarSpin(-(90 - angle))  #turn back for wall following

    robot.followWallBackwards(distance, diswall)
    print "!! I AM AT POINT 3 !!"
    time.sleep(1)

    robot.sonarSpin(-(angle * 2))
    robot.followWallLeft(small, diswall)
    robot.turnRightDeg(90)
    robot.followWallLeft(full - earlystop, diswall)

    robot.sonarSpin(-(90 - angle))  # look at right wall
    runToWallAt21()

    robot.turnRightDeg(90)
    distance = robot.getSonarSingle() - 21
    robot.sonarSpin(90 - angle)  #turn back for wall following

    robot.followWallLeft(distance, diswall)
    print "!! I AM AT POINT 1 !!"

示例#9

fastParams.pidParameters.maxOutput = 255
fastParams.pidParameters.k_p = 270
fastParams.pidParameters.k_i = 400
fastParams.pidParameters.k_d = 160

# (273,21,-90), (525,21,-90), (21,21,-90)

startTime = datetime.now()
measurements = robot.turnSonarTakingMeasurements()

# Change sonar to move FASTAAAAAAAAAAAAR!
robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams)

# place.get_Loc(measurements)

robot.sonarSpin(-90)
toturn = robot.getMeanAngle(measurements) - 13
toturn = toturn if toturn <= 180 else toturn - 360

robot.turnLeftDeg(toturn)

robot.forward(disout)

#### ***Decide location
sonarRight = robot.getSonarMeasurements(1)[0]

robot.sonarSpin(-180)

sonarLeft = robot.getSonarMeasurements(1)[0]

if sonarRight > 30: