def main():
arduino = Arduino()
pid_client = PidClient()
if(LOG):
timestr = time.strftime("%Y%m%d-%H%M%S")
filename = "pid_log-" + timestr + ".txt"
file = open(filename,"w+")
loglines = 0
while(True):
print("waiting for state...")
state = arduino.waitForState() # blocking
newControl = pid_client.determineControl(state)
if(LOG):
logLine = str(state) + "," + str(newControl) + "\n"
logLine = logLine.strip("[]")
file.write(logLine)
loglines += 1
if(loglines % 1000 == 0):
print("loglines: {}".format(loglines))
file.flush()
arduino.accelerateMotorPower(newControl)
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 __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)
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 test_open_all_channels(self):
"""docstring for test_open_all_channels"""
ard = Arduino()
ard.channels = {
'ems1': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 1000, 'last_value': 0, 'ems_on_off': False},
'ems2': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 2000, 'last_value': 0, 'ems_on_off': False}
}
ard.open_all_channels()
calls = [call("ems1", True), call("ems2", True)]
ard.change_relay_state.assert_has_calls(calls, True)
class Agent:
def __init__(self):
self.arduino = Arduino()
def run(self):
while True:
print(
"Type H for help. Prefix a command with ! if not expecting a response."
)
userString = input()
if userString[0] == '!':
userString = userString[1:]
self.arduino.writeMessage(userString)
else:
print(self.arduino.writeMessageAndWait(userString))
def test_run(self):
ar = Arduino()
ar.stop = False
ar.no_serial = False
cb = MagicMock()
ar.ser = MagicMock()
def fake_readline(val):
return b"l"
ar.ser.readline = fake_readline
ar.subscribers = [cb]
ar.start()
ar.stop = True
ar.join()
cb.assert_called_with("l")
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)
def test_change_relay_state(self):
ard = Arduino()
ard.channels = {
'relay1': {'type': 'relay', 'state': False, 'serial_open': 'o', 'serial_close': 'c'},
'relay2': {'type': 'relay', 'state': False, 'serial_open': 'k', 'serial_close': 'l'}
}
ard.change_relay_state("relay1", True)
self.assertEqual(ard.channels['relay1']['state'], True)
ard.send_value.assert_called_with("o")
ard.change_relay_state("relay2", True)
self.assertEqual(ard.channels['relay2']['state'], True)
ard.send_value.assert_called_with("k")
ard.change_relay_state("relay1", False)
self.assertEqual(ard.channels['relay1']['state'], False)
ard.send_value.assert_called_with("c")
ard.change_relay_state("relay2", 0)
self.assertEqual(ard.channels['relay2']['state'], False)
ard.send_value.assert_called_with("l")
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)
def test_calibration(self):
"""docstring for test_calibration"""
ard = Arduino()
ard.channels = {
'ems1': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 1000, 'last_value': 0, 'ems_on_off': False},
'ems2': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 2000, 'last_value': 0, 'ems_on_off': False}
}
def test_calibration_reset():
message = ["calibrate", "reset"]
ard.calibration(message)
ard.send_value.assert_called_with("r")
def test_calibration_set_min_max():
self.assertEqual(ard.channels['ems1']['min_max'],[0,80])
self.assertEqual(ard.channels['ems2']['min_max'],[0,80])
message = ["calibrate", "ems_min_max", "ems1", "10", "80"]
ard.calibration(message)
self.assertEqual(ard.channels['ems1']['min_max'],[10,80])
self.assertEqual(ard.channels['ems2']['min_max'],[0,80])
message = ["calibrate", "ems_min_max", "ems2", "6", "19"]
ard.calibration(message)
self.assertEqual(ard.channels['ems1']['min_max'],[10,80])
self.assertEqual(ard.channels['ems2']['min_max'],[6,19])
with self.assertRaises(ValueError):
message = ["calibrate", "ems_min_max", "ems2", "a", "19"]
ard.calibration(message)
with self.assertRaises(IndexError):
message = ["calibrate", "ems_min_max"]
ard.calibration(message)
def test_calibration_ems_on_off():
message = ["calibrate", "ems_on_off", "ems1", "true"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems1", True)
message = ["calibrate", "ems_on_off", "ems2", "true"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems2", True)
message = ["calibrate", "ems_on_off", "ems1", "false"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems1", False)
message = ["calibrate", "ems_on_off", "ems1", "0"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems1", False)
with self.assertRaises(ValueError):
message = ["calibrate", "ems_on_off", "ems1", "a"]
ard.calibration(message)
with self.assertRaises(IndexError):
message = ["calibrate", "ems_on_off"]
ard.calibration(message)
def test_calibration_ems_value():
message = ["calibrate", "ems_value", "ems1", "25"]
ard.calibration(message)
ard.send_ems_strength.assert_called_with({"ems1": 25})
message = ["calibrate", "ems_value", "ems2", "25"]
ard.calibration(message)
ard.send_ems_strength.assert_called_with({"ems2": 25})
with self.assertRaises(ValueError):
message = ["calibrate", "ems_value", "ems1", "a"]
ard.calibration(message)
with self.assertRaises(ValueError):
message = ["calibrate", "ems_value", "ems1", "49.1"]
ard.calibration(message)
with self.assertRaises(ValueError):
message = ["calibrate", "ems_value", "ems1", "-1"]
ard.calibration(message)
with self.assertRaises(IndexError):
message = ["calibrate", "ems_value", "ems1"]
ard.calibration(message)
def test_calibration_relay():
message = ["calibrate", "relay", "ems1", "true"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems1", True)
message = ["calibrate", "relay", "ems2", "true"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems2", True)
message = ["calibrate", "relay", "ems1", "false"]
ard.calibration(message)
ard.change_relay_state.assert_called_with("ems1", False)
with self.assertRaises(ValueError):
message = ["calibrate", "relay", "ems1", "a"]
ard.calibration(message)
with self.assertRaises(IndexError):
message = ["calibrate", "relay", "ems1"]
ard.calibration(message)
test_calibration_reset()
test_calibration_set_min_max()
test_calibration_ems_on_off()
test_calibration_ems_value()
test_calibration_relay()
def test_stop_all(self):
ard = Arduino()
ard.stop_all()
ard.send_value.assert_called_with("s")
def test_subscribe(self):
ar = Arduino()
ar.subscribers = []
cb = MagicMock()
ar.subscribe(cb)
self.assertEqual(ar.subscribers[0], cb)
def test_send_value(self):
ar = Arduino()
ar.ser = MagicMock()
ar.no_serial = True
ar.previously_sent = None
ar.send_value("1")
self.assertFalse(ar.ser.write.called)
ar.no_serial = False
ar.previously_sent = None
ar.send_value("1")
ar.ser.write.assert_called_with(bytes("1", "UTF-8"))
self.assertEqual(ar.previously_sent, "1")
ar.ser.write.reset_mock()
ar.previously_sent = "1"
ar.send_value("1")
self.assertFalse(ar.ser.write.called)
# Main.py
# Main Loop and intitializations for running project
DATABASE_ENDPOINT = "http://projects.cse.tamu.edu/shotaehrlich/passingPythonInfo.php" # Enter url to database endpoint here !!!!
# - - - Imports - - -
from lib.arduino import Arduino
from lib.database import Database
from lib.charecterization import Charecterization
# - - - Initialize Objects - - -
arduino = Arduino({'port':
'/dev/ttyACM0'}) # Change to proper port from Arduino IDE
database = Database(DATABASE_ENDPOINT)
charecterization = Charecterization(7, False)
# - - - Main Loop - - -
while True:
# Poll the arduino for new data
#print("hello")
r = arduino.poll()
#print("1hello")
# Plot and analyse the data for enterance and exit events
charecterization.record_raw(r)
#print(charecterization.enter)
def __init__(self):
self.arduino = Arduino()
def test_send_ems_strength(self):
"""docstring for test_send_percentage"""
ard = Arduino()
ard.channels = {
'ems1': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 1000, 'last_value': 0, 'ems_on_off': False},
'ems2': {'min_max': [0, 80], 'type': 'digipot', 'prefix': 2000, 'last_value': 0, 'ems_on_off': False}
}
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 0, "ems2": 0})
ard.send_value.assert_called_with("$1100%$2100%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 100, "ems2": 100})
ard.send_value.assert_called_with("$1020%$2020%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 100})
ard.send_value.assert_called_with("$1020%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 100, "ems2": 0})
ard.send_value.assert_called_with("$1020%$2100%")
ard.last_sent_ems = 0
ard.channels['ems1']['min_max'] = [12,100]
ard.send_ems_strength({"ems1": 0, "ems2": 0})
ard.send_value.assert_called_with("$1088%$2100%")
ard.last_sent_ems = 0
ard.channels['ems1']['min_max'] = [20,67]
ard.send_ems_strength({"ems1": 0, "ems2": 0})
ard.send_value.assert_called_with("$1080%$2100%")
ard.last_sent_ems = 0
ard.channels['ems1']['min_max'] = [20,67]
ard.send_ems_strength({"ems1": 100, "ems2": 0})
ard.send_value.assert_called_with("$1033%$2100%")
ard.last_sent_ems = 0
ard.channels['ems1']['min_max'] = [20,80]
ard.send_ems_strength({"ems1": 25, "ems2": 0})
ard.send_value.assert_called_with("$1075%$2100%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 25.3, "ems2": 0})
ard.send_value.assert_called_with("$1075%$2100%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": -1, "ems2": 0})
ard.send_value.assert_called_with("$1080%$2100%")
ard.last_sent_ems = 0
ard.send_ems_strength({"ems1": 101, "ems2": 0})
ard.send_value.assert_called_with("$1020%$2100%")
ard.last_sent_ems = 0
with self.assertRaises(ValueError):
ard.send_ems_strength({"ems1": "wrong", "ems2": 0})
ard.last_sent_ems = 0
with self.assertRaises(ValueError):
ard.send_ems_strength({"ems1": 0, "ems2": "wrong"})
ard.last_sent_ems = 0
with self.assertRaises(IndexError):
ard.send_ems_strength({"ThisWillNeverBeAnamefoooorANems": 100})
import time
import pygame
import pygame.camera
from pygame.locals import *
from lib.arduino import Arduino
from auth import client, apply_access_token, get_new_mentions
from config import ARDUINO_ADDRESS, RELAY_PIN
if __name__ == '__main__':
pygame.init()
pygame.camera.init()
cam = pygame.camera.Camera("/dev/video0",(640,480))
arduino = Arduino(ARDUINO_ADDRESS)
arduino.output([RELAY_PIN])
apply_access_token()
while 1:
mentions = get_new_mentions()
if mentions:
for mention in mentions:
command = mention['text'].split()[1]
if command == u'开灯':
client.statuses.update.post(status=u'开灯啦！')
arduino.setHigh(RELAY_PIN)
print '开灯'
elif command == u'关灯':
client.statuses.update.post(status=u'关灯啦！')
arduino.setLow(RELAY_PIN)
print '关灯'
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