def __init__(self, cartport="/dev/ttyACM0", imageport=1):
self.analyzer = ImageAnalyzer(imageport)
self.cart = CartCommand(port=cartport)
self.action_space = spaces.Discrete(2)
self.observation_space = spaces.Box(
np.array([0., -50., 0., -50., -1., -50.]),
np.array([1., 50., 1., 50., 1., 50.]))
self.last_state = None
self.state = self._getState()
self.last_state = self._getState()
var = np.array([
0.5, 1. / 50., 2., 1. / 5., 2.0, 1. / 5., 2048., 2048., 2048., 2048., 2048.
])
def learn(n):
cem.fit(env, nb_steps=n)
#####################
# testing functions #
#####################
analyzer = ImageAnalyzer(1)
cart = CartCommand(port="/dev/ttyACM0")
memory = Memory()
def test(n, random_action=False, eps=1.0):
global states, actions, next_states, command_queue
command_queue = Queue.Queue()
cart.toggleEnable()
current_states = []
current_actions = []
current_next_states = []
command = 0
import pygame
import sys
import time
import socket
import cPickle as pickle
from sabretooth_command import CartCommand
pygame.init()
cart = CartCommand()
cart.toggleEnable()
pygame.joystick.init()
clock = pygame.time.Clock()
print pygame.joystick.get_count()
_joystick = pygame.joystick.Joystick(0)
_joystick.init()
while 1:
pygame.event.get()
xdir = _joystick.get_axis(0)
#rtrigger = _joystick.get_axis(5)
#ltrigger = _joystick.get_axis(4)
#print(xdir * 200)
if abs(xdir) < 0.2:
xdir = 0.0
print(xdir * 100)
cart.setSpeed(xdir * 2046)
import serial.tools.list_ports
import scipy.linalg as linalg
lqr = linalg.solve_continuous_are
ports = list(serial.tools.list_ports.comports())
print(dir(ports))
for p in ports:
print(dir(p))
print(p.device)
if "Sabertooth" in p.description:
sabreport = p.device
else:
ardPort = p.device
print("Initilizing Commander")
comm = CartCommand(port=sabreport) #"/dev/ttyACM1")
print("Initilizing Analyzer")
analyzer = EncoderAnalyzer(port=ardPort) #"/dev/ttyACM0")
print("Initializing Controller.")
cart = CartController(comm, analyzer)
time.sleep(0.5)
print("Starting Zero Routine")
cart.zeroAnalyzer()
gravity = 9.8
mass_pole = 0.15
length = 0.5
moment_of_inertia = (1. / 3.) * mass_pole * length**2
def E(x): # energy
import cv2
import numpy as np
import pickle
from sabretooth_command import CartCommand
from image_analyzer_pseye import ImageAnalyzer
from time import time
import Queue
old_data = []
data = []
analyzer = ImageAnalyzer(1)
cart = CartCommand("/dev/ttyACM0")
commandqueue = Queue.Queue()
for i in range(5):
commandqueue.put(0)
def reset():
x = 0
cart.enabled = True
while not 0.4 < x < 0.6:
x, dx, theta, dtheta = analyzer.analyzeFrame()
command = 1000 * np.sign(x - 0.5)
command = min(max(command, -2046), 2046)
print(command)