<commit_before>// Copyright (c) 2013-2019 mogemimi. Distributed under the MIT license.

#include "GamepadMappings.hpp"
#include "../Basic/ConditionalCompilation.hpp"
#include "Pomdog/Basic/Platform.hpp"
#include "Pomdog/Input/GamepadState.hpp"
#include "Pomdog/Logging/Log.hpp"
#include "Pomdog/Utility/Assert.hpp"
#include "Pomdog/Utility/StringHelper.hpp"
#include <algorithm>
#include <array>
#include <unordered_map>
#include <utility>

namespace Pomdog {
namespace Detail {
namespace InputSystem {
namespace {

#ifdef POMDOG_PLATFORM_MACOSX
#ifndef __MACOSX__
#define __MACOSX__ 1
#endif
#endif

#ifdef POMDOG_PLATFORM_LINUX
#ifndef __LINUX__
#define __LINUX__ 1
#endif
#endif

#ifdef POMDOG_PLATFORM_WIN32
#ifndef SDL_JOYSTICK_XINPUT
#define SDL_JOYSTICK_XINPUT 1
#endif
#ifndef SDL_JOYSTICK_DINPUT
#define SDL_JOYSTICK_DINPUT 1
#endif
#endif

#include "SDL_gamecontrollerdb2.0.9.h"

std::tuple<std::string, const char*> Parse(const char* source, char delimiter)
{
    std::string identifier;
    while (*source != 0) {
        if (*source == delimiter) {
            ++source;
            break;
        }
        identifier += *source;
        ++source;
    }
    return std::make_tuple(identifier, source);
}

void ParseMapping(const char* source, GamepadMappings& mappings, std::string& name)
{
    std::string uuid;
    std::string key;
    std::string index;

    std::tie(uuid, source) = Parse(source, ',');
    std::tie(name, source) = Parse(source, ',');

    const std::unordered_map<std::string, std::tuple<ButtonKind, ThumbStickKind>> dict = {
        {"a", std::make_tuple(ButtonKind::A, ThumbStickKind::None)},
        {"b", std::make_tuple(ButtonKind::B, ThumbStickKind::None)},
        {"x", std::make_tuple(ButtonKind::X, ThumbStickKind::None)},
        {"y", std::make_tuple(ButtonKind::Y, ThumbStickKind::None)},
        {"guide", std::make_tuple(ButtonKind::Guide, ThumbStickKind::None)},
        {"back", std::make_tuple(ButtonKind::LeftMenu, ThumbStickKind::None)},
        {"start", std::make_tuple(ButtonKind::RightMenu, ThumbStickKind::None)},
        {"leftshoulder", std::make_tuple(ButtonKind::LeftShoulder, ThumbStickKind::None)},
        {"rightshoulder", std::make_tuple(ButtonKind::RightShoulder, ThumbStickKind::None)},
        {"leftstick", std::make_tuple(ButtonKind::LeftStick, ThumbStickKind::None)},
        {"rightstick", std::make_tuple(ButtonKind::RightStick, ThumbStickKind::None)},
        {"dpup", std::make_tuple(ButtonKind::DPadUp, ThumbStickKind::None)},
        {"dpdown", std::make_tuple(ButtonKind::DPadDown, ThumbStickKind::None)},
        {"dpleft", std::make_tuple(ButtonKind::DPadLeft, ThumbStickKind::None)},
        {"dpright", std::make_tuple(ButtonKind::DPadRight, ThumbStickKind::None)},
        {"lefttrigger", std::make_tuple(ButtonKind::LeftTrigger, ThumbStickKind::None)},
        {"righttrigger", std::make_tuple(ButtonKind::RightTrigger, ThumbStickKind::None)},
        {"leftx", std::make_tuple(ButtonKind::None, ThumbStickKind::LeftStickX)},
        {"lefty", std::make_tuple(ButtonKind::None, ThumbStickKind::LeftStickY)},
        {"rightx", std::make_tuple(ButtonKind::None, ThumbStickKind::RightStickX)},
        {"righty", std::make_tuple(ButtonKind::None, ThumbStickKind::RightStickY)},
    };

    int buttonIndex = 0;
    while (*source != 0) {
        std::tie(key, source) = Parse(source, ':');
        std::tie(index, source) = Parse(source, ',');

        auto kind = dict.find(key);
        if (kind == std::end(dict)) {
            ++buttonIndex;
            continue;
        }

        if (index[0] == 'b') {
            int i = std::atoi(StringHelper::TrimLeft(index, 'b').c_str());
            if ((i >= 0) && (i < static_cast<int>(mappings.buttons.size()))) {
                mappings.buttons[i] = std::get<ButtonKind>(kind->second);
            }
        }
        else if (index[0] == 'a') {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, 'a');
            int i = std::atoi(s.c_str());
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].thumbStick = std::get<ThumbStickKind>(kind->second);
                mappings.axes[i].positiveTrigger = std::get<ButtonKind>(kind->second);
            }
        }
        else if (StringHelper::HasPrefix(index, "+a")) {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, '+');
            s = StringHelper::TrimLeft(s, 'a');
            int i = std::atoi(s.c_str());
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].positiveTrigger = std::get<ButtonKind>(kind->second);
            }
        }
        else if (StringHelper::HasPrefix(index, "-a")) {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, '-');
            s = StringHelper::TrimLeft(s, 'a');
            int i = std::atoi(s.c_str());
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].negativeTrigger = std::get<ButtonKind>(kind->second);
            }
        }

        ++buttonIndex;
    }
}

ButtonKind ToButtonIndex(int physicalIndex, GamepadButtonMappings mappings)
{
    if ((physicalIndex < 0) || (physicalIndex >= static_cast<int>(mappings.size()))) {
        return ButtonKind::None;
    }
    POMDOG_ASSERT(physicalIndex >= 0);
    POMDOG_ASSERT(physicalIndex < static_cast<int>(mappings.size()));
    return mappings[physicalIndex];
}

} // namespace

ButtonState* GetButton(GamepadState& state, const GamepadButtonMappings& mappings, int physicalIndex)
{
    const auto kind = ToButtonIndex(physicalIndex, mappings);
    return GetButton(state, kind);
}

