def decideNextAction():
try:
SonarFilter.update()
b = ActionCommand.Body()
h = ActionCommand.Head()
l = ActionCommand.LED()
if Helpers.localised():
l.leftEye(0, 1, 1)
elif Helpers.unLocalised():
l.leftEye(1, 0, 1)
else:
l.leftEye(0, 0, 1)
if Robot.vNumBalls() > 0:
l.rightEye(0, 1, 0)
elif Helpers.foundBall():
l.rightEye(1, 1, 0)
else:
l.rightEye(1, 0, 0)
from math import radians
b.actionType(ActionCommand.Body.STAND)
skillInstance.execute(b, h, l)
Robot.setAction(h, b, l)
# print (b, h, l)
except KeyboardInterrupt:
print "###########################"
print "## SIGINT RECEIVED ##"
print "## BY PYTHON ##"
print "## ATTEMPTING SHUTDOWN ##"
print "###########################"
Robot.attemptShutdown()
def getNewPopulation(population):
wheel = createWheel(population)
newPopulation = []
for j in range(POPULATION_SIZE):
''' You can change the behaviour of the algorithm here'''
parent1 = population[ selectParent( wheel ) ]
''' either continue with original solution '''
if random.random() <= CROSSOVER_PROB:
newPopulation.append( parent1 )
continue
parent2 = population[ selectParent( wheel ) ]
''' create a new one '''
child = Robot.crossover( parent1, parent2 )
''' mutate new solution, or mutate original solution '''
Robot.mutate( child, MUTATION_PROB )
newPopulation.append( child )
''''''
return newPopulation
def close_to_the_real_dist(robot,epsilon_Stop_Simlashian,epsilonRMS,Onestep):
Tempx=robot.tempX
Tempy=robot.tempY
#while we dont close to epsilon
TempRms=RMS(Tempx,Tempy,robot.id)
if(TempRms==0):
return
startRms=TempRms
if(TempRms>RMS(Tempx,Tempy+Onestep,robot.id)and Robot.isOKtoMOVE(Tempx,Tempy+Onestep,robots)):# move up
Tempy= Tempy + Onestep
TempRms=RMS(Tempx,Tempy+Onestep,robot.id)
elif (TempRms > RMS(Tempx, Tempy - Onestep,robot.id) and Robot.isOKtoMOVE(Tempx, Tempy- Onestep, robots)): # move Down
Tempy = Tempy - Onestep
TempRms = RMS(Tempx, Tempy - Onestep,robot.id)
elif (TempRms > RMS(Tempx + Onestep, Tempy,robot.id)and Robot.isOKtoMOVE(Tempx + Onestep,Tempy,robots)): # move R
Tempx = Tempx + Onestep
TempRms = RMS(Tempx + Onestep, Tempy,robot.id)
elif(TempRms>RMS(Tempx-Onestep,Tempy,robot.id)and Robot.isOKtoMOVE(Tempx - Onestep,Tempy,robots)):#movw L
Tempx = Tempx - Onestep
TempRms =RMS(Tempx - Onestep, Tempy,robot.id)
else:#all is not ok! go with anathr step
robots[robot.id].oneStepRobot=robots[robot.id].oneStepRobot+5
robots[robot.id].tempY = Tempy
robots[robot.id].tempX = Tempx
#if is colse enough updata robot = make tree and tempX tempY
if((fabs(Tempx-robots[robot.id].x)<=epsilon_Stop_Simlashian) and (fabs(Tempy-robots[robot.id].y)<=epsilon_Stop_Simlashian)and epsilonRMS>TempRms and not robot.isTree):
i= (int)(robot.id)
robots[i].tempY = Tempy
robots[i].tempX = Tempx
robots.insert(i,Robot.Robot(robot.id, robot.tempX, robot.tempY,canvas,True,robots))
#print "ok is a tree now : Tempx",Tempx,"Tempy",Tempy
robots.remove(robot)
# print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!",robot.id
return
def main():
if len(sys.argv) > 2 and sys.argv[1] == '-filt':
filtFile = sys.argv[2]
strategy = FileStrategy(filtFile = filtFile)
motionFunction, logInfo = strategy.getNext()
elif len(sys.argv) > 2 and sys.argv[1] == '-sine':
sineModel5Params = [eval(xx) for xx in sys.argv[2].split()]
print 'Using SineModel5 with params: ', sineModel5Params
motionFunction = lambda time: SineModel5().model(time,
parameters = sineModel5Params)
else:
#filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00014_008_filt'
filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00004_007_filt'
strategy = FileStrategy(filtFile = filtFile)
motionFunction, logInfo = strategy.getNext()
#runman = RunManager()
#runman.do_many_runs(strategy, SineModel5.typicalRanges)
timeScale = .3
motionFunctionScaled = scaleTime(motionFunction, timeScale)
robot = Robot()
robot.run(motionFunction, runSeconds = 8, resetFirst = False,
interpBegin = 2, interpEnd = 2)
def update():
global lastSonarValues
leftSonar = Robot.sensorValues()['sonar'][0]
rightSonar = Robot.sensorValues()['sonar'][10]
lastSonarValues[0].append(leftSonar)
lastSonarValues[1].append(rightSonar)
if len(lastSonarValues[0]) > 10:
lastSonarValues[0].pop(0)
if len(lastSonarValues[1]) > 10:
lastSonarValues[1].pop(0)
def __init__(self):
self.lastYaw = 0.0
self.lastYawDiff = 0.0
self.lastPitch = 0.0
self.lastDist = 0
self.lastDistDiff = 0
self.usingTopCamera = False
Robot.setCamera(WhichCamera.BOTTOM_CAMERA)
def focusOnCoord(coords, **kwargs): #Arguments: targetFocus (The pixel location where you want the target to be alligned. Default is the center of the screen
targetFocus = kwargs.get('targetFocus', [screenDimensions[0] / 2, screenDimensions[1] / 2])
if len(coords) == 0:
print "Trouble focusing on coord:", coords
return
diffX = coords[0] - targetFocus[0]
diffY = coords[1] - targetFocus[1]
ratio = (Robot.pos['height'] + 60.0) / (V.heightMax + 60.0)
#print Robot.pos['height']
#ratio = 1 #for not, while I get THAT working...
xMove = (diffX / (V.pixelsPerX ))
yMove = (diffY / (V.pixelsPerY ))
print "Xmove: ", xMove, "Ymove: ", yMove
Robot.moveTo(rotation = yMove/2, stretch = xMove*2, waitForRobot = True)
def doPort(self, e=None):
""" open a serial port """
if self.port == None:
self.findPorts()
dlg = wx.SingleChoiceDialog(self,'Port (Ex. COM4 or /dev/ttyUSB0)','Select Communications Port',self.ports)
#dlg = PortDialog(self,'Select Communications Port',self.ports)
if dlg.ShowModal() == wx.ID_OK:
self.port = self.ports[dlg.GetSelection()]
print "Opening port: " + self.ports[dlg.GetSelection()]
try:
#TODO: switched to RobotPi code below
'''self.port = Driver(self.ports[dlg.GetSelection()], 38400, True) # w/ interpolation
self.panel.port = self.port
self.panel.portUpdated()
self.sb.SetStatusText(self.ports[dlg.GetSelection()] + "@38400",1)
'''
self.robot = Robot(expectedIds=range(self.project.count), skipInit=True)
self.robot.initServos(self.port)
self.panel.port = self.port
#self.panel.portUpdated()
if hasattr(self.panel, 'deltaTButton'): self.panel.deltaTButton.Enable()
self.sb.SetStatusText(self.ports[dlg.GetSelection()] + "@" + str(BAUD),1)
except RobotFailure:
self.port = None
self.sb.SetBackgroundColour('RED')
self.sb.SetStatusText("Could Not Open Port",0)
self.sb.SetStatusText('not connected',1)
self.timer.Start(20)
dlg.Destroy()
def nearRealDist(robot ,nearEpsilon ,epsilonRMS ,oneMove):
tx = robot.tmpx
ty = robot.tmpy
trms=RMS(tx ,ty ,robot.Id)
if(trms == 0):
return
if(trms > RMS(tx, ty + oneMove, robot.Id) and
Robot.checkTrans(tx, ty + oneMove, robots_arr)):
ty = ty + oneMove
trms=RMS(tx, ty + oneMove, robot.Id)
elif (trms > RMS(tx, ty - oneMove, robot.Id) and
Robot.checkTrans(tx, ty - oneMove, robots_arr)):
ty = ty - oneMove
trms = RMS(tx, ty - oneMove,robot.Id)
elif (trms > RMS(tx + oneMove, ty, robot.Id)and
Robot.checkTrans(tx + oneMove, ty, robots_arr)):
tx = tx + oneMove
trms = RMS(tx + oneMove, ty, robot.Id)
elif(trms>RMS(tx - oneMove, ty, robot.Id) and
Robot.checkTrans(tx - oneMove, ty, robots_arr)):
tx = tx - oneMove
trms = RMS(tx - oneMove, ty, robot.Id)
else:
robots_arr[robot.Id].oneRobotMove = robots_arr[robot.Id].oneRobotMove + 5
robots_arr[robot.Id].tmpy = ty
robots_arr[robot.Id].tmpx = tx
if((fabs(tx-robots_arr[robot.Id].x) <= nearEpsilon) and
(fabs(ty-robots_arr[robot.Id].y) <= nearEpsilon)and
epsilonRMS > trms and not robot.isStatic):
i = (int)(robot.Id)
robots_arr[i].tmpy = ty
robots_arr[i].tmpx = tx
robots_arr.insert(i,Robot.Robot(robot.Id, robot.tmpx, robot.tmpy,canvas,True,robots_arr))
robots_arr.remove(robot)
return
def focusOnTargetManual(target, **kwargs): #Arguments: targetFocus (The pixel location where you want the target to be alligned. Default is the center of the screen
print "focusOnTargetManual() THIS FUNCTION IS DEPRECATED, remove soon!"
targetFocus = kwargs.get('targetFocus', [screenDimensions[0] / 2, screenDimensions[1] / 2]) #The XY value of WHERE on the screen we want the robot to focus on.
moveIncrement = 3
while True: #Robot arm will slowly approach the target`
coords = objTracker.getTargetCenter(target)
if len(coords) == 0: continue #DO NOT MOVE. Prevents empty coords from continuing.
xMove = 0
yMove = 0
if not Common.isWithinTolerance(coords[0], targetFocus[0]):
if coords[0] < targetFocus[0]: xMove -= moveIncrement
if coords[0] > targetFocus[0]: xMove += moveIncrement
if not Common.isWithinTolerance(coords[1], targetFocus[1]):
if coords[1] < targetFocus[1]: yMove -= moveIncrement
if coords[1] > targetFocus[1]: yMove += moveIncrement
if Common.isWithinTolerance(coords[1], targetFocus[1]) and Common.isWithinTolerance(coords[0], targetFocus[0]):
break #Once the target has been focused on, quit
Robot.moveTo(x = xMove, y = -yMove)
def execute(self, b, h, l):
Robot.setCamera(self.waypoints[self.state][3])
if Robot.lBallLostCount() <= 15:
return True
h.pitch(self.waypoints[self.state][0])
h.yaw(self.waypoints[self.state][1] * 1.0)
h.yawSpeed(self.waypoints[self.state][2])
h.isRelative(False)
h.pitchSpeed(.2)
realPitch = Robot.sensorValues()['joints']['angles'][Joints.HeadPitch]
realYaw = Robot.sensorValues()['joints']['angles'][Joints.HeadYaw]
if abs(realPitch - h._pitch) < radians(10) and abs(realYaw - h._yaw) < radians(10):
self.state = (self.state + 1) % len(self.waypoints)
if self.state == 0:
return False
return True
def execute(self, b, h, l):
if Robot.lBallLostCount() > 15:
self.scanForBallSkill.execute(b, h, l)
b.actionType(ActionCommand.Body.WALK)
# debug leds
l.leftEye(1, 0, 0)
l.rightEye(1, 0, 0)
else:
self.gotoBallSkill.execute(b, h, l)
# debug leds
l.leftEye(0, 1, 0)
l.rightEye(0, 1, 0)
def get_atme(cookies):
atme_url = 'http://tieba.baidu.com/i/yoururl/atme'
atme_request = requests.get(atme_url, cookies=cookies)
atme_content = atme_request.text
atme_pattern = re.compile(r'<div class="atme_text clearfix j_atme">\s+(.+?)\s+(.+?)\s+</div>')
atme_user_pattern = re.compile(r'<div class="atme_user">.*?target="_blank">(.*?)</a></div>')
atme_content_pattern = re.compile(r'<div class="atme_content">.*?target="_blank">(.*?)</a></div>')
#atme_time_pattern = re.compile(r'<div class="feed_time">(\d+)-(\d+) (\d+):(\d+)</div>')
atme_keyword_pattern = re.compile(r'召唤逗比')
atme_url_pattern = re.compile(r'href="/p/(\d+)')
atme_url_all_pattern = re.compile(r'<a href="(.+?)" target="_blank">')
atme_names_pattern = re.compile(r'<a class="itb_kw" href=".+?target="_blank">.+?</a>')
atme_name_unicode_pattern = re.compile(r'href="/f\?kw=(.+?)"')
atme_name_pattern = re.compile(r'target="_blank">(.+?)</a>')
atmes = atme_pattern.findall(atme_content)
names = atme_names_pattern.findall(atme_content)
post = []
last_urls = []
post_log = open('./last_post.log', 'r')
for urls in post_log.readlines():
last_urls.append(urls[:-1])
post_log.close()
#print last_urls
count = 0
for atme in atmes:
#print type(atme[1].encode('utf-8'))
#print atme[1].encode('utf-8')
#print atme
name_unicode = atme_name_unicode_pattern.search(names[count]).group(1)
name = atme_name_pattern.search(names[count]).group(1)[:-1]
#print name
count += 1
url_all = atme_url_all_pattern.search(atme[0]).group(1)
#print url_all
#print last_urls
if atme_keyword_pattern.search(atme[1].encode('utf-8')) and url_all not in last_urls:
tmp_url = atme_url_pattern.search(atme[1]).group(1)
tmp_user = atme_user_pattern.search(atme[0]).group(1)[:-1]
tmp_content = atme_content_pattern.search(atme[1]).group(1)
tmp_new_content = tmp_content.replace(u'<b>@yourname</b> \u53ec\u5524\u9017\u6bd4 ', '')
tmp_respond = Robot.robot(tmp_new_content, tmp_user)
user = tmp_user
urls_all = url_all
#print urls_all
respond = tmp_respond
urls = tmp_url
user_and_content = [user, urls_all, respond, urls, name, name_unicode]
post.append(user_and_content)
return post
def main(args):
if args.filt:
filtFile = args.filt
strategy = FileStrategy(filtFile = filtFile)
motionFunction, logInfo = strategy.getNext()
elif args.sine:
sineModel5Params = [eval(xx) for xx in args.sine.split()]
print 'Using SineModel5 with params: ', sineModel5Params
motionFunction = lambda time: SineModel5().model(time,
parameters = sineModel5Params)
else:
# args.gait
strategy = TimedFileStrategy(posFile = args.gait)
motionFunction, logInfo = strategy.getNext()
# Old defaults
#filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00014_008_filt'
#filtFile = '../results/hyperneatTo20gens_101/neat_110115_175446_00004_007_filt'
#filtFile = '/home/team/results/kyrre_1.txt.cut'
#strategy = FileStrategy(filtFile = filtFile)
#runman = RunManager()
#runman.do_many_runs(strategy, SineModel5.typicalRanges)
#timeScale = .3
print 'scaling by', args.scale
motionFunctionScaled = scaleTime(motionFunction, args.scale)
robot = Robot()
robot.run(motionFunctionScaled, runSeconds = 10, resetFirst = False,
interpBegin = 2, interpEnd = 2)
def execute(self, b, h, l):
if Robot.lBallLostCount() > 15 :
self.scanForBallSkill.execute(b, h, l)
return False
else:
self.trackBallSkill.execute(b, h, l)
heading = Robot.vRrBallLocation()['heading']
distance = Robot.vRrBallLocation()['distance']
distance -= self.goalDistance
b.actionType(ActionCommand.Body.FAST)
if degrees(heading) < abs(30):
b.turn(heading/2.0)
b.forward(int(distance/2.5))
b.left(0)
else:
b.forward(0)
b.left(0)
b.turn(heading/2.0)
if distance < 15 and abs(heading) < radians(5):
return True
return False
def isObjectGrabbed():
currentWrist = Robot.pos["wrist"]
Robot.moveTo(wrist = -40, relative = False)
Robot.moveTo(wrist = 20, relative = False)
sleep(.01)
movement = objTracker.getMovement()
Robot.moveTo(wrist = currentWrist, relative = False)
print "isObjectGrabbed(", locals().get("args"), "): Movement = ", movement
return movement
import numpy as np
from Robot import *
from ParticleFilter import *
from map import *
List_plugged_motors = [[0, "Right"], [3, "Left"]]
# Information displayed, to be sure software motor configuration matches reality
print("The configuration states that plugged motors are :\n")
for i in List_plugged_motors:
print("Port ", i[0], " for ", i[1], " side motor\n")
# Necessary for the instantiation of the Robot, empty for the moment
List_plugged_sensors = [[2, "SENSOR_ULTRASONIC", 2]]
# Create the instance of the robot, initialize the interface
Simple_robot = Robot(List_plugged_motors, List_plugged_sensors)
# Enable its motors and sensors
Simple_robot.motor_init()
Simple_robot.sensor_init()
# Create a ParticleFilter
PF = ParticleFilter(100)
# Start point coordinates in the arena, which are passed to the robot, and to the particle filters
start_point = [84., 30.]
Simple_robot.set_x(start_point[0])
Simple_robot.set_y(start_point[1])
PF.particles = np.array([[start_point[0], start_point[1], 0]] *
PF.num_particles)
import Robot as robot
robotTom = robot.Robot('Tom', 2)
# robotTom.say_hi()
print(robotTom.getId())
#print lp
xc,yc = (0, 0)
alp = []
for p in lp:
x,y = p
xc += x
yc += y
alp.append((xc, yc))
#-- Add the first point
#alp.append(alp[0])
#-- Create the robot and set the connection
r = Robot.Robot(l1 = 73.0, l2 = 97.0)
r.test()
r.connect("/dev/ttyUSB0")
time.sleep(2)
r.speed(100)
#--- Create the figure with the absolute points!
f = fig.Figure(alp)
#-- Center the figure at origin (0,0)
f.center()
#-- Flip the y axis
f.flipy()
def __init__(self):
threading.Thread.__init__(self, daemon=True)
self.robot = Robot.Robot()
self._joystick = None
self._EXIT = False
class Follow_Thread():
def __init__(self, follow):
pass
def run():
pass
# setting up the components
left_motor = Motor.Motor(31, 33, 24)
left_motor_t = Motor.Motor_Thread(left_motor)
right_motor = Motor.Motor(35, 37, 26)
right_motor_t = Motor.Motor_Thread(right_motor)
wheels = Wheels.Wheels(left_motor, right_motor)
print("CREATED WHEELS")
sensor_values = dict()
sensor_rate = 1
sensors = Ultrasonic_sensors.setup(sensor_rate, sensor_values)
print("CREATED sensors")
robot = Robot.Robot(wheels, sensor_values)
print("Created robot")
y = Follow(robot)
#z = Follow_Thread(y)
y.follow()
# Jaikrishna # Initial Date: June 24, 2013 # Last Updated: June 24, 2013 # # These files have been made available online through a Creative Commons Attribution-ShareAlike 3.0 license. # (http://creativecommons.org/licenses/by-sa/3.0/) # # http://www.dexterindustries.com/ # This code is for testing the BrickPi with a Lego Motor #from Moves import forward, turn from Robot import * from DrawSquare import * robot = Robot() SLEEP_TIME = 1.5 # sleep 1.5s between each move DISTANCE = 40 # move forward in cm DEGREES = 90 # turn to the right in degrees ADJUSTED_DISTANCE = 100 draw_square(40) for i in range(0,4): robot.forward(DISTANCE) time.sleep(SLEEP_TIME) robot.turn(DEGREES) time.sleep(SLEEP_TIME)
"""
from Robot import *
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
sleepytime = 1 # Time between readings in seconds
steps = 8
sonarPositionResults = []
sonarVelocityResults = []
sonarAccelorationResults = []
#touch = Touch(brick, PORT_4)
robot = Robot() # new nerve object for reading and controlling the motors and sensors
#robot.resetMotors() # resets the tachometer
initialPosition = robot.sonarReading()
robot.move() # moves the robot
for i in range(steps):
robot.motorPosition()
robot.motorVelocity(sleepytime)
robot.motorAcceloration(sleepytime)
robot.sonarReading()
robot.sonarVelocity(sleepytime)
"""
Created on Wed Apr 10 19:40:11 2013
@author: benjamin
"""
# Works great! =D
from Robot import *
robot = Robot()
robot.move(75)
#robot.turn(75, 1800)
for i in range(4):
robot.wait(1)
#print robot.tacho()
try:
print robot.tacho()
except:
print "Your balls ass method didn't work"
break
robot.stop()
def experiment():
rows = 5
cols = 5
coverage = 20
numbits = 10 # I may need more bits for my readngs.
numRounds = 10
numRuns = 1
minimum_distance = 7
#trainingRounds = numRounds/4
accuracies = []
sonarPositionResults = []
originalInputVector = InputVector(numbits)
inputVector = InputVector(0)
predictions = dict()
state = "Going Forwards"
# Repeat several times to increase activity:
# Seems to just extend the number of elements in inputVector by numbits (10) 3 times.
# Why not extend it by 3*numbits?
# I think becuase rather than make it 3 times as long, it actually repeats the vector three times,
# probably so as to, as said above, increase activity in those cells and aid learning.
# Good old repartition. In which case, I'm not sure I want to use this in my tests...
for i in range(3):
inputVector.extendVector(originalInputVector)
# Get a non-local variable and feed it to a local one for local manipulation.
desiredLocalActivity = DESIRED_LOCAL_ACTIVITY
# This sets up a new region, called newRegion,
# a variable called ouputVector, which calls a method to find just that,
# and a variable for the number of correct prodicitons, initialised as 0.
newRegion = Region(rows,cols,inputVector,coverage,desiredLocalActivity)
outputVector = newRegion.getOutputVector()
correctBitPredictions = 0
robot = Robot()
starting_position = robot.sonarReading()
robot.move()
# This is where the action starts. This loop forms the main body of the test.
# For every time round, an input is given, the CLA updates spacial and temporal
# poolers with this new input and an output is found.
for round in range(numRounds):
if state == "Kill Switch: Engage!":
break
end_this_round = False
stuck_counter = 0
print state
round_number = round+1
print ("Round: %d" % round_number) # Prints the number of the round
#printStats(inputString, currentPredictionString)
robot.move()
while end_this_round is False:
val = robot.sonarReading()
print ("Sonar: %d cm" % val)
sonarPositionResults.append(robot.currentSonarReading)
setInput(originalInputVector,val) # These next few lines convert the inputs and outputs from integers to bitstrings,
inputString = inputVector.toString() # so that the CLA can handle them.
outputString = outputVector.toString()
#print(originalInputVector.toString())
#for bit in originalInputVector.getVector():
#print(bit.bit)
#print('')
#print(inputString)
if outputString in predictions: # If output string has been seen before,
currentPredictionString = predictions[outputString]
# summon the last input that caused that prediction and make it the "currentPredictionString"? That's confusing...
else:
currentPredictionString = "[New input]" # If not seen before,
predictions[outputString] = inputString # Update the i/o record with the new relationship
#if (round > trainingRounds):
correctBitPredictions += stringOverlap(currentPredictionString, predictions[outputString])
# without training rounds, stringOverlap will be trying to compare binary stings with the string 'New input'. So correct BitPredictions is going to be 0 for a while,
# until inputs start repeating.
newRegion.runCLA() # The CLA bit!
numRuns += 1
#printColumnStats(newRegion)
accuracy = float(correctBitPredictions)/float(30*numRuns)
# Times thirty becuase it's measuring the correct prediction of BITS not whole bit-strings, and there are 30 bits per input.
# This makes sense as bits have semantic meaning where as bit-strings dont!
accuracies.append(accuracy)
if robot.killSwitch() == True: # This will terminate all loops and move to the end of the program
end_this_round = True
state = "Kill Switch: Engage!"
print state
if state == "Going Forwards":
if robot.currentSonarReading <= minimum_distance:
stuck_counter += 1
if stuck_counter == 2: # This routine confirms that a wall is hit, then sends the robot back to the start position
robot.stop()
print "Stuck Reflex"
state = "Reversing"
print state
stuck_counter = 0
robot.move(-75)
if state == "Reversing":
if (starting_position-3) < robot.currentSonarReading < (starting_position+3): # Clean this up
state = "Going Forwards"
end_this_round = True
# With the experiment now over, stat summaries are now printed.
# for key in predictions:
# print("key: " + key + " predictions: " + predictions[key])
robot.stop()
#print("Accuracy: " + str(float(correctBitPredictions)/float(30*(numRounds-trainingRounds))))
#code below prints a graph of runs against accuracies
runs = np.arange(1, numRuns, 1)
plt.figure(1)
plt.subplot(211)
plt.grid(True)
plt.plot(runs, accuracies)
plt.plot(runs, accuracies, 'bo')
plt.ylabel('Accuracy (correct preditions/predictions)')
plt.title('Change in CLA accuracy of sonar position prediction')
plt.subplot(212)
plt.grid(True)
plt.plot(runs, sonarPositionResults, 'r-')
plt.plot(runs, sonarPositionResults, 'ro')
plt.ylabel('Sonar Readings (cm)')
plt.xlabel('Number of CLA runs')
plt.show()
def main(): global count, forward, shots if (Robot.isMoving() == False & count == 0): Robot.moveForward() if (Robot.collided() == True): Robot.moveBackward() forward = True if (count == 0): Robot.stop() if (shots < 10): shotRet = Robot.shot() if (shotRet == Robot.ShotOk): shots += 1 elif (shotRet == Robot.ShotNoMoreAmmo): Robot.invertMove() else: shots = 0 count = 50 forward = False elif (forward): count -= 1
move = False
finish = False
work = False
callibration = False
if __name__ == '__main__':
try:
measursment_list = []
with open("compass-offset.csv", "r") as f:
temp = f.readline().split(';')
x_csv_offset = float(temp[0])
y_csv_offset = float(temp[1])
i = 0
# first measurement and determination of the robot's position
position = Robot.TakeMeasurments(x_csv_offset, y_csv_offset)
position_mmaping[i] = position
old_x = 0.0
old_y = 0.0
measursment_list.append([old_x, old_y, 0.0, 0.0])
roud = Robot.SpecityTheDirection(position_mmaping[i])
i = i + 1
print("angle: " + str(roud))
Robot.MoveToAngle(roud, x_csv_offset, y_csv_offset)
work = True
while work:
start_dystans = Robot.sen.distance()
Robot.start_Time = time.time()
rospy.init_node('rosmain', anonymous=True)
rospy.Subscriber("uwb", Range, callback)
#rospy.spin() #Enable this if only listener() is called
# ROS MAIN FUNCTION
if __name__ == '__main__':
'''
while (x == 0. and y == 0.):
listener() #Gets the data from the range beacon
'''
print 'test'
box = [[4.0, 2.3, 0.]]
box_z = 0.5 + 5.122 # height/2 of box + z of robots
box_size = 0.2
s = st.Spawner(positions=box, size=box_size, z=box_z)
rand = s.create_targets()
#m1 = model.LawnMower(name='Selam', box_locs=box,goal=[10.,-15.,0.])
m1 = robot.Robot(name='Selam', goal=[7., 1.], way_points=[[7., 1.]])
v1 = vr.RosView(m1, rand=rand)
v1 = vr.RosView(m1, rand=rand)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
v1.update()
rate.sleep()
sigma_z = 0.025
x = 0
z = 0
dt = 0.5
k = 0.5
t_x = 2
t_z = 2
ok_dist = 0.05
robot = []
true_robot = []
kalman = []
controls = []
positions = np.array([[1, 4], [4, 2], [3, 1]])
orientat = np.array([0.4, 4, 4])
for i in range(0, n_rob):
robot += [Robot.Robot(x, z, orientat[i], positions[i])]
robot[i].set_kalman(sigma_meas, sigma_x, sigma_z)
robot[i].set_controls(x_min, x_max, z_min, z_max, k, t_x, t_z, ok_dist)
true_robot += [Robot.Robot(x, z, robot[i].get_theta(), robot[i].get_pos())]
true_robot[i].set_kalman(sigma_meas, sigma_x, sigma_z)
true_robot[i].set_controls(x_min, x_max, z_min, z_max, k, t_x, t_z,
ok_dist)
positions2 = np.array([[-0.2, -4], [-4, 2]])
base = Robot.Robot(0, 0, 0, positions2[0])
end_node = Robot.Robot(0, 0, 0, positions2[1])
plt.plot(base.get_x_pos(), base.get_y_pos(), 'bo')
plt.plot(end_node.get_x_pos(), end_node.get_y_pos(), 'go')
for j in range(0, n_iter):
corr_idx = np.mod(j,
n_rob) # Decide which robot should correct its position
import Robot
r = Robot.rmap()
r.lm('task2')
def task():
for x in range(7):
r.up()
r.pt()
r.up()
r.rt()
r.pt()
r.dn()
r.dn()
r.pt()
r.rt()
r.up()
r.pt()
r.start(task)
root.title("control Ex3")
canvas.create_rectangle(100, 100, 200, 200, width=1, fill='black')
canvas.create_rectangle(300, 600,700 , 700, width=3, fill='black')
canvas.create_rectangle(330, 330,600 , 500, width=2, fill='gray')
##make random robots:
for i in range(0,100):
random.seed(100-i)
x=random.random()*1000
y=random.random()*750
while ((x>=95 and x<=205 and y>=95 and y<=205)or (x>=295 and x<=705 and y>=595 and y<=705)):
x = random.random() * 1000
y = random.random() * 750
if(i<15):
robots.insert(i,Robot.Robot(i,x,y,canvas,True,robots))##add to array
robots[i].tempX=x
robots[i].tempY = y
else:
robots.insert(i,Robot.Robot(i,x,y,canvas,False,robots))##add to array
canvas.pack(expand=YES, fill=BOTH)#Show canvas
###if Simulation Finish return true else return false
def IsSimulationFinish(robots,epsilon):
print 'hi'
for r in robots:
if(r.id==64):
continue
from cs1lib import *
from Robot import *
rt = 4
if __name__ == '__main__':
env = Environment(500, 500)
robot = Robot(250, 250, 50, 3)
def draw():
global rt
rt += 1
robot.update_angles(rt)
end_points = robot.get_end_points()
count = 0
for i in range(len(end_points) - 1):
set_fill_color(.00 + count, .00 + count, .00 + count)
#set_fill_color(0.1, 0.8, 0.1)
p1 = end_points[i]
p2 = end_points[i + 1]
draw_circle(p1.x, p1.y, 5)
draw_polygon([(p1.x, p1.y), (p2.x, p2.y)])
count += .2
start_graphics(draw, width=env.width, height=env.height)
def __init__(self,
height,
width,
obstacles,
numRobots,
initLocs,
R=10,
k=10,
T=10,
base=[0, 0]):
# Initialize the grid world
self.gridworld = GridWorld.GridWorld(height, width, obstacles)
self.centroid = []
self.cluster = kmeans.kmeans()
# Initialize a list of robots
self.robots = [Robot.Robot(j + 1, -1, -1) for j in range(numRobots)]
# Initialize the starting location of each robot
i = 0
#self.allotted=Cell.Cell(0,0)
for initLoc in initLocs:
# If any robot is placed out of bounds or on an obstacle, print an error message and exit
currentPoint = (initLoc[0], initLoc[1])
if not self.gridworld.inBounds(
currentPoint) or not self.gridworld.passable(currentPoint):
print 'Initial location', currentPoint, 'is not possible'
sys.exit(-1)
# Otherwise, modify the current location of the robot to currentPoint
self.robots[i].setLocation(initLoc[0], initLoc[1])
# Update that particular grid cell's occupied status
self.gridworld.cells[initLoc[0]][initLoc[1]].occupied = True
self.gridworld.cells[initLoc[0]][initLoc[1]].visited = True
i += 1
# Initialize other parameters of the algorithm
# Height of the Grid
self.height = height
# Width of the Grid
self.width = width
# List of Clusters (obtained using K-Means clustering)
self.frontierClusters = []
# Number of Robots
self.numRobots = numRobots
# Commmunication Radius
self.R = R
# Parameter in Rooker's work (k is the number of configurations to be randomly generated)
self.k = k
# Time steps for which the algorithm is run
self.T = T
# Location of base station (Used only for simulating Rooker's work)
self.base = base
# Variable to indicate whether reclustering should be performed
self.reclusterFlag = True
# Centroids of clusters
self.centroids = []
# Number of time steps elapsed
self.t = 0
# Time taken to exhaust the frontier
self.completionTime = 0
# Set to True on completion
self.completedFlag = False
# List of Frontier Cells
self.frontier = []
# New Positions of each of the Robots
self.newPos = []
# Population of Configuration Changes
self.cfgc = []
# Number of Stalls (Used only for simulating Rooker's work)
self.stalls = 0
# Keeps track of whether the Final Stats were displayed
self.printedFinalStats = False
# Keeps track of Possible Configurations
self.possible = []
# Keeps track of Number of Cells Visited
# (Initialize this to self.numRobots, since the starting locations of the robots are considered visited)
self.visited = self.numRobots
self.sumNewVisited = numRobots
# Flag to switch between A* and Manhattan distance
self.aStarFlag = True
# Define value for infinity
self.infinity = 10000000
# Flag to switch between Hungarian and Greedy assignment
self.hungarianFlag = False
# Flag to switch between Greedy and Random Motion Planner
self.randomMotionPlan = False
# We also initialize an instance of AStar, which helps us in computing Manhattan distance
self.astar = AStar.AStar()
cnt += 1
HOST = '192.168.43.210'
PORT = 3000
socket.setdefaulttimeout(3)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind((HOST, PORT))
s.listen(12)
pygame.init()
display = pygame.display.set_mode((1100,600))
pygame.display.set_caption('IRA @Work')
clock = pygame.time.Clock()
robot = Robot(s, 500, 300, display)
arm = Arm(s)
robot_img = pygame.image.load('robot.png')
robot.image = pygame.transform.rotozoom(robot_img, 0, 0.25)
background_color = (50,50,50)
crashed = False
mouse_x = 0
mouse_y = 0
key_pressed = False
doScan = False
def run():
pass
def showDesks():
for i in range(len(desk)):
This program tests the ImageStitching.py library with the Robot.py library. The robot performs
several movements while taking pictures, and then sends those pictures to be stitched.
The final result is displayed on screen until the letter 'q' is pressed.
"""
picturePositions = [{'rotation': -13, 'stretch': 107},
{'rotation': -13, 'stretch': 42}]
# TAKE BASE PHOTO (This is the seed to all the rest of the photos)
Robot.moveTo(relative=False, **Robot.home)
Robot.moveTo(height=150, relative=False)
cap = cv2.VideoCapture(1)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 2000)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 2000)
_, throwaway = cap.read() #Wait for camera to adjust to lighting. Toss this frame (camera buffer)
cv2.imshow('main', throwaway)
cv2.waitKey(1000)
images_array = []
for index, position in enumerate(picturePositions):
Robot.moveTo(relative = False, **position)
cv2.waitKey(750)
# right. If it veers left then the _right_ motor is spinning faster so try
# setting RIGHT_TRIM to a small negative value, like -5, to slow down the right
# motor. Likewise if it veers right then adjust the _left_ motor trim to a small
# negative value. Increase or decrease the trim value until the bot moves
# straight forward/backward.
LEFT_TRIM = 0
RIGHT_TRIM = 0
# Create an instance of the robot with the specified trim values.
# Not shown are other optional parameters:
# - addr: The I2C address of the motor HAT, default is 0x60.
# - left_id: The ID of the left motor, default is 1.
# - right_id: The ID of the right motor, default is 2.
robot = Robot.Robot(addr=0x6f,
left_id=1,
right_id=2,
left_trim=LEFT_TRIM,
right_trim=RIGHT_TRIM)
# Now move the robot around!
# Each call below takes two parameters:
# - speed: The speed of the movement, a value from 0-255. The higher the value
# the faster the movement. You need to start with a value around 100
# to get enough torque to move the robot.
# - time (seconds): Amount of time to perform the movement. After moving for
# this amount of seconds the robot will stop. This parameter
# is optional and if not specified the robot will start moving
# forever.
robot.forward(150, 1.0) # Move forward at speed 150 for 1 second.
robot.left(200, 0.5) # Spin left at speed 200 for 0.5 seconds.
robot.forward(150, 1.0) # Repeat the same movement 3 times below...
def connect():
print("client has connected")
global robot
print('Initializing Robot')
robot = Robot.Robot()
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("num_objects_detected",Int32, queue_size=10)
print("__Init")
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/riabot/camera_node/image/compressed",CompressedImage,self.callback)
self.num_obj_detected = 0
def callback(self,data):
print("Call back")
center_circle = (0,0)
myimage = rgb_from_ros(data)
# Shantha: Full image size i captured is 1280 x 720.
# We want a 1200 x 300 cropped view in the bottom center of the full screenshot
myimage = myimage[40:1240, 420:720]
cv2.imwrite("cropped_ducks.jpg", myimage)
frame = myimage
im = frame
frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR)
# Convert BGR to HSV
hsv_blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# define range of yellow color in HSV
lower_blue = np.array([51, 97, 100])
upper_blue = np.array([56, 86, 100])
# Threshold the HSV image to get only blue colors
mask_blue = cv2.inRange(hsv_blue, lower_blue, upper_blue)
# Bitwise-AND mask and original image
res = cv2.bitwise_and(frame, frame, mask_blue)
res = cv2.cvtColor(res, cv2.COLOR_HSV2BGR)
cv2.imwrite('frame.jpg',frame)
cv2.imwrite('mask.jpg',mask_blue)
hsv_blue[mask_blue > 0] = ([56, 86, 100])
cv2.imwrite('hsv_yellow.jpg',hsv_blue)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray = cv2.medianBlur(gray, 5)
rows = gray.shape[0]
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8,
param1=100, param2=30,
minRadius=1, maxRadius=60)
circles_frame = frame.copy()
# ensure at least some circles were found
if circles is not None:
self.num_obj_detected = 1
print("Done! Found atleast one ducky")
if circles is None:
self.num_obj_detected = 0
print("Done! No ducky found")
def start(self):
print("num obj detected", self.num_obj_detected)
robot = Robot.Robot(left_trim=0, right_trim=0)
while not rospy.is_shutdown():
rate = rospy.Rate(2)
rate.sleep()
if self.num_obj_detected == 0:
robot.forward(100, 0.5)
if self.num_obj_detected >= 1:
robot.stop()
from math import *
import emptyWorld
import Robot
# Roboter in einer Welt positionieren:
myWorld = emptyWorld.buildWorld(30, 30)
myRobot = Robot.Robot()
myWorld.setRobot(myRobot, [2, 5.5, pi / 2])
myRobot.setTimeStep(0.1)
# Anzahl Zeitschritte n mit jeweils der Laenge T = 0.1 sec definieren.
# T laesst sich ueber die Methode myRobot.setTimeStep(T) einstellen.
# T = 0.1 sec ist voreingestellt.
n = 150
# Definiere Folge von Bewegungsbefehle:
motionCircle = [[1, -24 * pi / 180] for i in range(n)]
print(motionCircle)
# Bewege Roboter
for t in range(n):
# Bewege Roboter
motion = motionCircle[t]
print("v = ", motion[0], "omega = ", motion[1] * 180 / pi)
myRobot.move(motion)
sigma_motion = myRobot.getSigmaMotion()
# Gib Daten vom Distanzsensor aus:
distanceSensorData = myRobot.sense()
print("Dist Sensor: ", distanceSensorData)
def initRobot(LEFT_TRIM, RIGHT_TRIM):
print "initializing robot with l_trim = " + str(
LEFT_TRIM) + " and r_trim = " + str(RIGHT_TRIM)
robot = Robot.Robot(left_trim=LEFT_TRIM, right_trim=RIGHT_TRIM)
return robot
import random
from Robot import *
player = Robot()
npc = Robot()
name = input('>Enter your name: ')
print('Choose a robot type:')
robotType = input('>Fighter(1), Healer(2), Tank(3): ')
if(robotType == '1'):
player.createRobot(name, 15, 10, 100)
elif(robotType == '2'):
player.createRobot(name, 10, 15, 100)
elif(robotType == '3'):
player.createRobot(name, 10, 10, 130)
else:
player.createRobot(name, 15, 10, 100)
print('Invalid value, Fighter selected')
enemyType = random.randint(1, 3)
if(enemyType == 1):
npc.createRobot('RX-Fighter', 15, 10, 100)
elif(enemyType == 2):
npc.createRobot('RX-Healer', 10, 15, 100)
elif(enemyType == 3):
npc.createRobot('RX-Tank', 10, 10, 130)
play = True
turn = 0
while play:
# parcourir la liste path
for noNoeud in path:
# recuperer les coordoonnées x et y du noeud
xNoeud = carte.graph.listOfNodes[noNoeud].x
yNoeud = carte.graph.listOfNodes[noNoeud].y
# les ajouter à WPList
WPlist.append([xNoeud, yNoeud])
# robot
x0 = 0.0
y0 = 0.0
theta0 = 0.0
robot = rob.Robot(x0, y0, theta0)
k1 = 4.5
k2 = 12
# position control loop timer
positionCtrlPeriod = 0.2
timerPositionCtrl = tmr.Timer(positionCtrlPeriod)
# orientation control loop timer
orientationCtrlPeriod = 0.05
timerOrientationCtrl = tmr.Timer(orientationCtrlPeriod)
# list of way points: list of [x coord, y coord]
#WPlist = [ [2.0,2.0] ]
#Test de Passage d'un point de passage à l'autre
#WPlist = [ [2.0,2.0] , [2.0, -2.0] ,[-2.0, -2.0],[-2.0, 2.0],[2.0, 2.0],[0.0, 0.0]]
from Signature import *
from Robot import *
List_plugged_motors = [[0, "Right"], [3, "Left"]]
# Information displayed, to be sure software motor configuration matches reality
print("The configuration states that plugged motors are :\n")
for i in List_plugged_motors:
print("Port ", i[0], " for ", i[1], " side motor\n")
# Necessary for the instantiation of the Robot, empty for the moment
List_plugged_sensors = [[2, "SENSOR_ULTRASONIC", 2]]
# Create the instance of the robot, initialize the interface
Robot = Robot(List_plugged_motors, List_plugged_sensors)
# Enable its motors and sensors
Robot.motor_init()
Robot.sensor_init()
signatures = SignatureContainer(5)
def learn_location():
ls = LocationSignature()
Robot.characterize_location(ls)
idx = signatures.get_free_index()
if idx == -1: # run out of signature files
print "\nWARNING:"
print "No signature file is available. NOTHING NEW will be learned and stored."
print "Please remove some loc_%%.dat files.\n"
return
return (vl, vr)
#definizione ostacoli
ostacoli = [
Ostacolo((0.8, 0.5), 0.1, k_repulsiva_ostacoli),
Ostacolo((1.2, 1), 0.1, k_repulsiva_ostacoli),
Ostacolo((0.8, 2), 0.1, k_repulsiva_ostacoli),
Ostacolo((0.2, 1.5), 0.1, k_repulsiva_ostacoli)
]
#inizializzazione oggetti simulazione
START = (0.105, 0.105)
TARGET = (0.8, 2.5)
robot = rb.Robot(1.0, 5.0, whelebase)
robot.setPose(START[0], START[1], 0)
p = PotentialFieldControl(robot,
1,
0.22,
5,
2.84,
soglia=0.20,
ostacoli=ostacoli,
k_att=k_att)
#parametri temporali
delta_t = 0.01
t = 0
cnt = 0
robot.position[1])
robot.orientation = robot.orientation - 1
if self.__virage == "Gauche" and self.__orientation == 0:
robot.position = (robot.position[0],
robot.position[1] - self.__vitesse)
robot.orientation = robot.orientation + 1
elif self.__virage == "Gauche" and self.__orientation == 1:
robot.position = (robot.position[0] - self.__vitesse,
robot.position[1])
robot.orientation = robot.orientation + 1
elif self.__virage == "Gauche" and self.__orientation == 2:
robot.position = (robot.position[0],
robot.position[1] + self.__vitesse)
robot.orientation = robot.orientation + 1
elif self.__virage == "Gauche" and self.__orientation == 3:
robot.position = (robot.position[0] + self.__vitesse,
robot.position[1])
robot.orientation = robot.orientation + 1
if __name__ == "__main__":
case = Tapis((1, 2), 0, False)
print(case)
twonky = rob.Robot((1, 1), 1)
print(twonky)
case.effet(twonky)
print(twonky)
import Robot
import ActionCommand
import GameController
import Helpers
import sys
import SonarFilter
skill = Robot.pythonSkill()
exec "from skills.%s import %s" % (skill, skill)
skillInstance = eval(skill + "()")
def decideNextAction():
try:
SonarFilter.update()
b = ActionCommand.Body()
h = ActionCommand.Head()
l = ActionCommand.LED()
if Helpers.localised():
l.leftEye(0, 1, 1)
elif Helpers.unLocalised():
l.leftEye(1, 0, 1)
else:
l.leftEye(0, 0, 1)
if Robot.vNumBalls() > 0:
l.rightEye(0, 1, 0)
elif Helpers.foundBall():
l.rightEye(1, 1, 0)
else:
from Adafruit_IO import MQTTClient
import Robot
# globals
LEFT_TRIM = -4
RIGHT_TRIM = 0
ANGLE_MIN = -90
ANGLE_MAX = 90
THROTTLE_MAX = 255
THROTTLE_MIN = -255
USERNAME = '******'
ADAFRUIT_IO_KEY = "07939487d2614d2482d79902f43486a9" #adafruit io key
client = MQTTClient(USERNAME, ADAFRUIT_IO_KEY)
robot = Robot.Robot(left_trim=LEFT_TRIM,
right_trim=RIGHT_TRIM) #define the robot
angle = 0
throttle = 0
on = True
#map value from left range to right range
def translate(value, x_min, x_max, y_min, y_max):
#find slope
m = (y_max - y_min) / (x_max - x_min)
#solve for b
b = y_min - (m * x_min)
#get new value
def getStatistics( population ):
fitnesses = [ Robot.getFitness1(robot) for robot in population ]
return max(fitnesses), np.mean(fitnesses), min(fitnesses), np.std(fitnesses)
obstacles.append(
Circle(random.randrange(800), random.randrange(600),
random.randrange(40, 120)))
while not want_to_exit:
display.fill((255, 255, 255))
for event in pygame.event.get():
if event.type == pygame.QUIT:
want_to_exit = True
if event.type == pygame.MOUSEBUTTONUP:
robots.append(
Robot((random.randrange(0, displayWidth),
random.randrange(0, displayHeight))))
for i in range(len(robots)):
robots[i].move()
robots[i].show(display)
robots[i].applyBehaviour(robots, pygame.mouse.get_pos(), obstacles)
#print(rob.v.as_polar())
#print(rob.get_distance(obstacles))
for i in range(len(obstacles)):
obstacles[i].show(display)
pygame.display.update()
clock.tick(60)
def createWheel( population ):
cumulativeFitnesses = [0] * len(population)
cumulativeFitnesses[0] = Robot.getFitness1(population[0])
for i in range(1, len(population)):
cumulativeFitnesses[i] = cumulativeFitnesses[i - 1] + Robot.getFitness1(population[i])
return cumulativeFitnesses
def focusOnTarget(getTargetCoords, **kwargs): #Arguments: targetFocus (The pixel location where you want the target to be alligned. Default is the center of the screen
"""
:param getTargetCoords: FOCUSES ON TARGET
:param kwargs:
"targetFocus": (default [screenXDimension, screenYdimension]) HOLDS AN ARRAY THAT TELLS YOU WHERE ON THE SCREEN TO FOCUS THE OBJECT
"tolerance": (defaults to Variables default) How many pixels away till the object is considered "focused" on.
"ppX": (Default -1) The amount of pixels that one "stretch" move of the robot will move. This changes according to what height the robot is at
Which is the reason it is even adjustable. If this is not put in, the robot will target very slowly, but be most likely
to reach the target. So you are trading reliability (at all heights) for speed at a specific situation. Same for ppY, except
relating to the "rotation" movement of the robot.
"ppY": (Default -1)
:return: THE ROBOT WILL HAVE THE OBJECT FOCUSED ON THE PIXEL OF TARGETFOCUS
RECOMMENDED ppX and ppY VALUES FOR DIFFERENT HEIGHTS:
HEIGHT = MAX: ppX = 3, ppY = 12 (avg 3-4 moves)
HEIGHT = 70: ppX = 3.75 ppY = 15 (avg 2-4 moves)
HEIGHT = 0: ppx = 5.3 ppy = 38 (avg 3-5 moves)
"""
ppX = kwargs.get("ppX", -1) #Pixels per X move
ppY = kwargs.get("ppY", -1) #Pixels per Y move
targetFocus = kwargs.get('targetFocus', [screenDimensions[0] / 2, screenDimensions[1] / 2]) #The XY value of WHERE on the screen we want the robot to focus on.
tolerance = kwargs.get('tolerance', V.targetTolerance)
sign = lambda x: (1, -1)[x < 0] #sign(x) will return the sign of a number'
moveCount = 0 #The amount of moves that it took to focus on the object
while True: #Robot arm will slowly approach the target
try:
coords = getTargetCoords()
except NameError as e:
print "ERROR: focusOnTarget(", locals().get("args"), "): error (", e, "). Object not found. Leaving Function..."
raise #RE-Raise the exception for some other program to figure out
distance = ((coords[0] - targetFocus[0]) ** 2 + (coords[1] - targetFocus[1]) ** 2) ** 0.5 #For debugging
xMove = 0
yMove = 0
xDist = coords[0] - targetFocus[0]
yDist = coords[1] - targetFocus[1]
if abs(xDist) > .85 * tolerance: #I am stricter on x tolerance, bc the robot has more x incriments for movement.
xMove = sign(xDist) + (xDist / ppX) * (abs(xDist) > tolerance and not ppX == -1)
if abs(yDist) > tolerance:
yMove = sign(yDist) * .5 + (yDist / ppY) * (abs(yDist) > tolerance and not ppY == -1)
#print "focusOnTarget(", locals().get("args"), "): xMove: ", xMove, "yMove: ", yMove
if not (int(xMove) == 0 and int(yMove) == 0):
moveCount += 1
Robot.moveTo(rotation = yMove, stretch = xMove)
if yDist < tolerance * 2 or xDist < tolerance * 1.25: #Slow down a bit when approaching the target
sleep(.4)
else:
sleep(.2)
if Robot.pos["stretch"] == V.stretchMax or Robot.pos["stretch"] == V.stretchMin:
print "focusOnTarget(", locals().get("args"), "): Object out of Stretching reach."
break
if Robot.pos["rotation"] == V.rotationMax or Robot.pos["rotation"] == V.rotationMin:
print "focusOnTarget(", locals().get("args"), "): Object out of Rotating reach."
break
if moveCount >= 40:
raise Exception("TooManyMoves")
else:
print "focusOnTarget(", locals().get("args"), "): Object Targeted in ", moveCount, "moves."
break
#! /usr/bin/env python from Robot import * rr = Robot() worked = rr.shimmy() print 'Worked' if worked else 'Failed'
import Queue
import threading
import time
import Robot
import numpy as np
LEFT_TRIM = 0
RIGHT_TRIM = 0
robot = Robot.Robot(left_trim=LEFT_TRIM, right_trim=RIGHT_TRIM)
start1 = False
start2 = False
class listenerThread(threading.Thread):
def __init__(self, threadID, name, q):
threading.Thread.__init__(self)
self.threadID = threadID
self.name = name
self.q = q
def run(self):
print("Starting " + self.name)
get_input(self.name, self.q)
print("Exiting " + self.name)
def get_input(threadName, q):
global start1
global start2
while True:
data = input("->")
queueLock.acquire()
keep_going = True
#turn_p = 0.5
#turn_d = 0.01
#turn_i = 0.005
#turn_s = 0.005
#maxSpeed = 75
turn_p = 0.5
turn_d = 0.01
turn_i = 0.01
turn_s = 0.005
maxSpeed = 50
turnControl = pid.PIDController(turn_p, turn_i, turn_d, turn_s)
speedControl = pid.PIDController(0, 0, 0, turn_s)
robot = Robot.Robot()
move = True
i = 0
max_tries = 5
current_try = 1
prev_speed = maxSpeed
prev_steer = 0
def move_robot(angle_diff, framerate):
global prev_speed, maxSpeed
print("move robot", angle_diff, framerate)
MAX_ABS_SPEED = 255.0
#MIN_SPEED = 5.0
turn_p = maxSpeed / MAX_ABS_SPEED #0.5
def test2():
state = State()
r1 = Robot(20, 14)
r2 = Robot(20, 20)
r3 = Robot(20, 0)
r1.time = 14
r2.time = 8
r3.time = 8
r1.state = "on_delivery"
r2.state = "on_delivery"
r3.state = "on_delivery"
insort(state.delivery, r1)
insort(state.delivery, r2)
insort(state.delivery, r3)
pack1 = Pack(20, 0)
pack2 = Pack(20, 10)
pack3 = Pack(20, 20)
state.packs.append(pack1)
state.packs.append(pack2)
state.packs.append(pack3)
prod1 = Product(0, 0)
prod2 = Product(0, 10)
prod3 = Product(0, 20)
state.products.append(prod1)
state.products.append(prod2)
state.products.append(prod3)
ord1 = Order(prod1, pack1)
ord2 = Order(prod1, pack2)
ord3 = Order(prod1, pack3)
ord4 = Order(prod2, pack1)
ord5 = Order(prod2, pack2)
ord6 = Order(prod2, pack3)
ord7 = Order(prod3, pack1)
ord8 = Order(prod3, pack2)
ord9 = Order(prod3, pack3)
state.orders.append(ord1)
state.orders.append(ord2)
state.orders.append(ord3)
state.orders.append(ord4)
state.orders.append(ord5)
state.orders.append(ord6)
state.orders.append(ord7)
state.orders.append(ord8)
state.orders.append(ord9)
return state
def main(use_dummy_images, verbose_robot, verbose_options, use_stream):
robot = rob.Robot(use_dummy_images, verbose_level=verbose_robot, vrb=verbose_options)
robot.vim.save_stream = use_stream # Set whether to save the image for stream
robot.run()
from Robot import * from DrawSquare import * robot = Robot() #draw_square(40) draw_assignment() #robot.navigateToWaypoint(40, 0) #robot.navigateToWaypoint(40, 40) #robot.navigateToWaypoint(0, 40) #robot.navigateToWaypoint(0, 0) robot.navigateToWaypoint(50, 50) robot.navigateToWaypoint(50, -20) robot.navigateToWaypoint(0, 0)
# -*- coding: utf-8 -*-
from Robot import *
typeManette = "PS4"
continu = True
wr = Robot(4)
wr.initialiser_esc()
wr.set_joysticks(typeManette)
while continu:
repos = (wr.manette.get_axis(wr.gachetteGauche) == -1 or wr.manette.get_axis(wr.gachetteGauche) == 0) \
and wr.manette.get_axis(wr.stickGauche[0]) == 0 and wr.manette.get_axis(wr.stickGauche[1]) == 0 \
and wr.manette.get_axis(wr.stickDroit[0]) == 0 and wr.manette.get_axis(wr.stickDroit[1]) == 0
for event in pygame.event.get():
repos = (wr.manette.get_axis(wr.gachetteGauche) == -1 or wr.manette.get_axis(wr.gachetteGauche) == 0) \
and wr.manette.get_axis(wr.stickGauche[0]) == 0 and wr.manette.get_axis(wr.stickGauche[1]) == 0 \
and wr.manette.get_axis(wr.stickDroit[0]) == 0 and wr.manette.get_axis(wr.stickDroit[1]) == 0
if event.type == pygame.JOYBUTTONDOWN and event.button == wr.manette.select: #bouton select
continu = False
wr.fin_esc()
break #sortie du programme
if event.type == pygame.JOYAXISMOTION :
if repos :
wr.arret()
else :
if event.axis == wr.manette.gachetteGauche: #gachette gauche
wr.setAcc()
elif event.axis in wr.manette.stickDroit : #stick droit
wr.deplacement_std()
elif event.axis in wr.manette.stickGauche : #stick gauche
bestOfLastGen.append( best[-1] )
for i in range(GENERATION_COUNT):
averageBest[i] /= 20
return max(bestOfLastGen), np.mean(bestOfLastGen), min(bestOfLastGen), np.std(bestOfLastGen), averageBest
'''
def geneticAlgorithmMultipleRun( runCount ):
bestOfLast = [ 0 ] * runCount
meanOfLast = [ 0 ] * runCount
worstOfLast = [ 0 ] * runCount
for i in range( runCount ):
bestOfLast[i], meanOfLast[i], worstOfLast[i] = geneticAlgorithm()
return np.mean( bestOfLast ), np.mean( meanOfLast ), np.mean( worstOfLast )
# best, mean, worst = geneticAlgorithm()
'''
optimal = Robot.Robot()
optimal.strategy = Robot.simpleOptimal
print Robot.getFitness1( optimal )
print optimal.foundTreasures
print optimal.fitness
Robot.doDraw( Robot.simpleOptimal )
'''
print "Simple map with static initial population\n"
print Robot.values
#print "MAX_BOUND = 60\nMIN_BOUND = 20"
@author: Eric Gold
'''
def sinWaveMotion():
'''Returns a sine wave motion function'''
r = 300
sinF = lambda t : int(r * (sin(t)) + r)
cosF = lambda t : int(r * (cos(t)) + r)
f1 = lambda t : int(r * (sin(t*10)) + r)
f2 = lambda t : int(r * (sin(t) + cos(t)) + r)
#f = lambda t: (sinF(t), sinF(t), sinF(t), sinF(t), f2(t), f1(t), cosF(t), sinF(t), f2(t))
temp = copy.copy(POS_READY)
temp[0] = cosF
f = lambda t: temp
return f
def sinWave():
r = 300
p = 5
f = lambda t: (sin(t/p)*r+r,40,770,40,770,40,770,40,512)
return f
if __name__ == '__main__':
robot = Robot()
f = sinWave()
print "Beginning run"
robot.run(f,runSeconds=50)
