class Agent:
def __init__(self):
self.arduino = Arduino()
def run(self):
self.ensureStopped()
loops = 0
issueTime = current_milli_time()
self.arduino.writeMessage("W10")
while True:
observation = self.arduino.getState()
wheelSpeed = float(observation[3])
if (wheelSpeed > 0.0):
responseTime = current_milli_time() - issueTime
print("response time: {}".format(responseTime))
break
else:
loops += 1
self.ensureStopped()
print("End of experiment! Loops: {}".format(loops))
def ensureStopped(self):
print("Ensuring stopped...")
self.arduino.writeMessage("W0")
while True:
observation = self.arduino.getState()
wheelSpeed = float(observation[3])
if (wheelSpeed == 0.0):
print("Stopped! :)")
return
else:
time.sleep(0.5)
class World(BaseWorld):
MAX_EPISODE_DURATION = 10 # seconds
MAX_ANGLE = 0.25 # in radians. ~> 14.3 degrees
MAX_DISTANCE = 30 # in radians
def __init__(self):
self.state = State()
self.arduino = Arduino()
self.observation_space = SomeSpace(4)
self.action_space = SomeSpace(1, 1.0, -1.0)
self.episodeStartTime = datetime.datetime.now()
self.arduino.resetRobot()
# Pin Setup:
self.buttonPin = 4
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.buttonPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# returns four states
def getObservation(self):
self.state = self.arduino.getState()
self.observation = self.state[:4]
self.outerDt = self.state[4]
self.targetRPS = self.state[5]
return self.observation
# returns observation, reward, done
def step(self, action):
self.accelerateMotors(action)
observation = self.getObservation()
if observation == False:
return False, False, False
return observation, self.__reward(), self.isDone()
def reset(self):
print("initiating reset...")
self.arduino.stopMotors()
self.arduino.resetRobot() # primarily setting xPos = 0
print('\a')
print("waiting on reset button...")
while 1:
if GPIO.input(self.buttonPin): # button is released
a = 1
# do Nothing
else: # button is pressed:
print("button pressed! Here we go!")
break
self.episodeStartTime = datetime.datetime.now()
return self.getObservation()
def updateMotors(self, newPower):
self.arduino.updateMotorPower(newPower)
def accelerateMotors(self, acc):
self.arduino.accelerateMotorPower(acc)
# determines if episode is done
# conditions:
# 0) Robot has fallen over
# 1) Robot is too far from center-balanced
# 2) Episode is over MAX_EPISODE_DURATION
def isDone(self):
return self.__isFallen() or self.__isFarAway() or self.__isRetired()
def __reward(self):
return 1
def __stopMotors(self):
self.arduino.stopMotors()
# Is the robot angle or x position too ugly?
def __isFallen(self):
if (math.fabs(self.observation[0]) > self.MAX_ANGLE):
print("!!!!!!!!!!!!!!!isFallen. Angle: {} > {}".format(
self.observation[0], self.MAX_ANGLE))
return True
else:
return False
def __isFarAway(self):
if (math.fabs(self.observation[2]) > self.MAX_DISTANCE):
print("!!!!!!!!!!!!!!!isFarAway. Distance: {} > {}".format(
self.observation[2], self.MAX_DISTANCE))
return True
else:
return False
# Is the episode over MAX_EPISODE_DURATION
def __isRetired(self):
delta = datetime.datetime.now() - self.episodeStartTime
if (delta.seconds > self.MAX_EPISODE_DURATION):
print("!!!!!!!!!!!!!!!isRetired {} > {}".format(
delta.seconds, self.MAX_EPISODE_DURATION))
return True
else:
return False