ButtonState* GetButton(GamepadState& state, ButtonKind kind)
{
    if (kind == ButtonKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<ButtonState*, 19> buttons = {{
        &state.Buttons.A,
        &state.Buttons.B,
        &state.Buttons.X,
        &state.Buttons.Y,
        &state.Buttons.LeftShoulder,
        &state.Buttons.RightShoulder,
        &state.Buttons.LeftTrigger,
        &state.Buttons.RightTrigger,
        &state.Buttons.LeftMenu,
        &state.Buttons.RightMenu,
        &state.Buttons.LeftStick,
        &state.Buttons.RightStick,
        &state.Buttons.Guide,
        &state.Buttons.Extra1,
        &state.Buttons.Extra2,
        &state.DPad.Up,
        &state.DPad.Down,
        &state.DPad.Left,
        &state.DPad.Right,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(buttons.size()));
    return buttons[index];
}

float* GetThumbStick(GamepadState& state, ThumbStickKind kind)
{
    if (kind == ThumbStickKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<float*, 4> axes = {{
        &state.ThumbSticks.Left.X,
        &state.ThumbSticks.Left.Y,
        &state.ThumbSticks.Right.X,
        &state.ThumbSticks.Right.Y,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(axes.size()));
    return axes[index];
}

bool* HasButton(GamepadCapabilities& caps, const GamepadButtonMappings& mappings, int physicalIndex)
{
    const auto kind = ToButtonIndex(physicalIndex, mappings);
    return HasButton(caps, kind);
}

bool* HasButton(GamepadCapabilities& caps, ButtonKind kind)
{
    if (kind == ButtonKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<bool*, 19> buttons = {{
        &caps.HasAButton,
        &caps.HasBButton,
        &caps.HasXButton,
        &caps.HasYButton,
        &caps.HasLeftShoulderButton,
        &caps.HasRightShoulderButton,
        &caps.HasLeftTrigger,
        &caps.HasRightTrigger,
        &caps.HasLeftMenuButton,
        &caps.HasRightMenuButton,
        &caps.HasLeftStickButton,
        &caps.HasRightStickButton,
        &caps.HasGuideButton,
        &caps.HasExtra1Button,
        &caps.HasExtra2Button,
        nullptr, // DPad.Up
        nullptr, // DPad.Down
        nullptr, // DPad.Left
        nullptr, // DPad.Right
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(buttons.size()));
    return buttons[index];
}

bool* HasThumbStick(GamepadCapabilities& caps, ThumbStickKind kind)
{
    if (kind == ThumbStickKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<bool*, 4> axes = {{
        &caps.HasLeftXThumbStick,
        &caps.HasLeftYThumbStick,
        &caps.HasRightXThumbStick,
        &caps.HasRightYThumbStick,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(axes.size()));
    return axes[index];
}

std::tuple<GamepadMappings, std::string> GetMappings(const GamepadUUID& uuid)
{
    return GetMappings(uuid.ToString());
}

std::tuple<GamepadMappings, std::string> GetMappings(const std::string& uuidString)
{
    GamepadMappings mappings;
#if defined(POMDOG_PLATFORM_LINUX)
    // NOTE: Please see this header
    // https://github.com/torvalds/linux/blob/4982327ff6755377a8a66e84113f496f3a6c53bc/include/uapi/linux/input-event-codes.h#L379-L398
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::Guide,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
        ButtonKind::None,
        ButtonKind::None,
    }};
#elif defined(POMDOG_PLATFORM_MACOSX)
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftTrigger,
        ButtonKind::RightTrigger,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Guide,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
    }};
#else
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Guide,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
        ButtonKind::None,
        ButtonKind::None,
    }};
#endif

    mappings.axes = {{
        AxisMapper{ ThumbStickKind::LeftStickX, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::LeftStickY, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::None, ButtonKind::LeftTrigger, ButtonKind::None },
        AxisMapper{ ThumbStickKind::RightStickX, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::RightStickY, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::None, ButtonKind::RightTrigger, ButtonKind::None },
    }};

    std::string deviceName;
    for (auto m : s_ControllerMappings) {
        if (m == nullptr) {
            break;
        }
        std::string s = m;
        if (StringHelper::HasPrefix(s, uuidString)) {
            // found
            std::fill(std::begin(mappings.buttons), std::end(mappings.buttons), ButtonKind::None);
            for (auto& axis : mappings.axes) {
                axis.thumbStick = ThumbStickKind::None;
                axis.positiveTrigger = ButtonKind::None;
                axis.negativeTrigger = ButtonKind::None;
            }
            ParseMapping(m, mappings, deviceName);
            break;
        }
    }
    return std::make_tuple(mappings, deviceName);
}

} // namespace InputSystem
} // namespace Detail
} // namespace Pomdog
<commit_msg>Use std::from_chars instead of std::atoi<commit_after>// Copyright (c) 2013-2019 mogemimi. Distributed under the MIT license.

#include "GamepadMappings.hpp"
#include "../Basic/ConditionalCompilation.hpp"
#include "Pomdog/Basic/Platform.hpp"
#include "Pomdog/Input/GamepadState.hpp"
#include "Pomdog/Logging/Log.hpp"
#include "Pomdog/Utility/Assert.hpp"
#include "Pomdog/Utility/StringHelper.hpp"
#include <algorithm>
#include <array>
#include <charconv>
#include <unordered_map>
#include <utility>

namespace Pomdog {
namespace Detail {
namespace InputSystem {
namespace {

#ifdef POMDOG_PLATFORM_MACOSX
#ifndef __MACOSX__
#define __MACOSX__ 1
#endif
#endif

#ifdef POMDOG_PLATFORM_LINUX
#ifndef __LINUX__
#define __LINUX__ 1
#endif
#endif

#ifdef POMDOG_PLATFORM_WIN32
#ifndef SDL_JOYSTICK_XINPUT
#define SDL_JOYSTICK_XINPUT 1
#endif
#ifndef SDL_JOYSTICK_DINPUT
#define SDL_JOYSTICK_DINPUT 1
#endif
#endif

#include "SDL_gamecontrollerdb2.0.9.h"

std::tuple<std::string, const char*> Parse(const char* source, char delimiter)
{
    std::string identifier;
    while (*source != 0) {
        if (*source == delimiter) {
            ++source;
            break;
        }
        identifier += *source;
        ++source;
    }
    return std::make_tuple(identifier, source);
}

void ParseMapping(const char* source, GamepadMappings& mappings, std::string& name)
{
    std::string uuid;
    std::string key;
    std::string index;

    std::tie(uuid, source) = Parse(source, ',');
    std::tie(name, source) = Parse(source, ',');

    const std::unordered_map<std::string, std::tuple<ButtonKind, ThumbStickKind>> dict = {
        {"a", std::make_tuple(ButtonKind::A, ThumbStickKind::None)},
        {"b", std::make_tuple(ButtonKind::B, ThumbStickKind::None)},
        {"x", std::make_tuple(ButtonKind::X, ThumbStickKind::None)},
        {"y", std::make_tuple(ButtonKind::Y, ThumbStickKind::None)},
        {"guide", std::make_tuple(ButtonKind::Guide, ThumbStickKind::None)},
        {"back", std::make_tuple(ButtonKind::LeftMenu, ThumbStickKind::None)},
        {"start", std::make_tuple(ButtonKind::RightMenu, ThumbStickKind::None)},
        {"leftshoulder", std::make_tuple(ButtonKind::LeftShoulder, ThumbStickKind::None)},
        {"rightshoulder", std::make_tuple(ButtonKind::RightShoulder, ThumbStickKind::None)},
        {"leftstick", std::make_tuple(ButtonKind::LeftStick, ThumbStickKind::None)},
        {"rightstick", std::make_tuple(ButtonKind::RightStick, ThumbStickKind::None)},
        {"dpup", std::make_tuple(ButtonKind::DPadUp, ThumbStickKind::None)},
        {"dpdown", std::make_tuple(ButtonKind::DPadDown, ThumbStickKind::None)},
        {"dpleft", std::make_tuple(ButtonKind::DPadLeft, ThumbStickKind::None)},
        {"dpright", std::make_tuple(ButtonKind::DPadRight, ThumbStickKind::None)},
        {"lefttrigger", std::make_tuple(ButtonKind::LeftTrigger, ThumbStickKind::None)},
        {"righttrigger", std::make_tuple(ButtonKind::RightTrigger, ThumbStickKind::None)},
        {"leftx", std::make_tuple(ButtonKind::None, ThumbStickKind::LeftStickX)},
        {"lefty", std::make_tuple(ButtonKind::None, ThumbStickKind::LeftStickY)},
        {"rightx", std::make_tuple(ButtonKind::None, ThumbStickKind::RightStickX)},
        {"righty", std::make_tuple(ButtonKind::None, ThumbStickKind::RightStickY)},
    };

    int buttonIndex = 0;
    while (*source != 0) {
        std::tie(key, source) = Parse(source, ':');
        std::tie(index, source) = Parse(source, ',');

        auto kind = dict.find(key);
        if (kind == std::end(dict)) {
            ++buttonIndex;
            continue;
        }

        if (index[0] == 'b') {
            auto s = StringHelper::TrimLeft(index, 'b');
            int i = 0;
            if (auto[p, err] = std::from_chars(s.data(), s.data() + s.size(), i); err != std::errc{}) {
                i = -1;
            }
            if ((i >= 0) && (i < static_cast<int>(mappings.buttons.size()))) {
                mappings.buttons[i] = std::get<ButtonKind>(kind->second);
            }
        }
        else if (index[0] == 'a') {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, 'a');
            int i = 0;
            if (auto[p, err] = std::from_chars(s.data(), s.data() + s.size(), i); err != std::errc{}) {
                i = -1;
            }
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].thumbStick = std::get<ThumbStickKind>(kind->second);
                mappings.axes[i].positiveTrigger = std::get<ButtonKind>(kind->second);
            }
        }
        else if (StringHelper::HasPrefix(index, "+a")) {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, '+');
            s = StringHelper::TrimLeft(s, 'a');
            int i = 0;
            if (auto[p, err] = std::from_chars(s.data(), s.data() + s.size(), i); err != std::errc{}) {
                i = -1;
            }
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].positiveTrigger = std::get<ButtonKind>(kind->second);
            }
        }
        else if (StringHelper::HasPrefix(index, "-a")) {
            auto s = StringHelper::TrimRight(index, '~');
            s = StringHelper::TrimLeft(s, '-');
            s = StringHelper::TrimLeft(s, 'a');
            int i = 0;
            if (auto[p, err] = std::from_chars(s.data(), s.data() + s.size(), i); err != std::errc{}) {
                i = -1;
            }
            if ((i >= 0) && (i < static_cast<int>(mappings.axes.size()))) {
                mappings.axes[i].negativeTrigger = std::get<ButtonKind>(kind->second);
            }
        }

        ++buttonIndex;
    }
}

