juju
/
nppilot-v2
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Updates after the first runs.

Browse Source
master
Michael Hope 4 years ago
parent 2d89714772
commit aee51e3f8e
6 changed files with 59 additions and 32 deletions
Show Stats Download Patch File Download Diff File

10
src/nppilot/config/v_pid.yaml
Unescape View File

@ -1,11 +1,11 @@
Kp: 0
Kp: 0.2
Ki: 0
Kd: 0
upper_limit: 0.5
upper_limit: 0.7
lower_limit: 0
windup_limit: 0.5
setpoint_topic: /v/sp
topic_from_plant: /v/pv
topic_from_controller: /v/u
setpoint_topic: /mode
topic_from_plant: /vx
topic_from_controller: /cmd_f

2
src/nppilot/launch/manual.launch
Unescape View File

@ -1,6 +1,6 @@
<launch>
<node name="rosflight_io" pkg="rosflight" type="rosflight_io">
<param name="port" value="/dev/ttyUSB0"/>
<param name="port" value="/dev/ttyACM0"/>
</node>
<node pkg="ublox_gps" type="ublox_gps" name="ublox_gps">

8
src/nppilot/launch/rover1.launch
Unescape View File

@ -1,9 +1,9 @@
<launch>
<node name="yaw_rate_pid" pkg="pid" type="controller">
<rosparam command="load" file="$(find nppilot)/config/yaw_rate_pid.yaml"/>
<node pkg="ublox_gps" type="ublox_gps" name="ublox_gps">
<rosparam command="load" file="$(find nppilot)/config/rover_gps.yaml" />
</node>
<node name="v_pid" pkg="pid" type="controller">
<rosparam command="load" file="$(find nppilot)/config/v_pid.yaml"/>
<node name="rosflight_io" pkg="rosflight" type="rosflight_io">
<param name="port" value="/dev/ttyUSB0"/>
</node>
<node pkg="nppilot" name="revo" type="revo.py" output="screen">
</node>

44
src/nppilot/lib/grid.py
Unescape View File

