class PidClient:
MOMENTUM_CONSTANT = 0.10
def __init__(self):
stream = open("pid_values.yml", "r")
pid_values = yaml.load(stream)
self.thetaPid = Pid(pid_values['theta_p'], pid_values['theta_i'], pid_values['theta_d'], True)
self.xPosPid = Pid(pid_values['xpos_p'], pid_values['xpos_i'], pid_values['xpos_d'], False)
self.arduino = Arduino()
def parseState(self, state):
self.state = state
self.theta = state[0]
self.thetaDot = state[1]
self.xPos = state[2]
self.phiDot = state[3]
def updateRadPerSec(self, newRadPerSec):
self.arduino.updateMotorPower(newRadPerSec)
def determineControl(self, state):
self.parseState(state)
# emergency breaks
if(abs(self.theta) > 0.30):
return 0
self.thetaTerm = self.thetaPid.getControl(self.theta)
self.xPosTerm = self.xPosPid.getControl(self.xPos)
# // If the xPosTerm did its job and got us leaning back towards X=0,
# // then stop trying to accelerate away from X=0.
momentum = self.MOMENTUM_CONSTANT * abs(self.phiDot)
if abs(self.xPos) < 0.1:
self.xPosTerm = 0
if self.xPos > 0 and self.theta < -momentum:
self.xPosTerm = 0
if self.xPos < 0 and self.theta > momentum:
self.xPosTerm = 0
# note that thetaDotTerm and phiDotTerm are zeroed out.
newRadPerSec = self.thetaTerm + self.xPosTerm
newRadPerSec = -newRadPerSec
newRadPerSec = self._constrain(newRadPerSec, -10, 10)
return newRadPerSec
def _constrain(self, val, min_val, max_val):
return min(max_val, max(min_val, val))
def main():
if (len(sys.argv) != 2):
print("give me a model file as argument please!")
exit()
modelfile = sys.argv[1]
arduino = Arduino()
model = load_model(modelfile)
print("waiting for first state...")
obs = arduino.waitForState()
print("neat found first state!")
while (True):
obs = arduino.waitForState()
foo = np.array(obs)
bar = foo.reshape((1, -1))
action = model.predict(bar, batch_size=1)
action = action[0][0]
print(action)
arduino.updateMotorPower(action)
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