ButtonKind ToButtonIndex(int physicalIndex, GamepadButtonMappings mappings)
{
    if ((physicalIndex < 0) || (physicalIndex >= static_cast<int>(mappings.size()))) {
        return ButtonKind::None;
    }
    POMDOG_ASSERT(physicalIndex >= 0);
    POMDOG_ASSERT(physicalIndex < static_cast<int>(mappings.size()));
    return mappings[physicalIndex];
}

} // namespace

ButtonState* GetButton(GamepadState& state, const GamepadButtonMappings& mappings, int physicalIndex)
{
    const auto kind = ToButtonIndex(physicalIndex, mappings);
    return GetButton(state, kind);
}

ButtonState* GetButton(GamepadState& state, ButtonKind kind)
{
    if (kind == ButtonKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<ButtonState*, 19> buttons = {{
        &state.Buttons.A,
        &state.Buttons.B,
        &state.Buttons.X,
        &state.Buttons.Y,
        &state.Buttons.LeftShoulder,
        &state.Buttons.RightShoulder,
        &state.Buttons.LeftTrigger,
        &state.Buttons.RightTrigger,
        &state.Buttons.LeftMenu,
        &state.Buttons.RightMenu,
        &state.Buttons.LeftStick,
        &state.Buttons.RightStick,
        &state.Buttons.Guide,
        &state.Buttons.Extra1,
        &state.Buttons.Extra2,
        &state.DPad.Up,
        &state.DPad.Down,
        &state.DPad.Left,
        &state.DPad.Right,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(buttons.size()));
    return buttons[index];
}

float* GetThumbStick(GamepadState& state, ThumbStickKind kind)
{
    if (kind == ThumbStickKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<float*, 4> axes = {{
        &state.ThumbSticks.Left.X,
        &state.ThumbSticks.Left.Y,
        &state.ThumbSticks.Right.X,
        &state.ThumbSticks.Right.Y,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(axes.size()));
    return axes[index];
}

bool* HasButton(GamepadCapabilities& caps, const GamepadButtonMappings& mappings, int physicalIndex)
{
    const auto kind = ToButtonIndex(physicalIndex, mappings);
    return HasButton(caps, kind);
}

bool* HasButton(GamepadCapabilities& caps, ButtonKind kind)
{
    if (kind == ButtonKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<bool*, 19> buttons = {{
        &caps.HasAButton,
        &caps.HasBButton,
        &caps.HasXButton,
        &caps.HasYButton,
        &caps.HasLeftShoulderButton,
        &caps.HasRightShoulderButton,
        &caps.HasLeftTrigger,
        &caps.HasRightTrigger,
        &caps.HasLeftMenuButton,
        &caps.HasRightMenuButton,
        &caps.HasLeftStickButton,
        &caps.HasRightStickButton,
        &caps.HasGuideButton,
        &caps.HasExtra1Button,
        &caps.HasExtra2Button,
        nullptr, // DPad.Up
        nullptr, // DPad.Down
        nullptr, // DPad.Left
        nullptr, // DPad.Right
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(buttons.size()));
    return buttons[index];
}

bool* HasThumbStick(GamepadCapabilities& caps, ThumbStickKind kind)
{
    if (kind == ThumbStickKind::None) {
        return nullptr;
    }
    const auto index = static_cast<int>(kind);

    std::array<bool*, 4> axes = {{
        &caps.HasLeftXThumbStick,
        &caps.HasLeftYThumbStick,
        &caps.HasRightXThumbStick,
        &caps.HasRightYThumbStick,
    }};
    POMDOG_ASSERT(index >= 0);
    POMDOG_ASSERT(index < static_cast<int>(axes.size()));
    return axes[index];
}

std::tuple<GamepadMappings, std::string> GetMappings(const GamepadUUID& uuid)
{
    return GetMappings(uuid.ToString());
}

std::tuple<GamepadMappings, std::string> GetMappings(const std::string& uuidString)
{
    GamepadMappings mappings;
#if defined(POMDOG_PLATFORM_LINUX)
    // NOTE: Please see this header
    // https://github.com/torvalds/linux/blob/4982327ff6755377a8a66e84113f496f3a6c53bc/include/uapi/linux/input-event-codes.h#L379-L398
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::Guide,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
        ButtonKind::None,
        ButtonKind::None,
    }};
#elif defined(POMDOG_PLATFORM_MACOSX)
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftTrigger,
        ButtonKind::RightTrigger,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Guide,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
    }};
#else
    mappings.buttons = {{
        ButtonKind::A,
        ButtonKind::B,
        ButtonKind::X,
        ButtonKind::Y,
        ButtonKind::LeftShoulder,
        ButtonKind::RightShoulder,
        ButtonKind::LeftMenu,
        ButtonKind::RightMenu,
        ButtonKind::LeftStick,
        ButtonKind::RightStick,
        ButtonKind::Guide,
        ButtonKind::Extra1,
        ButtonKind::Extra2,
        ButtonKind::None,
        ButtonKind::None,
        ButtonKind::None,
    }};
#endif

    mappings.axes = {{
        AxisMapper{ ThumbStickKind::LeftStickX, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::LeftStickY, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::None, ButtonKind::LeftTrigger, ButtonKind::None },
        AxisMapper{ ThumbStickKind::RightStickX, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::RightStickY, ButtonKind::None, ButtonKind::None },
        AxisMapper{ ThumbStickKind::None, ButtonKind::RightTrigger, ButtonKind::None },
    }};

    std::string deviceName;
    for (auto m : s_ControllerMappings) {
        if (m == nullptr) {
            break;
        }
        std::string s = m;
        if (StringHelper::HasPrefix(s, uuidString)) {
            // found
            std::fill(std::begin(mappings.buttons), std::end(mappings.buttons), ButtonKind::None);
            for (auto& axis : mappings.axes) {
                axis.thumbStick = ThumbStickKind::None;
                axis.positiveTrigger = ButtonKind::None;
                axis.negativeTrigger = ButtonKind::None;
            }
            ParseMapping(m, mappings, deviceName);
            break;
        }
    }
    return std::make_tuple(mappings, deviceName);
}

} // namespace InputSystem
} // namespace Detail
} // namespace Pomdog
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2013-2015 Cisco Systems, Inc. and others.  All rights reserved.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License v1.0 which accompanies this distribution,
 * and is available at http://www.eclipse.org/legal/epl-v10.html
 *
 */

#include <sys/socket.h>

#include "client_thread.h"
#include "BMPReader.h"
#include "Logger.h"

