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)