Permalink
Browse files

got it working!

  • Loading branch information...
ishmandoo committed Apr 9, 2018
1 parent 68a8f1a commit ca4aabb86eca54c5a9c7884ad25fafdec4d545ce
View
BIN +24.9 KB 1518387710.model
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
View
@@ -1,12 +1,12 @@
(dp0
S'right'
p1
(I520
I234
(I526
I205
tp2
sS'left'
p3
(I130
I246
(I142
I210
tp4
s.
View
@@ -0,0 +1,57 @@
from sabretooth_command import CartCommand
from encoder_analyzer import EncoderAnalyzer
import threading
class CartController:
def __init__(self, cart, analyzer):
self.cart = cart
self.analyzer = analyzer
self.speed = 0
self.cart.enabled = True
self.mm_per_s_per_dc = 0.5 #determined experimentally
#self.limit_switch_thread = threading.Thread(target=self._limit_switch_check)
#self.limit_switch_thread.daemon = True
#self.limit_switch_thread.start()
def _limit_switch_check(self):
while True:
if (self.speed > 0) and self.analyzer.end_stop_high:
self.cart.setSpeed(0)
print("high limit switch")
if (self.speed < 0) and self.analyzer.end_stop_low:
self.cart.setSpeed(0)
print("low limit switch")
def setSpeed(self, speed):
self.speed = max(-2046, min(speed, 2047)) # clamp to correct range
self.cart.setSpeed(self.speed)
def setSpeedMmPerS(self, speed):
speed *= self.mm_per_s_per_dc
if speed < 0:
speed -= 100
else:
speed += 100
self.speed = max(-2046, min(speed, 2047)) # clamp to correct range
self.cart.setSpeed(self.speed)
def zeroAnalyzer(self):
while not self.analyzer.checkEndStopHigh():
self.cart.setSpeed(700)
self.analyzer.setMax()
while not self.analyzer.checkEndStopLow():
self.cart.setSpeed(-700)
self.cart.setSpeed(0)
self.analyzer.setZero()
def goTo(self, target_x):
print(self.analyzer.getCartPosition(), self.analyzer.getPolePosition())
while (self.analyzer.getCartPosition() - target_x) > 25:
self.cart.setSpeed(-1000)
while (self.analyzer.getCartPosition() - target_x) < -25:
self.cart.setSpeed(1000)
self.cart.setSpeed(0)
View
BIN +2.54 KB cart_controller.pyc
Binary file not shown.
View
BIN +35.7 KB cart_pole_messages.png
Binary file not shown.
View
@@ -8,22 +8,22 @@
from image_analyzer_pseye import ImageAnalyzer
import cv2
from time import sleep
logger = logging.getLogger(__name__)
class CartPoleEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
def __init__(self, cartport, imageport):
self.port = port
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()
@@ -54,7 +54,7 @@ def _step(self, action):
def _reset(self):
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
cart.enabled = True
self.cart.enabled = True
while not 0.4 < x < 0.6:
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
@@ -74,10 +74,10 @@ def _getData(self):
ypole = np.sin(theta)
return x, xpole, ypole
def _getState():
x, xpole, ypole = self._getData
if not old_state is None:
state = [x, x-old_state[0], xpole, xpole - old_state[2], ypole, ypole - old_state[4]]
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
View
BIN +3.63 KB cartpole_env.pyc
Binary file not shown.
View
Binary file not shown.
Oops, something went wrong.

0 comments on commit ca4aabb

Please sign in to comment.