#include <cxxabi.h>
#include <poll.h>
#include <thread>

/**
 * Client thread cancel
 * @param arg       Pointer to ClientThreadInfo struct
 */
void ClientThread_cancel(void *arg) {
    ClientThreadInfo *cInfo = static_cast<ClientThreadInfo *>(arg);
    Logger *logger = cInfo->log;

    LOG_INFO("Thread terminating due to cancel request.");

    LOG_INFO("Closing client connection to %s:%s", cInfo->client->c_ipv4, cInfo->client->c_port);
    shutdown(cInfo->client->c_sock, SHUT_RDWR);
    close (cInfo->client->c_sock);

    close (cInfo->client->pipe_sock);
    close (cInfo->bmp_write_end_sock);
    cInfo->bmp_reader_thread->join();

    delete cInfo->bmp_reader_thread;

    delete cInfo->mysql;
    cInfo->mysql = NULL;
}

/**
 * Client thread function
 *
 * Thread function that is called when starting a new thread.
 * The DB/mysql is initialized for each thread.
 *
 * @param [in]  arg     Pointer to the BMPServer ClientInfo
 */
void *ClientThread(void *arg) {
    // Setup the args
    ThreadMgmt *thr = static_cast<ThreadMgmt *>(arg);
    Logger *logger = thr->log;

    // Setup the client thread info struct
    ClientThreadInfo cInfo;
    cInfo.mysql = NULL;
    cInfo.client = &thr->client;
    cInfo.log = thr->log;

    int sock_fds[2];
    pollfd pfd;
    unsigned char *sock_buf = NULL;

    /*
     * Setup the cleanup routine for when the thread is canceled.
     *  A thread is only canceled if openbmpd is terminated.
     */
    pthread_cleanup_push(ClientThread_cancel, &cInfo);

    try {
        // connect to mysql
        cInfo.mysql = new mysqlBMP(logger, thr->cfg->dbURL,thr->cfg->username, thr->cfg->password, thr->cfg->dbName);

        if (thr->cfg->debug_mysql)
            cInfo.mysql->enableDebug();

        BMPReader rBMP(logger, thr->cfg);
        LOG_INFO("Thread started to monitor BMP from router %s using socket %d buffer in bytes = %u",
                cInfo.client->c_ipv4, cInfo.client->c_sock, thr->cfg->bmp_buffer_size);

        // Buffer client socket using pipe
        socketpair(PF_LOCAL, SOCK_STREAM, 0, sock_fds);
        cInfo.bmp_write_end_sock = sock_fds[1];
        cInfo.client->pipe_sock = sock_fds[0];

        /*
         * Create and start the reader thread to monitor the pipe fd (read end)
         */
        bool bmp_run = true;
        cInfo.bmp_reader_thread = new std::thread([&] {rBMP.readerThreadLoop(bmp_run,cInfo.client,
                                                                             (DbInterface *)cInfo.mysql ); } );

        // Variables to handle circular buffer
        sock_buf = new unsigned char[thr->cfg->bmp_buffer_size];
        int bytes_read = 0;
        int write_buf_pos = 0;
        int read_buf_pos = 0;
        int bytes_in_buf = 0;
        bool wrap_state = false;
        unsigned char *sock_buf_read_ptr = sock_buf;
        unsigned char *sock_buf_write_ptr = sock_buf;

        /*
         * monitor and buffer the client socket
         */
        while (bmp_run) {

            if ((wrap_state and write_buf_pos < read_buf_pos) or
                    (not wrap_state and write_buf_pos < thr->cfg->bmp_buffer_size)) {

                pfd.fd = cInfo.client->c_sock;
                pfd.events = POLLIN | POLLHUP | POLLERR;
                pfd.revents = 0;

                // Attempt to read from socket
                if (poll(&pfd, 1, 5)) {
                    if (pfd.revents & POLLHUP or pfd.revents & POLLERR) {
                        bytes_read = 0;                     // Indicate to close the connection

                    } else {
                            if (not wrap_state)     // write is ahead of read in terms of buffer pointer
                                bytes_read = read(cInfo.client->c_sock, sock_buf_write_ptr,
                                                  thr->cfg->bmp_buffer_size - write_buf_pos);

                            else if (read_buf_pos > write_buf_pos) // read is ahead of write in terms of buffer pointer
                                bytes_read = read(cInfo.client->c_sock, sock_buf_write_ptr,
                                                  read_buf_pos - write_buf_pos);
                    }

                    if (bytes_read <= 0) {
                        close(sock_fds[0]);
                        close(sock_fds[1]);
                        close(cInfo.client->c_sock);

                        bmp_run = false;
                        cInfo.bmp_reader_thread->join();
                        delete cInfo.bmp_reader_thread;
                        break;
                    }
                    else {
                        sock_buf_write_ptr += bytes_read;
                        write_buf_pos += bytes_read;
                    }

                }

            } else if (write_buf_pos >= thr->cfg->bmp_buffer_size) { // if reached end of buffer space
                // Reached end of buffer, wrap to start
                write_buf_pos = 0;
                sock_buf_write_ptr = sock_buf;
                wrap_state = wrap_state ? false : true;
                //LOG_INFO("write buffer wrapped");
            }

            /*
            else {
                LOG_INFO("buffer stall, waiting for read to catch up");
            }*/

            //if (write_buf_pos != read_buf_pos)
            //    LOG_INFO("CHECK: state=%d w=%u r=%u", wrap_state, write_buf_pos, read_buf_pos);

            if ((not wrap_state and read_buf_pos < write_buf_pos) or
                    (wrap_state and read_buf_pos < thr->cfg->bmp_buffer_size)) {

                pfd.fd = cInfo.bmp_write_end_sock;
                pfd.events = POLLOUT | POLLHUP | POLLERR;
                pfd.revents = 0;

                // Attempt to write buffer to bmp reader
                if (poll(&pfd, 1, 10)) {

                    if (pfd.revents & POLLHUP or pfd.revents & POLLERR) {
                        close(sock_fds[0]);
                        close(sock_fds[1]);
                        close(cInfo.client->c_sock);

                        bmp_run = false;
                        cInfo.bmp_reader_thread->join();
                        delete cInfo.bmp_reader_thread;
                        break;
                    }

                    if (not wrap_state) // Write buffer is a head of read in terms of buffer pointer
                        bytes_read = write(cInfo.bmp_write_end_sock, sock_buf_read_ptr,
                                           (write_buf_pos - read_buf_pos) > 8192 ? 8192 : (write_buf_pos - read_buf_pos));

                    else // Read buffer is ahead of write in terms of buffer pointer
                        bytes_read = write(cInfo.bmp_write_end_sock, sock_buf_read_ptr,
                                           (thr->cfg->bmp_buffer_size - read_buf_pos) > 8192 ? 8192 : (thr->cfg->bmp_buffer_size - read_buf_pos));

                    if (bytes_read > 0) {
                        sock_buf_read_ptr += bytes_read;
                        read_buf_pos += bytes_read;
                    }
                }
            }
            else if (read_buf_pos >= thr->cfg->bmp_buffer_size) {
                read_buf_pos = 0;
                sock_buf_read_ptr = sock_buf;
                wrap_state = wrap_state ? false : true;
                //LOG_INFO("read buffer wrapped");
            }
        }

        LOG_INFO("%s: Thread for sock [%d] ended normally", cInfo.client->c_ipv4, cInfo.client->c_sock);

    } catch (char const *str) {
        LOG_INFO("%s: %s - Thread for sock [%d] ended", cInfo.client->c_ipv4, str, cInfo.client->c_sock);
        close(sock_fds[0]);
        close(sock_fds[1]);

    } catch (abi::__forced_unwind&) {
        close(sock_fds[0]);
        close(sock_fds[1]);
        throw;

    } catch (...) {
        LOG_INFO("%s: Thread for sock [%d] ended abnormally: ", cInfo.client->c_ipv4, cInfo.client->c_sock);
        close(sock_fds[0]);
        close(sock_fds[1]);

    }

    if (sock_buf != NULL)
        delete [] sock_buf;

    pthread_cleanup_pop(0);

    // Delete mysql
    if (cInfo.mysql != NULL)
       delete cInfo.mysql;

    // close the socket
    shutdown(cInfo.client->c_sock, SHUT_RDWR);
    close (cInfo.client->c_sock);

    // Indicate that we are no longer running
    thr->running = false;

    // Exit the thread
    pthread_exit(NULL);

    return NULL;
}
<commit_msg>Fixed centos6 compilation error<commit_after>/*
 * Copyright (c) 2013-2015 Cisco Systems, Inc. and others.  All rights reserved.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License v1.0 which accompanies this distribution,
 * and is available at http://www.eclipse.org/legal/epl-v10.html
 *
 */

