Пример #1
0
    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)
Пример #2
0
    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)
Пример #3
0
    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']