def _prepare_fire_command(self, command):
"""Prepare fire command. You still need to send_control for it to take
an effect
"""
if self.server_state.get('srv_time') is None:
self.logger.error('Cannot fire -- no server time')
return
server_time = (self.server_state['srv_time']
- self.server_state['cli_time'] + time.time())
self.fire_cmd_seq += 1
timeout = NORMAL_FIRE_TIMEOUT
if FCMD._is_inpos(command):
# Give extra time for servoes to settle
timeout += INPOS_EXTRA_FIRE_TIMEOUT
self.control_dict.update(
fire_duration = self.ui_state['fire_duration'],
fire_cmd = [command, self.fire_cmd_seq],
# Ignore command if it is delayed by >0.5 sec
fire_cmd_deadline = server_time + timeout)
self.logger.debug('Sent fire command %d', command)
def _prepare_fire_command(self, command):
"""Prepare fire command. You still need to send_control for it to take
an effect
"""
if self.server_state.get('srv_time') is None:
self.logger.error('Cannot fire -- no server time')
return
server_time = (self.server_state['srv_time'] -
self.server_state['cli_time'] + time.time())
self.fire_cmd_seq += 1
timeout = NORMAL_FIRE_TIMEOUT
if FCMD._is_inpos(command):
# Give extra time for servoes to settle
timeout += INPOS_EXTRA_FIRE_TIMEOUT
self.control_dict.update(
fire_duration=self.ui_state['fire_duration'],
fire_cmd=[command, self.fire_cmd_seq],
# Ignore command if it is delayed by >0.5 sec
fire_cmd_deadline=server_time + timeout)
self.logger.debug('Sent fire command %d', command)
def _send_servo_commands_once(self, data, olddata):
if not self.mech_driver:
return
servo = self.mech_driver.servo
if servo is None:
self.logger.debug('Not sending commands -- no servo yet')
raise Return()
if olddata is None:
olddata = dict() # so one can use 'get'
now = time.time()
agitator_on = (data['agitator_mode'] == 2)
if data['agitator_mode'] == 1 and (self.auto_agitator_expires > now):
agitator_on = 1
self.status_packet['agitator_on'] = agitator_on
# re-sent magic servo command. Unlike real servo,
# it will timeout and turn off all LEDs if there were no
# commands for a while.
yield From(servo.mjmech_set_outputs(
laser=data['laser_on'],
green=data['green_led_on'],
# LED_BLUE is an actual blue LED on the board. Flash it
# as we receive the packets.
blue=(self.recent_packets % 2),
agitator=(data['agitator_pwm'] if agitator_on else 0)
))
poll_servo_id = self.next_servo_to_poll.next()
now = time.time() # update now -- we sent a command
turret_inmotion = []
new_command = False
# check for new turret command
if data.get('turret') != self.turret_last_command:
self.turret_last_command = data['turret']
self.turret_last_command_time = now
self.turret_fastpoll_until = now + TURRET_FASTPOLL_TIME
new_command = True
# Send command if needed -- when changed or periodically
if (new_command or (self.servo_poll_count % 113 == 0)
) and data.get('turret'):
if not self.turret_servoes_ready:
self.turret_servoes_ready = True
sid_list = [TURRET_SERVO_X, TURRET_SERVO_Y]
# Enable power
yield From(servo.enable_power(selector.POWER_ENABLE, sid_list))
# Configure servo
config = dict(TURRET_SERVO_CONFIG)
yield From(servo.configure_servo(config, [TURRET_SERVO_X]))
config.update(TURRET_SERVO_CONFIG_Y)
yield From(servo.configure_servo(config, [TURRET_SERVO_Y]))
servo_x, servo_y = data['turret']
send_x = min(TURRET_RANGE_X[1],
max(TURRET_RANGE_X[0], -servo_x))
send_y = min(TURRET_RANGE_Y[1],
max(TURRET_RANGE_Y[0], servo_y))
yield From(servo.set_pose({TURRET_SERVO_X: send_x,
TURRET_SERVO_Y: send_y},
pose_time=TURRET_POSE_TIME))
# Poll servoes if we are in fastpoll mode
if self.turret_fastpoll_until > now:
for axis, sid in (('x', TURRET_SERVO_X),
('y', TURRET_SERVO_Y)):
status = yield From(self._poll_servo_status(sid))
if 'moving' in status:
turret_inmotion.append(axis + '.moving')
elif 'inposition' not in status:
# 'inposition' seems to be always false when moving is true
turret_inmotion.append(axis + '.ninpos')
if sid == poll_servo_id:
# Do not poll for turret servos again
poll_servo_id = self.next_servo_to_poll.next()
# TODO mafanasyev: also add requirement to be in steady state
# for X samples?
if new_command and not turret_inmotion:
turret_inmotion.append('cmd')
# Update new inmpotion flags
inmotion_str = ','.join(turret_inmotion)
if self.status_packet['turret_inmotion'] != inmotion_str:
self.status_packet['turret_inmotion'] = inmotion_str
dt = now - self.turret_last_command_time
if inmotion_str:
self.logger.debug(
'Turret in motion: %s (dt %.3f)', inmotion_str, dt)
else:
self.logger.debug(
'Turret no longer in motion (dt %.3f)', dt)
if turret_inmotion:
# Keep fastpolling while we are moving, and then some
self.turret_fastpoll_until = now + TURRET_FASTPOLL_TIME
# poll other servos (one at a time)
self.servo_poll_count += 1
special_step = 0
if (poll_servo_id != 99) and (
'offline' not in self.last_servo_status.get(poll_servo_id, [])):
# Note that mod-factor should be relatively prime to number
# of servoes
special_step = (self.servo_poll_count % 103)
if special_step == 10:
voltage_dict = yield From(servo.get_voltage([poll_servo_id]))
self.status_packet['servo_voltage'][poll_servo_id] = \
voltage_dict.get(poll_servo_id, None)
elif special_step == 18:
temp_dict = yield From(servo.get_temperature([poll_servo_id]))
self.status_packet['servo_temp'][poll_servo_id] = \
temp_dict.get(poll_servo_id, None)
else:
yield From(self._poll_servo_status(poll_servo_id))
# Process firing
fire_cmd = data['fire_cmd']
if fire_cmd is None and self.last_fire_cmd is not None:
self.last_fire_cmd = None
# Stop firing motor
yield From(servo.mjmech_fire(fire_time=0, fire_pwm=0))
elif fire_cmd == self.last_fire_cmd:
# No new command -- current one is processed
pass
elif data['fire_cmd_deadline'] < now:
# Command expired
self.last_fire_cmd = fire_cmd
dt = now - data['fire_cmd_deadline']
self.logger.warn('Ignoring old fire_cmd %r: %.3f sec old',
fire_cmd, dt)
else:
command, seq = fire_cmd
# Start agitator early, but only for every Nth shot
# (The reason for that is if there is a BB stuck in the agitator,
# you need to fire with agitator DISABLED in order to dislodge it)
if (seq % 2) == 0:
self.auto_agitator_expires = now + AUTO_AGITATOR_TIME
if FCMD._is_inpos(command) and turret_inmotion:
# We are not in position yet. Do not fire.
pass
# Do not touch last_fire_cmd so we keep retrying the test
else:
# Firetime!
# We can do few shots at once, but no more than 25.5 sec.
self.last_fire_cmd = fire_cmd
command = fire_cmd[0]
if command == FCMD.cont:
duration = 0.5 # bigger than poll interval
# TODO mafanasyev: implement
numshots = 1
else:
numshots = FCMD._numshots(command)
duration = data['fire_duration'] * numshots
self.logger.debug('Bang bang %.2f sec!!', duration)
# TODO mafanasyev: do not add it if previous shot is not
# yet complete.
self.status_packet["shots_fired"] += numshots
yield From(servo.mjmech_fire(
fire_time=duration, fire_pwm=data['fire_motor_pwm']))
if data.get('gait'):
# We got a gait command. Start the mech driver.
if self.mech_driver and not self.mech_driver_started:
self.mech_driver_started = True
# Gait engine started, but no motion
self.status_packet["last_motion_time"] = None
self.logger.debug('Gait engine started')
CriticalTask(self.mech_driver.run())
gait = data['gait']
if gait['type'] == 'idle':
# TODO mafanasyev: update last_motion_time with the time
# motion actually ends.
command = self.mech_driver.create_default_command()
# Apply anything that isn't a movement command.
for key, value in gait.iteritems():
if key != 'type' and not key.endswith('_s'):
setattr(command, key, value)
command.lift_percent = 0.0
self.mech_driver.set_command(command)
if self.gait_commanded_nonidle:
self.logger.debug('No longer commanding gait')
self.gait_commanded_nonidle = False
elif gait['type'] == 'ripple':
self.status_packet["last_motion_time"] = time.time()
command = self.mech_driver.create_default_command()
# Now apply any values we received from the client.
for key, value in gait.iteritems():
if key != 'type':
setattr(command, key, value)
if not self.gait_commanded_nonidle:
self.logger.debug('Commanding nonidle gait %r' % gait)
self.gait_commanded_nonidle = True
self.mech_driver.set_command(command)
else:
assert False, 'Invalid gait type %r' % gait['type']