#include <sys/socket.h>

#include <cstdlib>
#include <cstring>
#include <iostream>
#include <thread>

#include "client_thread.h"
#include "BMPReader.h"
#include "Logger.h"

#include <cxxabi.h>
#include <poll.h>

/**
 * Client thread cancel
 * @param arg       Pointer to ClientThreadInfo struct
 */
void ClientThread_cancel(void *arg) {
    ClientThreadInfo *cInfo = static_cast<ClientThreadInfo *>(arg);
    Logger *logger = cInfo->log;

    LOG_INFO("Thread terminating due to cancel request.");

    LOG_INFO("Closing client connection to %s:%s", cInfo->client->c_ipv4, cInfo->client->c_port);
    shutdown(cInfo->client->c_sock, SHUT_RDWR);
    close (cInfo->client->c_sock);

    close (cInfo->client->pipe_sock);
    close (cInfo->bmp_write_end_sock);
    cInfo->bmp_reader_thread->join();

    delete cInfo->bmp_reader_thread;

    delete cInfo->mysql;
    cInfo->mysql = NULL;
}

/**
 * Client thread function
 *
 * Thread function that is called when starting a new thread.
 * The DB/mysql is initialized for each thread.
 *
 * @param [in]  arg     Pointer to the BMPServer ClientInfo
 */
void *ClientThread(void *arg) {
    // Setup the args
    ThreadMgmt *thr = static_cast<ThreadMgmt *>(arg);
    Logger *logger = thr->log;

    // Setup the client thread info struct
    ClientThreadInfo cInfo;
    cInfo.mysql = NULL;
    cInfo.client = &thr->client;
    cInfo.log = thr->log;

    int sock_fds[2];
    pollfd pfd;
    unsigned char *sock_buf = NULL;

    /*
     * Setup the cleanup routine for when the thread is canceled.
     *  A thread is only canceled if openbmpd is terminated.
     */
    pthread_cleanup_push(ClientThread_cancel, &cInfo);

    try {
        // connect to mysql
        cInfo.mysql = new mysqlBMP(logger, thr->cfg->dbURL,thr->cfg->username, thr->cfg->password, thr->cfg->dbName);

        if (thr->cfg->debug_mysql)
            cInfo.mysql->enableDebug();

        BMPReader rBMP(logger, thr->cfg);
        LOG_INFO("Thread started to monitor BMP from router %s using socket %d buffer in bytes = %u",
                cInfo.client->c_ipv4, cInfo.client->c_sock, thr->cfg->bmp_buffer_size);

        // Buffer client socket using pipe
        socketpair(PF_LOCAL, SOCK_STREAM, 0, sock_fds);
        cInfo.bmp_write_end_sock = sock_fds[1];
        cInfo.client->pipe_sock = sock_fds[0];

        /*
         * Create and start the reader thread to monitor the pipe fd (read end)
         */
        bool bmp_run = true;
        //cInfo.bmp_reader_thread = new std::thread([&] {rBMP.readerThreadLoop(bmp_run,cInfo.client,
        cInfo.bmp_reader_thread = new std::thread(&BMPReader::readerThreadLoop, &rBMP, std::ref(bmp_run), cInfo.client,
                                                                             (DbInterface *)cInfo.mysql );

        // Variables to handle circular buffer
        sock_buf = new unsigned char[thr->cfg->bmp_buffer_size];
        int bytes_read = 0;
        int write_buf_pos = 0;
        int read_buf_pos = 0;
        int bytes_in_buf = 0;
        bool wrap_state = false;
        unsigned char *sock_buf_read_ptr = sock_buf;
        unsigned char *sock_buf_write_ptr = sock_buf;

        /*
         * monitor and buffer the client socket
         */
        while (bmp_run) {

            if ((wrap_state and write_buf_pos < read_buf_pos) or
                    (not wrap_state and write_buf_pos < thr->cfg->bmp_buffer_size)) {

                pfd.fd = cInfo.client->c_sock;
                pfd.events = POLLIN | POLLHUP | POLLERR;
                pfd.revents = 0;

                // Attempt to read from socket
                if (poll(&pfd, 1, 5)) {
                    if (pfd.revents & POLLHUP or pfd.revents & POLLERR) {
                        bytes_read = 0;                     // Indicate to close the connection

                    } else {
                            if (not wrap_state)     // write is ahead of read in terms of buffer pointer
                                bytes_read = read(cInfo.client->c_sock, sock_buf_write_ptr,
                                                  thr->cfg->bmp_buffer_size - write_buf_pos);

                            else if (read_buf_pos > write_buf_pos) // read is ahead of write in terms of buffer pointer
                                bytes_read = read(cInfo.client->c_sock, sock_buf_write_ptr,
                                                  read_buf_pos - write_buf_pos);
                    }

                    if (bytes_read <= 0) {
                        close(sock_fds[0]);
                        close(sock_fds[1]);
                        close(cInfo.client->c_sock);

                        bmp_run = false;
                        cInfo.bmp_reader_thread->join();
                        delete cInfo.bmp_reader_thread;
                        break;
                    }
                    else {
                        sock_buf_write_ptr += bytes_read;
                        write_buf_pos += bytes_read;
                    }

                }

            } else if (write_buf_pos >= thr->cfg->bmp_buffer_size) { // if reached end of buffer space
                // Reached end of buffer, wrap to start
                write_buf_pos = 0;
                sock_buf_write_ptr = sock_buf;
                wrap_state = wrap_state ? false : true;
                //LOG_INFO("write buffer wrapped");
            }

            /*
            else {
                LOG_INFO("buffer stall, waiting for read to catch up");
            }*/

            //if (write_buf_pos != read_buf_pos)
            //    LOG_INFO("CHECK: state=%d w=%u r=%u", wrap_state, write_buf_pos, read_buf_pos);

            if ((not wrap_state and read_buf_pos < write_buf_pos) or
                    (wrap_state and read_buf_pos < thr->cfg->bmp_buffer_size)) {

                pfd.fd = cInfo.bmp_write_end_sock;
                pfd.events = POLLOUT | POLLHUP | POLLERR;
                pfd.revents = 0;

                // Attempt to write buffer to bmp reader
                if (poll(&pfd, 1, 10)) {

                    if (pfd.revents & POLLHUP or pfd.revents & POLLERR) {
                        close(sock_fds[0]);
                        close(sock_fds[1]);
                        close(cInfo.client->c_sock);

                        bmp_run = false;
                        cInfo.bmp_reader_thread->join();
                        delete cInfo.bmp_reader_thread;
                        break;
                    }

                    if (not wrap_state) // Write buffer is a head of read in terms of buffer pointer
                        bytes_read = write(cInfo.bmp_write_end_sock, sock_buf_read_ptr,
                                           (write_buf_pos - read_buf_pos) > 8192 ? 8192 : (write_buf_pos - read_buf_pos));

                    else // Read buffer is ahead of write in terms of buffer pointer
                        bytes_read = write(cInfo.bmp_write_end_sock, sock_buf_read_ptr,
                                           (thr->cfg->bmp_buffer_size - read_buf_pos) > 8192 ? 8192 : (thr->cfg->bmp_buffer_size - read_buf_pos));

                    if (bytes_read > 0) {
                        sock_buf_read_ptr += bytes_read;
                        read_buf_pos += bytes_read;
                    }
                }
            }
            else if (read_buf_pos >= thr->cfg->bmp_buffer_size) {
                read_buf_pos = 0;
                sock_buf_read_ptr = sock_buf;
                wrap_state = wrap_state ? false : true;
                //LOG_INFO("read buffer wrapped");
            }
        }

        LOG_INFO("%s: Thread for sock [%d] ended normally", cInfo.client->c_ipv4, cInfo.client->c_sock);

    } catch (char const *str) {
        LOG_INFO("%s: %s - Thread for sock [%d] ended", cInfo.client->c_ipv4, str, cInfo.client->c_sock);
        close(sock_fds[0]);
        close(sock_fds[1]);

    } catch (abi::__forced_unwind&) {
        close(sock_fds[0]);
        close(sock_fds[1]);
        throw;

    } catch (...) {
        LOG_INFO("%s: Thread for sock [%d] ended abnormally: ", cInfo.client->c_ipv4, cInfo.client->c_sock);
        close(sock_fds[0]);
        close(sock_fds[1]);

    }

    if (sock_buf != NULL)
        delete [] sock_buf;

    pthread_cleanup_pop(0);

    // Delete mysql
    if (cInfo.mysql != NULL)
       delete cInfo.mysql;

    // close the socket
    shutdown(cInfo.client->c_sock, SHUT_RDWR);
    close (cInfo.client->c_sock);

    // Indicate that we are no longer running
    thr->running = false;

    // Exit the thread
    pthread_exit(NULL);

    return NULL;
}
<|endoftext|>