@ -43,7 +43,7 @@ class Grid:
for d in DIRS.values():
v = self.get((y + d[0], x + d[1]))
if v >= INF:
v = 10
v = 3
acc += v
out[y][x] = acc / len(DIRS)
self._cells = out
@ -142,7 +142,7 @@ def plan(gr: Grid, keepout: Grid, start, goal):
continue
if keepout.get(n) >= INF:
continue
gn = g[c] + di[-1]
gn = g[c] + di[-1] + kf(n, keepout)
if gr.get(n) < gn:
gr.set(n, gn)
if n not in open:
@ -151,24 +151,46 @@ def plan(gr: Grid, keepout: Grid, start, goal):
continue
came_from[n] = c
g[n] = gn
open.sort(key=lambda x: gf(g, x) + h(x, goal) + kf(x, keepout))
open.sort(key=lambda x: gf(g, x) + h(x, goal)) # + kf(x, keepout))
def main():
w, h = 30, 30
g = Grid(h, w, 1)
keepout = Grid(h, w, 1)
for i in range(20):
def random_walls(g):
for i in range(2):
d = (1, 0)
if random.randint(0, 1) == 1:
d = (0, 1)
keepout.line((random.randint(0, h), random.randint(0, w)), d, random.randint(5, h // 2),
INF)
def parking(g):
h = 20
w = 10
y = 6
g.line((y, 10), (1, 0), h, INF)
g.line((y + h, 10), (0, 1), w, INF)
g.line((y, 10), (0, 1), w, INF)
g.line((y, 10 + w - 1), (1, 0), h, INF)
g.line((y, 10 + w - 1 + 3), (1, 0), h, INF)
g.line((y, 10 + w - 1 + 3), (0, 1), 9, INF)
g.line((y, 10 - 2), (1, 0), h, INF)
# g.line((5, 0), (0, 1), 9, INF)
def main():
w, h = 30, 30
g = Grid(h, w, 1)
keepout = Grid(h, w, 1)
parking(keepout)
keepout.smear()
start = (random.randint(0, h), random.randint(0, w), 0)
goal = (random.randint(0, h), random.randint(0, w))
#start = (random.randint(0, h), random.randint(0, w), 0)
#goal = (random.randint(0, h), random.randint(0, w))
start = (2, 16, 3)
goal = (28, 16)
path = plan(g, keepout, start, goal)
path = np.array(path)

16
src/nppilot/scripts/controller.py
Unescape View File

@ -37,13 +37,13 @@ class PID:
self._upper_limit = upper_limit if upper_limit is not None else 1e12
self.enabled = True
self._sp = 0.
self.sp = 0.
self._ti = 0.
self._errs = [0.] * 3
self._t: Optional[float] = None
self._t = None
def step(self, t: float, pv: float) -> float:
err = self._sp - pv
err = self.sp - pv
td = 0
if self._t is not None:
@ -67,19 +67,19 @@ class PID:
return u
class LPF:
def __init__(self):
self.c_ = 0
self.vs_ = (0., 0., 0.)
self.fs_ = (0., 0., 0.)
def step(self, v: float) -> float:
c = self.c_
vs = (v, self.vs_[0], self.vs_[1])
fs = self.fs_
f = (1 / (1 + c * c + 1.414 * c)) * (vs[2] + 2 * vs[1] + vs[0] -
(c * c - 1.414 * c + 1) * fs[1] -
(-2 * c * c + 2) * fs[0])
f = (1 / (1 + c * c + 1.414 * c)) * (
vs[2] + 2 * vs[1] + vs[0] - (c * c - 1.414 * c + 1) * fs[1] - (-2 * c * c + 2) * fs[0])
self.vs_ = vs
self.fs_ = (f, fs[0], fs[1])
return f

11
src/nppilot/scripts/revo.py
Unescape View File

@ -25,6 +25,7 @@ class RCChannel(enum.IntEnum):
pitch = 1
throttle = 2
yaw = 3
dial = 5
mode = 6
@ -100,6 +101,7 @@ class Controller:
self._wz = rospy.Publisher('wz', Float64, queue_size=10)
self._wz_sp = rospy.Publisher('wz_sp', Float64, queue_size=10)
self._vx = rospy.Publisher('vx', Float64, queue_size=10)
self._vx_sp = rospy.Publisher('vx_sp', Float64, queue_size=10)
self._rc_rate = limiter.Rate(0.02)
self._cmd = [0] * 4
@ -123,7 +125,7 @@ class Controller:
self._wz_pid = controller.PID(
kp=0.2, ki=0.01, windup_limit=1, lower_limit=-1, upper_limit=1)
self._v_pid = controller.PID(
self._vx_pid = controller.PID(
kp=0.1, ki=0.01, windup_limit=1, lower_limit=0, upper_limit=0.7)
def rc_raw(self, msg):
@ -131,6 +133,9 @@ class Controller:
rc[RCChannel.throttle] = _scale_throttle(rc[RCChannel.throttle], self._throttle_scale)
rc = [x + trim for x, trim in zip(rc, self._rc_trim)]
rc = [round(x, 5) for x in rc]
sp = round((rc[RCChannel.dial] + 1) / 2 * 8)
self._vx_sp.publish(sp)
self._vx.sp = sp
self._rc = rc
def status(self, msg):
@ -153,7 +158,7 @@ class Controller:
t = msg.header.stamp.to_sec()
self._vx.publish(vx)
u = self._v_pid.step(t, vx)
u = self._vx_pid.step(t, vx)
self._cmd[3] = u
def send_command(self):
@ -170,7 +175,7 @@ class Controller:
f = (abs(self._rc[RCChannel.throttle]) < 0.1) and self._armed
self._enable_f.publish(f)
self._v_pid.enable = f
self._vx_pid.enable = f
def tick(self, msg):
self.send_enable()

Write Preview
Loading…
Cancel
Save