void Environment::handleRadioTx(bool blue, const Packet::RadioTx& tx) {
    for (int i = 0; i < tx.robots_size(); ++i) {
        const Packet::RadioTx::Robot& cmd = tx.robots(i);

        Robot* r = robot(blue, cmd.robot_id());
        if (r) {
            // run controls update
            r->radioTx(&cmd);

            // trigger signals to update visualization
            Geometry2d::Point pos2 = r->getPosition();
            QVector3D pos3(pos2.x, pos2.y, 0.0);
            QVector3D axis(0.0, 0.0, 1.0);
        } else {
            printf("Commanding nonexistent robot %s:%d\n",
                   blue ? "Blue" : "Yellow", cmd.robot_id());
        }

        Packet::RadioRx rx = r->radioRx();
        rx.set_robot_id(r->shell);

        // Send the RX packet
        std::string out;
        rx.SerializeToString(&out);
        if (blue)
            _radioSocketBlue.writeDatagram(&out[0], out.size(), LocalAddress,
                                           RadioRxPort + 1);
        else
            _radioSocketYellow.writeDatagram(&out[0], out.size(), LocalAddress,
                                             RadioRxPort);
    }

    // FIXME: the interface changed for this part
    // Robot *rev = robot(blue, tx.robot_id());
    // if (rev)
    // {
    // 	Packet::RadioRx rx = rev->radioRx();
    // 	rx.set_robot_id(tx.robot_id());
    //
    // 	// Send the RX packet
    // 	std::string out;
    // 	rx.SerializeToString(&out);
    // 	_radioSocket[ch].writeDatagram(&out[0], out.size(), LocalAddress,
    // RadioRxPort + ch);
    // }
}
Esempio n. 2
0
void Processor::sendRadioData() {
    Packet::RadioTx* tx = _state.logFrame->mutable_radio_tx();
    tx->set_txmode(Packet::RadioTx::UNICAST);

    // Halt overrides normal motion control, but not joystick
    if (_state.gameState.halt()) {
        // Force all motor speeds to zero
        for (OurRobot* r : _state.self) {
            Packet::Control* control = r->control;
            control->set_xvelocity(0);
            control->set_yvelocity(0);
            control->set_avelocity(0);
            control->set_dvelocity(0);
            control->set_kcstrength(0);
            control->set_shootmode(Packet::Control::KICK);
            control->set_triggermode(Packet::Control::STAND_DOWN);
            control->set_song(Packet::Control::STOP);
        }
    }

    // Add RadioTx commands for visible robots and apply joystick input
    for (OurRobot* r : _state.self) {
        if (r->visible || _manualID == r->shell()) {
            Packet::Robot* txRobot = tx->add_robots();

            // Copy motor commands.
            // Even if we are using the joystick, this sets robot_id and the
            // number of motors.
            txRobot->CopyFrom(r->robotPacket);

            if (r->shell() == _manualID) {
                const JoystickControlValues controlVals =
                    getJoystickControlValues();
                applyJoystickControls(controlVals, txRobot->mutable_control(),
                                      r);
            }
        }
    }

    if (_radio) {
        _radio->send(*_state.logFrame->mutable_radio_tx());
    }
}
Esempio n. 3
0
void Processor::sendRadioData() {
    Packet::RadioTx* tx = _state.logFrame->mutable_radio_tx();

    // Halt overrides normal motion control, but not joystick
    if (_state.gameState.halt()) {
        // Force all motor speeds to zero
        for (OurRobot* r : _state.self) {
            Packet::RadioTx::Robot& txRobot = r->radioTx;
            txRobot.set_body_x(0);
            txRobot.set_body_y(0);
            txRobot.set_body_w(0);
            txRobot.set_kick(0);
            txRobot.set_dribbler(0);
        }
    }

    // Add RadioTx commands for visible robots and apply joystick input
    for (OurRobot* r : _state.self) {
        if (r->visible || _manualID == r->shell()) {
            Packet::RadioTx::Robot* txRobot = tx->add_robots();

            // Copy motor commands.
            // Even if we are using the joystick, this sets robot_id and the
            // number of motors.
            txRobot->CopyFrom(r->radioTx);

            if (r->shell() == _manualID) {
                JoystickControlValues controlVals = getJoystickControlValues();
                applyJoystickControls(controlVals, txRobot, r);
            }
        }
    }

    if (_radio) {
        _radio->send(*_state.logFrame->mutable_radio_tx());
    }
}
Esempio n. 4
0
void SimRadio::send(Packet::RadioTx& packet)
{
	std::string out;
	packet.SerializeToString(&out);
	_socket.writeDatagram(&out[0], out.size(), LocalAddress, RadioTxPort + _channel);
}
Esempio n. 5
0
void USBRadio::send(Packet::RadioTx& packet) {
    QMutexLocker lock(&_mutex);
    if (!_device) {
        if (!open()) {
            return;
        }
    }

    uint8_t forward_packet[rtp::Forward_Size];

    // ensure Forward_Size is correct
    static_assert(sizeof(rtp::header_data) + 6 * sizeof(rtp::ControlMessage) ==
                      rtp::Forward_Size,
                  "Forward packet contents exceeds buffer size");

    // Unit conversions
    static const float Seconds_Per_Cycle = 0.005f;
    static const float Meters_Per_Tick = 0.026f * 2 * M_PI / 6480.0f;
    static const float Radians_Per_Tick = 0.026f * M_PI / (0.0812f * 3240.0f);

    rtp::header_data* header = (rtp::header_data*)forward_packet;
    header->port = rtp::Port::CONTROL;
    header->address = rtp::BROADCAST_ADDRESS;
    header->type = rtp::header_data::Type::Control;

    // Build a forward packet
    for (int slot = 0; slot < 6; ++slot) {
        // Calculate the offset into the @forward_packet for this robot's
        // control message and cast it to a ControlMessage pointer for easy
        // access
        size_t offset =
            sizeof(rtp::header_data) + slot * sizeof(rtp::ControlMessage);
        rtp::ControlMessage* msg =
            (rtp::ControlMessage*)(forward_packet + offset);

        if (slot < packet.robots_size()) {
            const Packet::Control& robot = packet.robots(slot).control();

            msg->uid = packet.robots(slot).uid();

            msg->bodyX =
                robot.xvelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;
            msg->bodyY =
                robot.yvelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;
            msg->bodyW =
                robot.avelocity() * rtp::ControlMessage::VELOCITY_SCALE_FACTOR;

            msg->dribbler =
                max(0, min(255, static_cast<uint16_t>(robot.dvelocity()) * 2));

            msg->kickStrength = robot.kcstrength();

            msg->shootMode = robot.shootmode();
            msg->triggerMode = robot.triggermode();
            msg->song = robot.song();
        } else {
            // empty slot
            msg->uid = rtp::INVALID_ROBOT_UID;
        }
    }

    // Send the forward packet
    int sent = 0;
    int transferRetCode =
        libusb_bulk_transfer(_device, LIBUSB_ENDPOINT_OUT | 2, forward_packet,
                             sizeof(forward_packet), &sent, Control_Timeout);
    if (transferRetCode != LIBUSB_SUCCESS || sent != sizeof(forward_packet)) {
        fprintf(stderr, "USBRadio: Bulk write failed\n");
        if (transferRetCode != LIBUSB_SUCCESS)
            fprintf(stderr, "  Error: '%s'\n",
                    libusb_error_name(transferRetCode));

        libusb_close(_device);
        _device = nullptr;
    }
}