<commit_before>
#include <gtest/gtest.h>
#include <Unittests/unittests_common.hh>

#include <iostream>

namespace {

class OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement : public OpenMeshBase {

    protected:

        // This function is called before each test is run
        virtual void SetUp() {
        }

        // This function is called after all tests are through
        virtual void TearDown() {

            // Do some final stuff with the member data here...
        }

    // Member already defined in OpenMeshBase
    //Mesh mesh_;
};

/*
 * ====================================================================
 * Define tests below
 * ====================================================================
 */

/*
 * duplicate dereferencing behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, dereference) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from fh_it.current_halfedge_handle()
  int current_halfedge_handles[12] = {
    4,0,2,10,6,8,1,12,7,14,3,11
  };

  size_t i(0);
  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceHalfedgeIter fh_it = mesh_.fh_iter(*f_it); fh_it.is_valid(); ++fh_it, ++i) {
      EXPECT_EQ(current_halfedge_handles[i], fh_it->idx() ) << "halfedge handles don't match";
    }
  }
}

/*
 * duplicate vv_iter behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, vv_iter) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from vv_it.current_halfedge_handle()
  size_t current_halfedge_handles[16] = {
    5,
    0,
    12,
    11,
    6,
    1,
    2,
    15,
    3,
    4,
    13,
    7,
    8,
    9,
    10,
    14
  };

  std::vector<Mesh::EdgeHandle> eh0;
  std::vector<Mesh::EdgeHandle> eh1;
  size_t i(0);
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexVertexIter vv_it = mesh_.vv_iter(*v_it); vv_it.is_valid(); ++vv_it, ++i)
      eh0.push_back(mesh_.edge_handle(Mesh::HalfedgeHandle(int(current_halfedge_handles[i]))));
  }
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexOHalfedgeIter voh_it = mesh_.voh_iter(*v_it); voh_it.is_valid(); ++voh_it)
      eh1.push_back(mesh_.edge_handle(*voh_it));
  }

  EXPECT_EQ(eh0.size(), eh1.size()) << "size of vectors does not match";
  for (size_t i = 0; i < eh0.size(); ++i)
    EXPECT_EQ(eh0[i], eh1[i]) << "edge handles do not match";

}

/*
 * duplicate fe_iter behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, fe_iter) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from fe_it.current_halfedge_handle()
  size_t current_halfedge_handles[12] = {
    4,0,2,10,6,8,1,12,7,14,3,11
  };

  size_t i(0);

  std::vector<Mesh::HalfedgeHandle> heh0;
  std::vector<Mesh::HalfedgeHandle> heh1;

  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceEdgeIter fe_it = mesh_.fe_iter(*f_it); fe_it.is_valid(); ++fe_it, ++i) {
      heh0.push_back(Mesh::HalfedgeHandle(int(current_halfedge_handles[i])));
    }
  }
  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceHalfedgeIter fh_it = mesh_.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
      heh1.push_back(*fh_it);
    }
  }

  EXPECT_EQ(heh0.size(), heh1.size()) << "size of vectors does not match";
  for (size_t i = 0; i < heh0.size(); ++i)
    EXPECT_EQ(heh0[i], heh1[i]) << "halfedge handles do not match";

}
}
<commit_msg>added a unittest that checks if the VertexFaceIter also works at boundaries after replacing current_halfedge_handle<commit_after>
#include <gtest/gtest.h>
#include <Unittests/unittests_common.hh>

#include <iostream>

namespace {

class OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement : public OpenMeshBase {

    protected:

        // This function is called before each test is run
        virtual void SetUp() {
        }

        // This function is called after all tests are through
        virtual void TearDown() {

            // Do some final stuff with the member data here...
        }

    // Member already defined in OpenMeshBase
    //Mesh mesh_;
};

/*
 * ====================================================================
 * Define tests below
 * ====================================================================
 */

/*
 * duplicate dereferencing behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, dereference) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from fh_it.current_halfedge_handle()
  int current_halfedge_handles[12] = {
    4,0,2,10,6,8,1,12,7,14,3,11
  };

  size_t i(0);
  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceHalfedgeIter fh_it = mesh_.fh_iter(*f_it); fh_it.is_valid(); ++fh_it, ++i) {
      EXPECT_EQ(current_halfedge_handles[i], fh_it->idx() ) << "halfedge handles don't match";
    }
  }
}

/*
 * duplicate vv_iter behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, vv_iter) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from vv_it.current_halfedge_handle()
  size_t current_halfedge_handles[16] = {
    5,
    0,
    12,
    11,
    6,
    1,
    2,
    15,
    3,
    4,
    13,
    7,
    8,
    9,
    10,
    14
  };

  std::vector<Mesh::EdgeHandle> eh0;
  std::vector<Mesh::EdgeHandle> eh1;
  size_t i(0);
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexVertexIter vv_it = mesh_.vv_iter(*v_it); vv_it.is_valid(); ++vv_it, ++i)
      eh0.push_back(mesh_.edge_handle(Mesh::HalfedgeHandle(int(current_halfedge_handles[i]))));
  }
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexOHalfedgeIter voh_it = mesh_.voh_iter(*v_it); voh_it.is_valid(); ++voh_it)
      eh1.push_back(mesh_.edge_handle(*voh_it));
  }

  EXPECT_EQ(eh0.size(), eh1.size()) << "size of vectors does not match";
  for (size_t i = 0; i < eh0.size(); ++i)
    EXPECT_EQ(eh0[i], eh1[i]) << "edge handles do not match";

}

/*
 * duplicate fe_iter behaviour
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, fe_iter) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[5];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(0,-1, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add two faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[1]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
      0 ==== 2
      |\  0 /|
      | \  / |
      |2  1 3|
      | /  \ |
      |/  1 \|
      3 ==== 4 */
  // Starting vertex is 1->4

  // output from fe_it.current_halfedge_handle()
  size_t current_halfedge_handles[12] = {
    4,0,2,10,6,8,1,12,7,14,3,11
  };

