API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 24th, 2014
47
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 16.42 KB | None | 0 0
raw download clone embed print report
  1.  
  2.  
  3.  
  4.  
  5. import urllib2
  6. import time
  7. import copy
  8. import math
  9. import libdw.sm as sm
  10. import libdw.sonarDist as sonarDist
  11. import libdw.util as util
  12. import libdw.search as s
  13. import libdw.gfx as gfx
  14. from soar.io import io
  15.  
  16. sonarPoses = [util.Pose(0.073, 0.105, 90 * math.pi / 180),
  17. util.Pose(0.13, 0.078, 41 * math.pi / 180),
  18. util.Pose(0.154, 0.03, 15 * math.pi / 180),
  19. util.Pose(0.154, -0.03, -15 * math.pi / 180),
  20. util.Pose(0.13, -0.078, -41 * math.pi / 180),
  21. util.Pose(0.073, -0.105, -90 * math.pi / 180)]
  22. sonarMax = 1.5
  23.  
  24. def getDistanceRight(sonarValues):
  25. return getDistanceRightAndAngle(sonarValues)[0]
  26.  
  27. def getDistanceLeft(sonarValues):
  28. return getDistanceLeftAndAngle(sonarValues)[0]
  29.  
  30. def getDistanceRightAndAngle(sonarValues):
  31. hits = []
  32. for spose, d in zip(sonarPoses, sonarValues):
  33. if d < sonarMax:
  34. hits.append((spose.x + d * math.cos(spose.theta), spose.y + d * math.sin(spose.theta)))
  35. else:
  36. hits.append(None)
  37. return distAndAngle(hits[4], hits[5])
  38.  
  39. def getDistanceLeftAndAngle(sonarValues):
  40. hits = []
  41. for spose, d in zip(sonarPoses, sonarValues):
  42. if d < sonarMax:
  43. hits.append((spose.x + d * math.cos(spose.theta), spose.y + d * math.sin(spose.theta)))
  44. else:
  45. hits.append(None)
  46. return distAndAngle(hits[1], hits[0])
  47.  
  48. def sonarHit(distance, sonarPose, robotPose):
  49. return robotPose.transformPoint(sonarPose.transformPoint(util.Point(distance, 0)))
  50.  
  51. def distAndAngle(h0, h1):
  52. if h0 and h1:
  53. linex, liney, lined = line(h0, h1)
  54. return (abs(lined), math.pi / 2 - math.atan2(liney, linex))
  55. elif h0:
  56. hx, hy = h0
  57. return (math.sqrt(hx * hx + hy * hy), None)
  58. elif h1:
  59. hx, hy = h1
  60. return (math.sqrt(hx * hx + hy * hy), None)
  61. else:
  62. return (sonarMax, None)
  63. return None
  64.  
  65. def line(h0, h1):
  66. h0x, h0y = h0
  67. h1x, h1y = h1
  68. dx = h1x - h0x
  69. dy = h1y - h0y
  70. mag = math.sqrt(dx * dx + dy * dy)
  71. nx = dy / mag
  72. ny = -dx / mag
  73. d = nx * h0x + ny * h0y
  74. return (nx, ny, d)
  75.  
  76. #-------------------------------------------------------------------------------
  77.  
  78. #this script creates a botlog.txt file in the same folder as the script if it does not exist.
  79. #instructions are contained in (botnamehere).trip
  80. #call (botnamehere).arrival() when bot reaches destination. this deletes the instruction entry from the instruction list and outputs to the log.
  81. #call (botnamehere).homecoming() when bot reaches X. this outputs to the log.
  82.  
  83. inputUrl = 'http://people.sutd.edu.sg/~oka_kurniawan/10_009/y2014/2d/tests/level2_1.inp'
  84. logFile = 'botlog.txt'
  85.  
  86. class bot:
  87. def __init__(self, url):
  88. self.continueMovement = True
  89. self.trip = [['X', None]]
  90. for line in urllib2.urlopen(url):
  91. for x in xrange(-(-int(line.strip('\n').split(' ')[1]) // 6)):
  92. if x+1 < (-(-int(line.strip('\n').split(' ')[1]) // 6)):
  93. self.trip += [[line.strip('\n').split(' ')[0]] + [6]] + [['X', None]]
  94. if x+1 == (-(-int(line.strip('\n').split(' ')[1]) // 6)):
  95. self.trip += [[line.strip('\n').split(' ')[0]] + [int(line.strip('\n').split(' ')[1]) % 6 if int(line.strip('\n').split(' ')[1]) % 6 != 0 else 6]] + [['X', None]]
  96. def __str__(self):
  97. return d
  98. def arrival(self):
  99. if self.trip[0][0] == 'X':
  100. if len(self.trip) == 1:
  101. desc = 'Arrived at X. Welcome home!'
  102. self.continueMovement = False
  103. else:
  104. self.trip.pop(0)
  105. desc = 'Arrived at X. Shipment received!'
  106. with open(logFile, 'a') as outf:
  107. outf.write('\n' + time.strftime('<%H:%M:%S> || <%d-%m-%Y> || ', time.localtime()) + desc)
  108. else:
  109. if len(self.trip) == 0:
  110. return
  111. desc = 'Arrived at %s. %i bottles delivered!' % (self.trip[0][0], self.trip[0][1])
  112. self.trip.pop(0)
  113. with open(logFile, 'a') as outf:
  114. outf.write('\n' + time.strftime('<%H:%M:%S> || <%d-%m-%Y> || ', time.localtime()) + desc)
  115.  
  116. botbot = bot(inputUrl)
  117.  
  118. #-------------------------------------------------------------------------------
  119.  
  120. class SearchL3(sm.SM):
  121. startState='H'
  122. #legalInputs=['N','S','W','E']
  123. legalInputs=[0,1,2,3]
  124. mapl3={'H':[None,None,None,'T1'],
  125. 'T1':['T4',None,'H','T2'],
  126. 'T2':['T3',None,'T1','X'],
  127. 'T3':['B','T2','T4','A'],
  128. 'T4':['C','T1','D','T3'],
  129. 'A':[None,None,'T3',None],
  130. 'B':[None,'T3',None,None],
  131. 'C':[None,'T4',None,None],
  132. 'D':[None,None,None,'T4'],
  133. 'X':[None,None,'T2',None]}
  134.  
  135. def __init__(self,goal):
  136. self.goal = goal
  137.  
  138. def nextState(self,state,action):
  139. return self.mapl3[state][action]
  140.  
  141. def getNextValues(self,state,action):
  142. nextState=self.nextState(state,action)
  143. return (nextState,nextState)
  144.  
  145. def done(self,state):
  146. return state==self.goal
  147.  
  148. #-------------------------------------------------------------------------------
  149.  
  150. y='H'
  151.  
  152. a=lambda x,y:s.smSearch(SearchL3(x),initialState=y,depthFirst=False,DP=False)
  153.  
  154. class State():
  155.  
  156. amigo=None
  157. n=a(botbot.trip[0][0],y)
  158.  
  159. def start(self):
  160. self.n.pop(0)
  161. return ('straight', 0, 0)
  162.  
  163. def end(self):
  164. if botbot.continueMovement == False:
  165. self.amigo = ('stop', None, 0)
  166. return self.amigo
  167. self.amigo = ('end', None, 0)
  168. y=self.n[0][1]
  169. botbot.arrival()
  170. self.n=a(botbot.trip[0][0],y)
  171. self.n.pop(0)
  172. return self.amigo
  173.  
  174. def junction(self):
  175. if self.n[0][0] == 3:
  176. if self.n[1][0] == 0:
  177. self.amigo = ('turnLeft', turnTimeStep, 0)
  178. self.n.pop(0)
  179. pass
  180. elif self.n[1][0] == 1:
  181. self.amigo = ('turnRight', turnTimeStep, 0)
  182. self.n.pop(0)
  183. pass
  184. elif self.n[1][0] == 3:
  185. self.amigo = ('junctionStraight', junctionTolerance, 0)
  186. self.n.pop(0)
  187. pass
  188. elif self.n[0][0] == 2:
  189. if self.n[1][0] == 0:
  190. self.amigo = ('turnRight', turnTimeStep, 0)
  191. self.n.pop(0)
  192. pass
  193. elif self.n[1][0] == 1:
  194. self.amigo = ('turnLeft', turnTimeStep, 0)
  195. self.n.pop(0)
  196. pass
  197. elif self.n[1][0] == 2:
  198. self.amigo = ('junctionStraight', junctionTolerance, 0)
  199. self.n.pop(0)
  200. pass
  201. elif self.n[0][0] == 0:
  202. if self.n[1][0] == 2:
  203. self.amigo = ('turnLeft', turnTimeStep, 0)
  204. self.n.pop(0)
  205. pass
  206. elif self.n[1][0] == 3:
  207. self.amigo = ('turnRight', turnTimeStep, 0)
  208. self.n.pop(0)
  209. pass
  210. elif self.n[1][0] == 0:
  211. self.amigo = ('junctionStraight', junctionTolerance, 0)
  212. self.n.pop(0)
  213. pass
  214. elif self.n[0][0] == 1:
  215. if self.n[1][0] == 2:
  216. self.amigo = ('turnRight', turnTimeStep, 0)
  217. self.n.pop(0)
  218. pass
  219. elif self.n[1][0] == 3:
  220. self.amigo = ('turnLeft', turnTimeStep, 0)
  221. self.n.pop(0)
  222. pass
  223. elif self.n[1][0] == 1:
  224. self.amigo = ('junctionStraight', junctionTolerance, 0)
  225. self.n.pop(0)
  226. pass
  227. return self.amigo
  228. def obstacleLeft(self): #optional
  229. return 'obstacleLeft'
  230. pass
  231. def obstacleRight(self):
  232. return 'obstacleRight'
  233.  
  234. class Sensor(sm.SM):
  235. def getNextValues(self, state, inp):
  236.  
  237. R = getDistanceRightAndAngle(inp.sonars)
  238. L = getDistanceLeftAndAngle(inp.sonars)
  239. frontRead=(inp.sonars[2]+inp.sonars[3])/2
  240. xrightRead=inp.sonars[5]
  241. xleftRead=inp.sonars[0]
  242. odo=inp.odometry.theta
  243. b = inp.sonars
  244.  
  245. k=[R,L,frontRead,xrightRead,xleftRead,odo,b]
  246. print 'Dist from robot center to wall on right', R[0]
  247. print inp.sonars
  248. if not R[1]:
  249. print '****** Angle reading not valid ******'
  250. return (state, k)
  251.  
  252. #workingvalues
  253. #left/right = 0.8, fvel = 0.25, rotate = 90, 0.3, turn = 40, fvel/dleft + 0.1, tt 40, jt = 20, tt = 5, rts = 30, k3 = 3, k4 = 1.7
  254.  
  255. desiredRight = 0.74
  256. desiredLeft = 0.74
  257. forwardVelocity = 0.375
  258.  
  259. rotateTimeStep = 85
  260. rotateRvel = 0.375
  261.  
  262. turnTimeStep = 24
  263. turnRvel = forwardVelocity/desiredLeft + 0.6
  264. turnFvel = forwardVelocity*0.5
  265. turnTolerance = 20
  266.  
  267. junctionTolerance = 15
  268. transitionTolerance = 2
  269.  
  270. reverseTimeStep = 30
  271.  
  272. obstacleTimeStep =25
  273. k3=2
  274. k4=1.72
  275. k3o=2
  276. k4o=1.22
  277. hello=State()
  278.  
  279. class WallFollower(sm.SM):
  280.  
  281. startState = hello.start()
  282.  
  283. def getNextValues(self, state, inp):
  284. print state
  285. print botbot.trip
  286. print hello.n
  287. print botbot.continueMovement
  288. print inp[6]
  289.  
  290. midpoint = 0.5*(inp[1][0] + inp[0][0])
  291.  
  292. if state[0] == 'stop':
  293. return (state,io.Action(0.0,0.0))
  294.  
  295. if state[0] == 'end':
  296. if inp[2] >= 0.75:
  297. state = ('end', None, 0)
  298. return(state,io.Action(forwardVelocity * 0.5))
  299. else:
  300. state= ('reverse', reverseTimeStep, 0)
  301. return(state,io.Action())
  302.  
  303. if state[0]=='obstacleLeft':
  304. if state[1]==0:
  305. state=('ostraight',0,0)
  306. return (state,io.Action())
  307. else:
  308.  
  309. print 'hello'
  310. if inp[0][1]==None:
  311. return (state,io.Action(0.1,-0.1))
  312. state=('obstacleLeft',state[1] - 1,0)
  313. n=1.26*(0 - inp[0][1])
  314. m=2*(0.3-inp[0][0])
  315. rvel = m+n
  316. return(state,io.Action(0.1,rvel))
  317.  
  318.  
  319.  
  320. if state[0] == 'reverse':
  321. if state[1] > 0:
  322. state = ('reverse', state[1] - 1, 0)
  323. return (state,io.Action(-forwardVelocity,0.0))
  324. else:
  325. state = ('rotate', rotateTimeStep, 0)
  326. return (state,io.Action(0.0,0.0))
  327.  
  328. if state[0] == 'rotate':
  329. if state[1] > 0:
  330. state = ('rotate', state[1] - 1, 0)
  331. return (state, io.Action(0.0,rotateRvel))
  332. elif botbot.continueMovement == False:
  333. state = ('stop', None, 0)
  334. return (state, io.Action(0.0,0.0))
  335. else:
  336. state = ('straight', 0, 0)
  337. return (state,io.Action(0.0,0.0))
  338.  
  339. if state[0] == 'turnLeft':
  340. if state[1] > 0:
  341. state = ('turnLeft', state[1] - 1, 0)
  342. return (state,io.Action(turnFvel,turnRvel))
  343. else:
  344. state = ('straight', turnTolerance, 0)
  345. return (state,io.Action(forwardVelocity*0.5,0.0))
  346.  
  347. if state[0] == 'turnRight':
  348. if state[1] > 0:
  349. state = ('turnRight', state[1] - 1, 0)
  350. return (state,io.Action(turnFvel,-turnRvel))
  351. else:
  352. state = ('straight', turnTolerance, 0)
  353. return(state,io.Action(forwardVelocity*0.5,0.0))
  354.  
  355. if state[0] =='junctionStraight':
  356. if state[1] > 0:
  357. state = ('junctionStraight', state[1] - 1, 0)
  358. elif inp[6][0] <= 2 and inp[6][5] <= 2:
  359. state = ('straight', turnTolerance, 0)
  360. return (state,io.Action(forwardVelocity,0.0))
  361. else:
  362. return(state,io.Action(forwardVelocity,0.0))
  363.  
  364. if state[1] == 0 and state[0] != 'junctionStraight':
  365. if inp[6][0] > 4 or inp[6][5] > 4:
  366. state = (state[0], state[1], state[2] + 1)
  367. else:
  368. state = (state[0], state[1], 0)
  369. if state[2] > transitionTolerance:
  370. state = hello.junction()
  371.  
  372. if hello.n[0][1] in ('A', 'B', 'C', 'D', 'X') and inp[6][2] <= 1 and inp[6][3] <= 1:
  373. state = hello.end()
  374.  
  375. if inp[6][2]<=0.75 and inp[6][4]>=0.85 and inp[6][4]!=5:
  376. state=('obstacleLeft', obstacleTimeStep,0)
  377. return (state, io.Action())
  378.  
  379. if state[0] == 'straight':
  380. if inp[0][1] == None and inp[1][1] == None:
  381. return(state,io.Action(forwardVelocity,0))
  382. elif inp[0][1] == None:
  383. state = ('Lstraight', state[1], state[2])
  384. else:
  385. state = ('Rstraight', state[1], state[2])
  386. return(state,io.Action(forwardVelocity,0))
  387.  
  388. if state[0] == 'Lstraight':
  389. if state[1] > 0:
  390. state = ('Lstraight', state[1] - 1, state[2])
  391. if inp[0][1] == None and inp[1][1] == None:
  392. rvel=0
  393. elif inp[1][1] == None:
  394. n=k4*(0-inp[0][1])
  395. m=k3*(midpoint-inp[0][0])
  396. rvel= m+n
  397. state = ('Rstraight', state[1], state[2])
  398. else:
  399. n=k4*(0 - inp[1][1])
  400. m=k3*(midpoint-inp[1][0])
  401. rvel = -m+n
  402. state = ('Lstraight', state[1], state[2])
  403. return(state,io.Action(forwardVelocity,rvel))
  404.  
  405. if state[0] == 'Rstraight':
  406. if state[1] > 0:
  407. state = ('Rstraight', state[1] - 1, state[2])
  408. if inp[0][1] == None and inp[1][1] == None:
  409. rvel=0
  410. elif inp[0][1] == None:
  411. n=k4*(0-inp[1][1])
  412. m=k3*(midpoint-inp[1][0])
  413. rvel= -m+n
  414. state = ('Lstraight', state[1], state[2])
  415. else:
  416. n=k4*(0 - inp[0][1])
  417. m=k3*(midpoint-inp[0][0])
  418. rvel = m+n
  419. state = ('Rstraight', state[1], state[2])
  420. return(state,io.Action(forwardVelocity,rvel))
  421.  
  422. ###obstaclestraight
  423. if state[0] == 'ostraight':
  424. if inp[0][1] == None and inp[1][1] == None:
  425. return(state,io.Action(forwardVelocity/2,0))
  426. elif inp[0][1] == None:
  427. state = ('oLstraight', state[1], state[2])
  428. else:
  429. state = ('oRstraight', state[1], state[2])
  430. return(state,io.Action(forwardVelocity/2,0))
  431.  
  432. if state[0] == 'oLstraight':
  433. if state[1] > 0:
  434. state = ('oLstraight', state[1] - 1, state[2])
  435. if inp[0][1] == None and inp[1][1] == None:
  436. rvel=0
  437. elif inp[1][1] == None:
  438. n=k4o*(0-inp[0][1])
  439. m=k3o*(midpoint-inp[0][0])
  440. rvel= m+n
  441. state = ('oRstraight', state[1], state[2])
  442. else:
  443. n=k4o*(0 - inp[1][1])
  444. m=k3o*(midpoint-inp[1][0])
  445. rvel = -m+n
  446. state = ('oLstraight', state[1], state[2])
  447. return(state,io.Action(forwardVelocity/2,rvel))
  448.  
  449. if state[0] == 'oRstraight':
  450. if state[1] > 0:
  451. state = ('oRstraight', state[1] - 1, state[2])
  452. if inp[0][1] == None and inp[1][1] == None:
  453. rvel=0
  454. elif inp[0][1] == None:
  455. n=k4o*(0-inp[1][1])
  456. m=k3o*(midpoint-inp[1][0])
  457. rvel= -m+n
  458. state = ('oLstraight', state[1], state[2])
  459. else:
  460. n=k4o*(0 - inp[0][1])
  461. m=k3o*(midpoint-inp[0][0])
  462. rvel = m+n
  463. state = ('oRstraight', state[1], state[2])
  464. return(state,io.Action(forwardVelocity/2,rvel))
  465.  
  466. return(state,io.Action(forwardVelocity/2,0.0))
  467.  
  468. print botbot.trip
  469.  
  470. sensorMachine = Sensor()
  471. sensorMachine.name = 'sensor'
  472. mySM = sm.Cascade(sensorMachine, WallFollower())
  473.  
  474. ######################################################################
  475. #
  476. # Running the robot
  477. #
  478. ######################################################################
  479.  
  480. def setup():
  481. robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
  482. robot.behavior = mySM
  483. robot.behavior.start(traceTasks = robot.gfx.tasks())
  484.  
  485. def step():
  486. robot.behavior.step(io.SensorInput()).execute()
  487.  
  488. def brainStop():
  489. pass
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!