def set_power(self, **power):
Besiege.Watch('power', pretty(power.values()))
Besiege.Watch('avg_power', sum(power.values()) / len(power))
for k, v in power.items():
v = pid.clip(v, -12, 12)
for m in self.motors[k]:
m.SetSliderValue('SPEED', v)
def get_collisions(self):
radius = self.config['collision_vehicle_radius']
range_ = self.config['collision_range']
vel = self.velocity
# get velocity relative to our y rotation
d = self.quat_ry * vel
dn = d.normalized
# the nearest point is fixed relative to the vehicle
a = self.position + (dn * radius)
b = self.position + (d * range_)
if any(self._colmarks):
self._colmarks[0].Move(a)
self._colmarks[1].Move(b)
else:
self._colmarks[0] = Besiege.CreateMark(a)
self._colmarks[1] = Besiege.CreateMark(b)
hit = Physics.Linecast(a, b)
c = Color.red if hit else Color.yellow
self._colmarks[0].SetColor(c)
self._colmarks[1].SetColor(c)
# if there's a collision, find the y velocity change that clears us out of it
if hit:
for dv in xrange(10):
tvel = Vector3(vel[0], vel[1] + dv, vel[2])
dg = self.quat_ry * tvel
dng = dg.normalized
ag = self.position + (dng * radius)
bg = self.position + (dg * range_)
# if there's a hit, continue
if Physics.Linecast(ag, bg):
continue
if any(self._avdmarks):
self._avdmarks[0].Move(ag)
self._avdmarks[1].Move(bg)
else:
self._avdmarks[0] = Besiege.CreateMark(ag)
self._avdmarks[1] = Besiege.CreateMark(b)
self._avdmarks[0].SetColor(Color.green)
self._avdmarks[1].SetColor(Color.green)
else:
if any(self._avdmarks):
self._avdmarks[0].Clear()
self._avdmarks[1].Clear()
self._avdmarks = [None, None]
def __call__(self):
try:
self.update()
except Exception:
import sys
typ, val, tb = sys.exc_info()
Besiege.Watch('exc', str(typ))
Besiege.Watch('msg', val.message[-20:])
Besiege.Watch('file', tb.tb_frame.f_code.co_filename.split('/')[-1])
Besiege.Watch('line', tb.tb_lineno)
raise
def goto_position(self, throttle, pitch, yaw, roll):
h, v, t = self.position_sp_distance
self.auto_pid[0].gain_f = 0.8
self.auto_pid[1].gain_f = 0.8
self.auto_pid[2].gain_f = 0.8
# TODO: have to find a dynamic p value that gives a smooth
# ascent and descent curve.
p = 0.2
# if below target, prioritize climbing, so lower xz gain
if h > 10 and v > 10:
self.auto_pid[0].gain_f = p
self.auto_pid[2].gain_f = p
# if above target, prioritize moving, so lower y gain
if h > 10 and v < -10:
self.auto_pid[1].gain_f = p
auto_error = Vector3(self.auto_pid[0].last_error,
self.auto_pid[1].last_error,
self.auto_pid[2].last_error)
Besiege.Watch('auto_error', auto_error)
roll_adj = self.auto_pid[0].update(self.position_sp_local[0], self.dt)
throttle_adj = self.auto_pid[1].update(-self.position_sp_local[1], self.dt)
pitch_adj = self.auto_pid[2].update(-self.position_sp_local[2], self.dt)
return self.velocity_mode(throttle_adj, pitch_adj, yaw, roll_adj)
def set_next_waypoint(self):
# get the waypoint direction
b = self.waypoints[0].position
d = (b - self.position).normalized
a = self.position + (d * 10)
# check for collisions between the current position and the waypoint
if not Physics.Linecast(a, b):
self.current_waypoint = self.waypoints.pop(0)
return True
# if there's a collision, try to find a clear path above the obstacle
a2 = Vector3(a[0], a[1], a[2])
b2 = Vector3(b[0], b[1], b[2])
for y in xrange(0, 1000, 10):
a2 = Vector3(a[0], a[1] + y, a[2])
b2 = Vector3(b[0], a[1] + y, b[2])
segs = [Physics.Linecast(a, a2), Physics.Linecast(a2, b2), Physics.Linecast(b2, b)]
if any(segs):
continue
self.waypoints.insert(0, Waypoint(b2))
self.current_waypoint = Waypoint(a2)
return True
Besiege.Watch("panic", "set next waypoint")
def set_mark(self):
try:
p = Besiege.GetRaycastHit()
except Exception:
return
p[1] += 30
self.waypoints.append(Waypoint(p))
def get_collisions(self):
# get horizontal velocity relative to our y rotation
d = self.quat_ry * self.velocity
self.collision_points = [
self.position + (d.normalized * v) for v in self.collision_radius
]
if any(self.colmarks):
self.colmarks[0].Move(self.collision_points[0])
self.colmarks[1].Move(self.collision_points[1])
self.colmarks[2].Move(self.collision_points[2])
else:
self.colmarks[0] = Besiege.CreateMark(self.collision_points[0])
self.colmarks[1] = Besiege.CreateMark(self.collision_points[1])
self.colmarks[2] = Besiege.CreateMark(self.collision_points[2])
self.colmarks[0].SetColor(Color(0, 1, 0))
self.colmarks[1].SetColor(Color(0, 1, 0))
self.colmarks[2].SetColor(Color(0, 1, 0))
near = Physics.Linecast(self.collision_points[0],
self.collision_points[1])
mid = Physics.Linecast(self.collision_points[1],
self.collision_points[2])
if near:
self.colmarks[0].SetColor(Color(1, 0, 0))
self.colmarks[1].SetColor(Color(1, 0, 0))
else:
self.colmarks[0].SetColor(Color(0, 1, 0))
self.colmarks[1].SetColor(Color(0, 1, 0))
if mid:
self.colmarks[1].SetColor(Color(1, 0, 0))
self.colmarks[2].SetColor(Color(1, 0, 0))
else:
self.colmarks[1].SetColor(Color(0, 1, 0))
self.colmarks[2].SetColor(Color(0, 1, 0))
Besiege.Watch('colzones', str((near, mid)))
return near, mid
def terrain(self):
# raycast straight down and get the real altitude
p = Vector3(self.position[0], self.position[1] - self.core_height,
self.position[2])
try:
hit = Besiege.GetRaycastHit(p, Vector3.down)
except Exception:
return 0
return hit.y
def __init__(self, position, mark=None):
self.position = position
self.mark = mark or Besiege.CreateMark(position)
def update(self):
self.dt = Time.deltaTime
if self.dt == 0:
return
if self._first_update:
self.home = self.core.Position
if self.core_height is None:
self.core_height = self.core.Position.y
# Besiege.ClearMarks() # ClearMarks is broken
self._first_update = False
return
# check for key press events and respective callbacks
self.update_keys()
# get the current relative angular velocity
self.rate = Quaternion.Inverse(
self.core.RotationQuaternion) * self.core.AngularVelocityDeg
# get the current relative angles
self.rotation = self.core.Rotation
# get the current world position
self.position = self.core.Position
# get the current axes values
self.controls = self.get_controls()
# A mode function can return None if it's delegating to
# another mode. In that case, we loop until we get a power
# adjustment response
power = None
while power is None:
power = self.mode(*self.controls)
Besiege.Watch('dt', self.dt)
Besiege.Watch('mode', self.mode.__name__)
# using Vector4 merely for the nice string repr
Besiege.Watch('controls', Vector4(*self.controls))
Besiege.Watch('rotation', self.rotation)
Besiege.Watch('rotation_sp', self.rotation_sp)
Besiege.Watch('position', self.position)
Besiege.Watch('position_sp', self.position_sp)
Besiege.Watch('position_sp_local', self.position_sp_local)
Besiege.Watch('position_sp_distance', self.position_sp_distance)
Besiege.Watch('rate', self.rate)
Besiege.Watch('rate_sp', self.rate_sp)
Besiege.Watch('velocity', self.velocity)
Besiege.Watch('velocity_sp', self.velocity_sp)
# get ASL and real altitude
Besiege.Watch('altitude',
Vector2(self.position.y, self.position.y - self.terrain))
self.set_power(*power)
def update(self):
self.dt = Time.deltaTime
if self.dt == 0:
return
if self._first_update:
# self.home = self.core.Position
# if self.core_height is None:
# self.core_height = self.core.Position.y
# Besiege.ClearMarks() # ClearMarks is broken
self._first_update = False
return
# check for key press events and respective callbacks
self.update_keys()
# get the current relative angular velocity
self.rate = Quaternion.Inverse(self.core.RotationQuaternion) * self.core.AngularVelocityDeg
# get the current relative angles
self.rotation = self.core.Rotation
# get the current world position
self.position = self.core.Position
# get the current axes values
self.controls = self.get_controls()
# self.get_collisions()
# A mode function can return None if it's delegating to
# another mode that was appended to the stack, or if it's
# leaving the top of the stack. In that case, we loop until we
# get a power adjustment response
adjs = None
while adjs is None:
adjs = self.mode[-1](*self.controls)
Besiege.Watch('dt', self.dt)
Besiege.Watch('mode', self.mode[-1].__name__)
Besiege.Watch('yaw_mode', self.yaw_mode.__name__ if self.yaw_mode else 'none')
Besiege.Watch('controls', pretty(self.controls))
Besiege.Watch('rate', self.rate)
Besiege.Watch('rate_sp', self.rate_sp)
Besiege.Watch('rotation', self.rotation)
Besiege.Watch('rotation_sp', self.rotation_sp)
Besiege.Watch('position', self.position)
Besiege.Watch('position_sp', self.position_sp)
Besiege.Watch('position_sp_local', self.position_sp_local)
Besiege.Watch('position_sp_distance', self.position_sp_distance)
Besiege.Watch('velocity', self.velocity)
Besiege.Watch('velocity_sp', self.velocity_sp)
# get ASL and real altitude
Besiege.Watch('altitude', Vector2(self.position.y, self.position.y - self.terrain))
self.set_power(**self.get_power(*adjs))
def __init__(self, **kwargs):
clear_marks()
config = CONFIG_DEFAULTS.copy()
config.update(kwargs)
self.hover = config['hovering_speed']
self.core = Besiege.GetBlock(config['core_block'])
self.core_height = config['core_height']
self.motors = {}
for k, v in config['motors'].items():
self.motors[k] = [Besiege.GetBlock(b) for b in v]
self.axes = {k: AdvancedControls.GetAxis(v) for (k, v) in config['axes'].items()}
self.keys = {k.split('_', 1)[1]: v for (k, v) in config.iteritems() if k.startswith('key_')}
self.mode = [self.rate_mode]
self.yaw_mode = None
self.position = Vector3()
self.position_sp = Vector3()
self.rotation = Vector3()
self.rotation_sp = Vector3()
self.rate_sp = Vector3()
self.velocity_sp = Vector3()
self.rate_pid = (
pid.PID(config['rate_gain_pitch'], limit_i=mm(1)),
pid.PID(config['rate_gain_yaw'], limit_i=mm(1)),
pid.PID(config['rate_gain_roll'], limit_i=mm(1)),
)
self.yaw_pid = pid.PID(config['yaw_gain'], limit=mm(1))
self.stabilize_pid = (
pid.AngularPID(config['stabilize_gain_pitch']),
pid.AngularPID(config['stabilize_gain_yaw']),
pid.AngularPID(config['stabilize_gain_roll']),
)
self.velocity_pid = (
pid.PID(config['velocity_gain_x'], limit=mm(2.0 / 3), limit_i=mm(config['velocity_cf_x'])),
pid.PID(config['velocity_gain_y'], limit=mm(1), limit_i=mm(config['velocity_cf_y'])),
pid.PID(config['velocity_gain_z'], limit=mm(2.0 / 3), limit_i=mm(config['velocity_cf_z'])),
)
self.althold_pid = pid.PID(config['althold_gain'], limit=mm(1))
self.poshold_pid = (
pid.PID(config['poshold_gain_x'], limit=mm(1)),
None,
pid.PID(config['poshold_gain_z'], limit=mm(1)),
)
self.auto_pid = (
pid.PID(config['auto_gain_x'], limit=mm(1)),
pid.PID(config['auto_gain_y'], limit=mm(1)),
pid.PID(config['auto_gain_z'], limit=mm(1)),
)
self.home = None
self._hold_yaw = None
self.waypoints = []
self.waypoints = [
Waypoint(Vector3(200, 100, -1500)),
Waypoint(Vector3(100, 60, -1900)),
Waypoint(Vector3(1900, 60, -1900)),
Waypoint(Vector3(1900, 60, 1900)),
Waypoint(Vector3(-1900, 60, 1900)),
Waypoint(Vector3(-1900, 60, -1900)),
]
self.current_waypoint = None
self.config = config
self._first_update = True
self._colmarks = [None, None]
self._avdmarks = [None, None]
def __init__(self, position, mark=None):
self.position = position
self.mark = mark or Besiege.CreateMark(position)
if self.mark:
self.mark.SetColor(Color.green)