  size_t i(0);

  std::vector<Mesh::HalfedgeHandle> heh0;
  std::vector<Mesh::HalfedgeHandle> heh1;

  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceEdgeIter fe_it = mesh_.fe_iter(*f_it); fe_it.is_valid(); ++fe_it, ++i) {
      heh0.push_back(Mesh::HalfedgeHandle(int(current_halfedge_handles[i])));
    }
  }
  for (Mesh::FaceIter f_it = mesh_.faces_begin(); f_it != mesh_.faces_end(); ++f_it) {
    for (Mesh::FaceHalfedgeIter fh_it = mesh_.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
      heh1.push_back(*fh_it);
    }
  }

  EXPECT_EQ(heh0.size(), heh1.size()) << "size of vectors does not match";
  for (size_t i = 0; i < heh0.size(); ++i)
    EXPECT_EQ(heh0[i], heh1[i]) << "halfedge handles do not match";

}

/*
 * Small BoundaryVertexFaceIterator Test
 */
TEST_F(OpenMeshTrimeshCirculatorCurrentHalfedgeHandleReplacement, vf_iter_boundary) {

  mesh_.clear();

  // Add some vertices
  Mesh::VertexHandle vhandle[6];

  vhandle[0] = mesh_.add_vertex(Mesh::Point(0, 1, 0));
  vhandle[1] = mesh_.add_vertex(Mesh::Point(1, 0, 0));
  vhandle[2] = mesh_.add_vertex(Mesh::Point(2, 1, 0));
  vhandle[3] = mesh_.add_vertex(Mesh::Point(3, 0, 0));
  vhandle[4] = mesh_.add_vertex(Mesh::Point(4, 1, 0));
  vhandle[5] = mesh_.add_vertex(Mesh::Point(2,-1, 0));

  // Add three faces
  std::vector<Mesh::VertexHandle> face_vhandles;

  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[3]);
  face_vhandles.push_back(vhandle[4]);
  mesh_.add_face(face_vhandles);

  face_vhandles.clear();

  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[5]);
  face_vhandles.push_back(vhandle[3]);
  mesh_.add_face(face_vhandles);

  /* Test setup:
   *
   * 0 ------ 2 ------ 4
   *  \      / \      /
   *   \  0 /   \  1 /
   *    \  /     \  /
   *     1 ------- 3
   *      \       /
   *       \  2  /
   *        \   /
   *         \ /
   *          5
   */
  size_t current_halfedge_handles[9] = {
    0,2,12,4,6,8,16,10,14
  };
  std::vector<Mesh::FaceHandle> fh0;
  std::vector<Mesh::FaceHandle> fh1;
  size_t i(0);
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexFaceIter vf_it = mesh_.vf_iter(*v_it); vf_it.is_valid(); ++vf_it, ++i)
      fh0.push_back(mesh_.face_handle(Mesh::HalfedgeHandle(current_halfedge_handles[i])));
  }
  for (Mesh::VertexIter v_it = mesh_.vertices_begin(); v_it != mesh_.vertices_end(); ++v_it) {
    for (Mesh::VertexFaceIter vf_it = mesh_.vf_iter(*v_it); vf_it.is_valid(); ++vf_it)
      fh1.push_back(*vf_it);
  }

  EXPECT_EQ(fh0.size(), fh1.size()) << "size of vectors does not match";
  for (size_t i = 0; i < fh0.size(); ++i)
    EXPECT_EQ(fh0[i], fh1[i]) << "face handles do not match";
}

}
<|endoftext|>


<commit_before>/*--------------------------------------------------*/
/* @Program name ; mbed_servo                       */
/* @Author : Kohei Izumi                            */
/* @Comment : Servo motor operation                 */
/*                                                  */
/* Copyright(c) 2014 Kohei Izumi                    */
/*--------------------------------------------------*/

#include "mbed.h"
Serial device(p13, p14);         // tx = P9, rx = P10
DigitalOut REDE(p11);           // RS485 Transmit Enable
/*--------------------------------------------------*/
/* Function     : mbed initialize                   */
/* NAME         : Init                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Init(void){
    device.baud(115200);        // baud Rate = 115.2kbps [Futaba default]
    REDE = 0;                   // RS485 Transmit disable
}
/*--------------------------------------------------*/
/* Function     : Servo torque enable               */
/* NAME         : Torque                            */
/* Argument     : ID (Servo ID)                     */
/*              : data (Torque enable)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Torque (unsigned char ID, unsigned char data){

    unsigned char TxData[10];    // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = data;           // Data

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<8; i++){
        CheckSum = CheckSum ^ TxData[i];
    }

    TxData[8] = CheckSum;       // Sum

    // Send Packet
    REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<9; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmitt disable
}
/*--------------------------------------------------*/
/* Function     : Servo goal position instruction   */
/*                (Not specify time)                */
/* NAME         : SetPosition                       */
/* Argument     : ID (Servo ID)                     */
/*              : data (Set position)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetPosition (unsigned char ID, short data){

    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<9; i++){
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;       // Sum
    // Send Packet
    REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<10; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmit disable
}

/*--------------------------------------------------*/
/* Function     : Set the time and position         */
/* NAME         : SetTimeAndPosition                */
/* Argument     : ID (servo ID)                     */
/*              : data (set position)               */
/*              : time (time to move)               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetTimeAndPosition(unsigned char ID, short data, unsigned short stime){
    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x04;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte
    TxData[9] = (unsigned char)0x00FF & stime;           // Low byte
    TxData[10] = (unsigned char)0x00FF * (stime >> 8);   // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for (int i=3; i<11; i++) {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[11] = CheckSum;      // Sum

    // Send Packet
    REDE = 1;                   // Transmit Enable
    for(int i=0; i<12; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // Transmitt disable
}

/*--------------------------------------------------*/
/* Function     : main program                      */
/* NAME         : main                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
int main() {
    Init();                     // initialize
    Torque(1, 1);         // ID = 1(0x01) , torque = OFF (0x00)
    Torque(2, 1);
                                // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
    wait(1);                    // wait (1sec)
    while(1){
        SetTimeAndPosition(1, 0, 1);
        SetTimeAndPosition(2, 0, 1);
        wait(1);
    }
}
<commit_msg>Bug fix<commit_after>/*--------------------------------------------------*/
/* @Program name ; mbed_servo                       */
/* @Author : Kohei Izumi                            */
/* @Comment : Servo motor operation                 */
/*                                                  */
/* Copyright(c) 2014 Kohei Izumi                    */
/*--------------------------------------------------*/

