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 numpy as np import pickle from sabretooth_command import CartCommand from image_analyzer_pseye import ImageAnalyzer from time import time import Queue from keras.models import load_model import sys old_data = [] data = [] analyzer = ImageAnalyzer() cart = CartCommand(port="/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)
"v4l2src device=/dev/video0 ! ffmpegcolorspace ! video/x-raw-bgr ! appsink"
)
_, frame = cap.read()
print(frame)
cv2.imshow('image', frame)
analyzer = ImageAnalyzer()
analyzer.cap.release()
analyzer.cap = cv2.VideoCapture(
"v4l2src device=/dev/video0 ! ffmpegcolorspace ! video/x-raw-rgb ! appsink"
)
_, frame = analyzer.cap.read()
cv2.imshow('image', frame)
#"autovideosrc ! appsink")
cart = CartCommand()
xs = []
cart.toggleEnable()
N = 60
def grabber():
for i in range(2 * N):
analyzer.cap.grab()
t1 = Thread(target=grabber, args=())
t2 = Thread(target=grabber, args=())
t3 = Thread(target=grabber, args=())
class CartPoleEnv(gym.Env):
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()
def _step(self, action):
if action == self.action_space[0]:
d_command = 1.
else:
d_command = -1.
command += commandStep * d_command
command = min(max(command, -2046), 2046)
if x < 0.35:
command = min(command, -500)
if x > 0.65:
command = max(command, 500)
self.cart.setSpeed(command)
self.last_state = self.state
self.state = self._getState()
reward = self._getReward(self.state)
done = False
return np.array(self.state), reward, done, {}
def _reset(self):
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
self.cart.enabled = True
while not 0.4 < x < 0.6:
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
command = 1000 * np.sign(x - 0.5)
command = min(max(command, -2046), 2046)
self.cart.setSpeed(command)
cv2.waitKey(1)
self.cart.setSpeed(0)
sleep(0.3)
self.cart.enabled = False
def _getData(self):
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
xpole = np.cos(theta)
ypole = np.sin(theta)
return x, xpole, ypole
def _getState(self):
x, xpole, ypole = self._getData()
if not self.last_state is None:
state = [
x, x - self.last_state[0], xpole, xpole - self.last_state[2],
ypole, ypole - self.last_state[4]
]
else:
state = [x, 0, xpole, 0, ypole, 0]
return state
def _getReward(self, state):
rewards_pole = 0.0 * (state[:, 4] + 0.5)**2 #ypole hieght
rewards_cart = -2.0 * np.power(state[:, 0], 2) #xcart pos
return rewards_cart + rewards_pole
def _render(self, mode='human', close=False):
pass
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
elif key & 0xFF == ord('q'):
analyzer.save()
break
elif key & 0xFF == ord('r'):
print("reset")
reset()
return data
model = makeModel()
analyzer = ImageAnalyzer(1)
cart = CartCommand("/dev/ttyACM0")
commandqueue = Queue.Queue()
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)
cart.setSpeed(command)
cv2.waitKey(1)
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
analyzer = ImageAnalyzer()
cart = CartCommand()
itheta = 0
start = 0
def nothing(x):
pass
cv2.namedWindow('PID', cv2.WINDOW_NORMAL)
cv2.resizeWindow('PID', 600,200)
cv2.createTrackbar('P','PID',0,200000,nothing)
cv2.setTrackbarPos('P', 'PID', 100000)
cv2.createTrackbar('I','PID',0,16000,nothing)
cv2.setTrackbarPos('I', 'PID', 8000)
cv2.createTrackbar('D','PID',0,200000,nothing)