#include "mbed.h"
Serial device(p13, p14);         // tx = P9, rx = P10
DigitalOut REDE(p11);           // RS485 Transmit Enable
/*--------------------------------------------------*/
/* Function     : mbed initialize                   */
/* NAME         : Init                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Init(void){
    device.baud(115200);        // baud Rate = 115.2kbps [Futaba default]
    REDE = 0;                   // RS485 Transmit disable
}
/*--------------------------------------------------*/
/* Function     : Servo torque enable               */
/* NAME         : Torque                            */
/* Argument     : ID (Servo ID)                     */
/*              : data (Torque enable)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Torque (unsigned char ID, unsigned char data){

    unsigned char TxData[10];    // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = data;           // Data

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<8; i++){
        CheckSum = CheckSum ^ TxData[i];
    }

    TxData[8] = CheckSum;       // Sum

    // Send Packet
    REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<9; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmitt disable
}
/*--------------------------------------------------*/
/* Function     : Servo goal position instruction   */
/*                (Not specify time)                */
/* NAME         : SetPosition                       */
/* Argument     : ID (Servo ID)                     */
/*              : data (Set position)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetPosition (unsigned char ID, short data){

    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<9; i++){
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;       // Sum
    // Send Packet
    REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<10; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmit disable
}

/*--------------------------------------------------*/
/* Function     : Set the time and position         */
/* NAME         : SetTimeAndPosition                */
/* Argument     : ID (servo ID)                     */
/*              : data (set position)               */
/*              : time (time to move)               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetTimeAndPosition(unsigned char ID, short data, unsigned short stime){
    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x04;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte
    TxData[9] = (unsigned char)0x00FF & stime;           // Low byte
    TxData[10] = (unsigned char)0x00FF & (stime >> 8);   // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for (int i=3; i<11; i++) {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[11] = CheckSum;      // Sum

    // Send Packet
    REDE = 1;                   // Transmit Enable
    for(int i=0; i<12; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // Transmitt disable
}

/*--------------------------------------------------*/
/* Function     : main program                      */
/* NAME         : main                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
int main() {
    Init();                     // initialize
    Torque(1, 1);         // ID = 1(0x01) , torque = OFF (0x00)
    Torque(2, 1);
                                // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
    wait(1);                    // wait (1sec)
    while(1){
        SetTimeAndPosition(1, 0, 1);
        SetTimeAndPosition(2, 0, 1);
        wait(1);
    }
}
<|endoftext|>


<commit_before>/*--------------------------------------------------*/
/* @Program name ; mbed_servo                       */
/* @Author : Kohei Izumi                            */
/* @Comment : Servo motor operation                 */
/*                                                  */
/* Copyright(c) 2014 Kohei Izumi                    */
/*--------------------------------------------------*/

#include "mbed.h"
Serial device(p9, p10);         // tx = P9, rx = P10
DigitalOut REDE(p11);           // RS485 Transmit Enable
/*--------------------------------------------------*/
/* Funcyion     : mbed initialize                   */
/* NAME         : Init                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Init(void){
    device.baud(115200);        // baud Rate = 115.2kbps [Futaba default]
    REDE = 0;                   // RS485 Transmit disable
}
/*--------------------------------------------------*/
/* Funcyion     : Servo torque enable               */
/* NAME         : Torque                            */
/* Argument     : ID (Servo ID)                     */
/*              : data (Torque enable)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Torque (unsigned char ID, unsigned char data){

    unsigned char TxData[10];    // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = data;           // Data

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<8; i++){
        CheckSum = CheckSum ^ TxData[i];
    }

    TxData[8] = CheckSum;       // Sum

    // Send Packet
    REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<9; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmitt disable
}
/*--------------------------------------------------*/
/* Function     : Servo goal position instruction   */
/*                (Not specify time)                */
/* NAME         : SetPosition                       */
/* Argument     : ID (Servo ID)                     */
/*              : data (Set position)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetPosition (unsigned char ID, short data){

    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<9; i++){
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;       // Sum
    // Send Packet
    REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<10; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmit disable
}

/*--------------------------------------------------*/
/* Function     : Set the time and position         */
/* NAME         : SetTimeAndPosition                */
/* Argument     : ID (servo ID)                     */
/*              : data (set position)               */
/*              : time (time to move)               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetTimeAndPosition(unsigned char ID, short data, unsigned short stime){
    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x04;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte
    TxData[9] = (unsigned char)0x00FF & stime;           // Low byte
    TxData[10] = (unsigned char)0x00FF * (stime >> 8);   // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for (int i=3; i<11; i++) {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[11] = CheckSum;      // Sum

    // Send Packet
    REDE = 1;                   // Transmit Enable
    for(int i=0; i<12; i++){
        device.putc(data[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // Transmitt disable
}

/*--------------------------------------------------*/
/* Function     : main program                      */
/* NAME         : main                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
int main() {
    Init();                     // initialize
    Torque(0x01, 0x01);         // ID = 1(0x01) , torque = OFF (0x00)
                                // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
    wait(1);                    // wait (1sec)
    while(1){
        SetPosition(0x01,300); // ID = 1(0x01) , GoalPosition = 30.0deg(300)
        wait(1);                // wait (1sec)
        SetPosition(0x01,-300);// ID = 1(0x01) , GoalPosition = -30.0deg(-300)
        wait(1);                // wait (1sec)

        SetTimeAndPosition(0x01, 500, 35);
	wait(1);
	SetTimeAndPosition(0x01, -500, 35);
	wait(1);
    }
}
<commit_msg>Modification of comment out.<commit_after>/*--------------------------------------------------*/
/* @Program name ; mbed_servo                       */
/* @Author : Kohei Izumi                            */
/* @Comment : Servo motor operation                 */
/*                                                  */
/* Copyright(c) 2014 Kohei Izumi                    */
/*--------------------------------------------------*/

#include "mbed.h"
Serial device(p9, p10);         // tx = P9, rx = P10
DigitalOut REDE(p11);           // RS485 Transmit Enable
/*--------------------------------------------------*/
/* Function     : mbed initialize                   */
/* NAME         : Init                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Init(void){
    device.baud(115200);        // baud Rate = 115.2kbps [Futaba default]
    REDE = 0;                   // RS485 Transmit disable
}
/*--------------------------------------------------*/
/* Function     : Servo torque enable               */
/* NAME         : Torque                            */
/* Argument     : ID (Servo ID)                     */
/*              : data (Torque enable)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void Torque (unsigned char ID, unsigned char data){

    unsigned char TxData[10];    // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = data;           // Data

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<8; i++){
        CheckSum = CheckSum ^ TxData[i];
    }

    TxData[8] = CheckSum;       // Sum

    // Send Packet
    REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<9; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmitt disable
}
/*--------------------------------------------------*/
/* Function     : Servo goal position instruction   */
/*                (Not specify time)                */
/* NAME         : SetPosition                       */
/* Argument     : ID (Servo ID)                     */
/*              : data (Set position)              */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetPosition (unsigned char ID, short data){

    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for(int i=3; i<9; i++){
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[9] = CheckSum;       // Sum
    // Send Packet
    REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<10; i++){
        device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // RS485 Transmit disable
}

/*--------------------------------------------------*/
/* Function     : Set the time and position         */
/* NAME         : SetTimeAndPosition                */
/* Argument     : ID (servo ID)                     */
/*              : data (set position)               */
/*              : time (time to move)               */
/* Return value : ---                               */
/*--------------------------------------------------*/
void SetTimeAndPosition(unsigned char ID, short data, unsigned short stime){
    unsigned char TxData[15];   // TransmitByteData [15byte]
    unsigned char CheckSum = 0; // CheckSum calculation

    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x04;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte
    TxData[9] = (unsigned char)0x00FF & stime;           // Low byte
    TxData[10] = (unsigned char)0x00FF * (stime >> 8);   // Hi  byte

    // CheckSum calculation
    CheckSum = TxData[2];
    for (int i=3; i<11; i++) {
        CheckSum = CheckSum ^ TxData[i];
    }
    TxData[11] = CheckSum;      // Sum

    // Send Packet
    REDE = 1;                   // Transmit Enable
    for(int i=0; i<12; i++){
        device.putc(data[i]);
    }
    wait_us(250);               // Wait for transmission
    REDE = 0;                   // Transmitt disable
}

/*--------------------------------------------------*/
/* Function     : main program                      */
/* NAME         : main                              */
/* Argument     : ---                               */
/* Return value : ---                               */
/*--------------------------------------------------*/
int main() {
    Init();                     // initialize
    Torque(0x01, 0x01);         // ID = 1(0x01) , torque = OFF (0x00)
                                // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
    wait(1);                    // wait (1sec)
    while(1){
        SetPosition(0x01,300); // ID = 1(0x01) , GoalPosition = 30.0deg(300)
        wait(1);                // wait (1sec)
        SetPosition(0x01,-300);// ID = 1(0x01) , GoalPosition = -30.0deg(-300)
        wait(1);                // wait (1sec)

        SetTimeAndPosition(0x01, 500, 35);
	wait(1);
	SetTimeAndPosition(0x01, -500, 35);
	wait(1);
    }
}
<|endoftext|>