From d4b47ccaefd7a5bcb4a614d485e2e138945d2fb8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 4 Jun 2024 21:09:37 +0900 Subject: [PATCH] =?UTF-8?q?=E5=AE=9F=E6=A9=9F=E7=94=A8=E3=83=91=E3=82=B1?= =?UTF-8?q?=E3=83=83=E3=83=88=E3=81=A7grSim=E3=82=92=E9=A7=86=E5=8B=95?= =?UTF-8?q?=E3=81=99=E3=82=8B=20(#3)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * use C++17 * add ibis_robot_packet.hpp * add ibis_ssl_client.h * (WIP) connect ibis packet * adjust dribbler * format * succeeded to conncet ibis packet * comment out unused code * Add ibis * set correct robot * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * Delete unnecessary diffs * update * ibis_orion.hを追加 * AIコマンドを受け取る部分 * ビルドエラー修正 * orionのパラメータを反映 * 不要なDiffの削除 * Update src/sslworld.cpp * Apply suggestions from code review * Update src/sslworld.cpp * ibis_orion.hを消してファイルまるごとコピーに変更 * windows向けのCIを削除 * mac向けのCIを削除 * よく分からん * とりあえずAIパケットで動くようになった * dockerイメージ作るやつを作った * イメージ名に大文字はだめらしい * 誤字ってたw * リングバッファ関連のファイルを追加 * omniOdometryの移植 * 変な動きをするね * AIパケットで動くことを優先して一旦理想速度指令で動かすようにした --- .github/workflows/build.yaml | 42 ---- .github/workflows/docker.yaml | 33 +++ CMakeLists.txt | 103 ++++---- config/ibis.ini | 35 +++ include/ibis/management.h | 276 ++++++++++++++++++++++ include/ibis/ring_buffer.h | 73 ++++++ include/ibis/robot_control.h | 257 ++++++++++++++++++++ include/ibis/util.h | 62 +++++ include/net/ibis_robot_packet.hpp | 194 +++++++++++++++ include/net/ibis_ssl_client.h | 380 ++++++++++++++++++++++++++++++ include/sslworld.h | 3 + src/configwidget.cpp | 2 + src/ibis/ring_buffer.c | 68 ++++++ src/ibis/robot_control.c | 253 ++++++++++++++++++++ src/ibis/util.c | 220 +++++++++++++++++ src/net/ibis_ssl_client.cpp | 4 + src/robot.cpp | 9 +- src/sslworld.cpp | 10 +- 18 files changed, 1924 insertions(+), 100 deletions(-) create mode 100644 .github/workflows/docker.yaml create mode 100644 config/ibis.ini create mode 100644 include/ibis/management.h create mode 100644 include/ibis/ring_buffer.h create mode 100644 include/ibis/robot_control.h create mode 100644 include/ibis/util.h create mode 100644 include/net/ibis_robot_packet.hpp create mode 100644 include/net/ibis_ssl_client.h create mode 100644 src/ibis/ring_buffer.c create mode 100644 src/ibis/robot_control.c create mode 100644 src/ibis/util.c create mode 100644 src/net/ibis_ssl_client.cpp diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 3ec7f355..67422dda 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -20,45 +20,3 @@ jobs: run: sudo apt-get install -y build-essential cmake pkg-config qtbase5-dev libqt5opengl5-dev libgl1-mesa-dev libglu1-mesa-dev libprotobuf-dev protobuf-compiler libode-dev libboost-dev - name: "Build" run: make - - build-macos: - runs-on: ${{ matrix.os }} - continue-on-error: true - strategy: - fail-fast: false - matrix: - os: [ macos-12, macos-13 ] - steps: - - uses: actions/checkout@v4 - - name: "Install dependencies" - run: brew tap robotology/formulae && brew install cmake pkg-config qt@5 - - name: "Build" - # for some reason qt5 is not correctly in the path and this will break whenever the location of it changes - # 5.15.11 is for macos-12 and 5.15.12 is for macos-13 - run: PATH=/usr/local/Cellar/qt@5/5.15.11/lib/cmake/Qt5:/usr/local/Cellar/qt@5/5.15.12/lib/cmake/Qt5:$PATH && make - -# Windows build does not work currently, see https://github.com/RoboCup-SSL/grSim/issues/183 -# build-windows: -# runs-on: windows-latest -# steps: -# - uses: actions/checkout@v4 -# -# - name: Install dependencies # saves / restores cache to avoid rebuilding dependencies -# uses: lukka/run-vcpkg@v11 -# with: -# vcpkgGitCommitId: 8eb57355a4ffb410a2e94c07b4dca2dffbee8e50 -# vcpkgDirectory: c:/vcpkg # folder must reside in c:\ otherwise qt wont install due to long path errors -# -# - name: Run CMake and run vcpkg to build packages -# uses: lukka/run-cmake@v10 -# with: -# # this preset is needed to actually install the vcpkg dependencies -# configurePreset: "ninja-multi-vcpkg" -# configPresetAdditionalArgs: "[-DVCPKG_TARGET_TRIPLET=x64-windows]" -# buildPreset: "ninja-multi-vcpkg" -# buildPresetAdditionalArgs: "['--config Release']" -# env: -# # [OPTIONAL] Define the vcpkg's triplet you want to enforce, otherwise the default one -# # for the hosting system will be automatically choosen (x64 is the default on all -# # platforms, e.g. `x64-osx`). -# VCPKG_DEFAULT_TRIPLET: "x64-windows" diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml new file mode 100644 index 00000000..ad87bce7 --- /dev/null +++ b/.github/workflows/docker.yaml @@ -0,0 +1,33 @@ +name: build and push ibis customized grSim image + +on: + push: + branches: + - main + workflow_dispatch: + pull_request: + +jobs: + build-and-push-image: + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Build and push Docker image + uses: docker/build-push-action@v3 + with: + context: . + push: true + tags: ghcr.io/ibis-ssl/grsim:customized diff --git a/CMakeLists.txt b/CMakeLists.txt index f9ffc9e6..6966ff2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,12 +51,13 @@ if(UNIX) endif() # set explicitly the c++ standard to use -set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") # add src dir to included directories include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/include/net) include_directories(${PROJECT_SOURCE_DIR}/include/physics) +include_directories(${PROJECT_SOURCE_DIR}/include/ibis) ## Handling depenendcies @@ -193,6 +194,10 @@ set(SOURCES src/physics/pray.cpp src/net/robocup_ssl_server.cpp src/net/robocup_ssl_client.cpp + src/ibis/util.c +# src/ibis/ring_buffer.c +# src/ibis/robot_control.c + src/net/ibis_ssl_client.cpp src/sslworld.cpp src/robot.cpp src/configwidget.cpp @@ -216,6 +221,11 @@ set(HEADERS include/physics/pray.h include/net/robocup_ssl_server.h include/net/robocup_ssl_client.h + include/net/ibis_ssl_client.h + include/ibis/util.h + include/ibis/ring_buffer.h + include/ibis/robot_control.h + include/ibis/management.h include/sslworld.h include/robot.h include/configwidget.h @@ -237,6 +247,9 @@ set(srcs ${SOURCES} ) + +message(WARNING "Sources: ${srcs}") + file(GLOB CONFIG_FILES "config/*.ini") set_source_files_properties(${CONFIG_FILES} PROPERTIES MACOSX_PACKAGE_LOCATION "config") @@ -244,68 +257,48 @@ target_sources(${app} PRIVATE ${srcs}) install(TARGETS ${app} DESTINATION bin) target_link_libraries(${app} ${libs}) -if(APPLE AND CMAKE_MACOSX_BUNDLE) - # use CMAKE_MACOSX_BUNDLE if you want to build a mac bundle - set(MACOSX_BUNDLE_ICON_FILE "${PROJECT_SOURCE_DIR}/resources/icons/grsim.icns") - set(MACOSX_BUNDLE_SHORT_VERSION_STRING ${VERSION}) - set(MACOSX_BUNDLE_VERSION ${VERSION}) - set(MACOSX_BUNDLE_LONG_VERSION_STRING "Version ${VERSION}") - set(BUNDLE_APP ${PROJECT_SOURCE_DIR}/bin/${app}.app) - install( - CODE " - include(BundleUtilities) - fixup_bundle(\"${BUNDLE_APP}\" \"\" \"/opt/local/lib;/usr/local/lib\")" - COMPONENT Runtime) - set(CPACK_GENERATOR "DragNDrop" "TGZ") -elseif(WIN32 AND CMAKE_WIN32_EXECUTABLE) - # use CMAKE_WIN32_EXECUTABLE if you want to build a windows exe - install(DIRECTORY config DESTINATION .) - install(DIRECTORY bin DESTINATION . - FILES_MATCHING PATTERN "*.dll") - set(CPACK_PACKAGE_EXECUTABLES ${app} ${app}) -else() - install(DIRECTORY config DESTINATION share/${app}) - install(FILES resources/grsim.desktop DESTINATION share/applications) - install(FILES resources/icons/grsim.svg DESTINATION share/icons/hicolor/scalable/apps) -endif() +install(DIRECTORY config DESTINATION share/${app}) +install(FILES resources/grsim.desktop DESTINATION share/applications) +install(FILES resources/icons/grsim.svg DESTINATION share/icons/hicolor/scalable/apps) + option(BUILD_CLIENTS "Choose this option if you want to build the example Qt client." ON) if(BUILD_CLIENTS) - add_subdirectory(clients/qt) +# add_subdirectory(clients/qt) endif() file(COPY README.md LICENSE.md DESTINATION ${CMAKE_BINARY_DIR}) file(RENAME ${CMAKE_BINARY_DIR}/README.md ${CMAKE_BINARY_DIR}/README.txt) file(RENAME ${CMAKE_BINARY_DIR}/LICENSE.md ${CMAKE_BINARY_DIR}/LICENSE.txt) -## Packaging -if(UNIX) - execute_process(COMMAND uname -p OUTPUT_VARIABLE ARCH) - string(STRIP ${ARCH} ARCH) - if(APPLE) - set(ARCH "osx-universal") - endif() -elseif(WIN32) - set(ARCH "win32") - set(CPACK_GENERATOR ZIP NSIS) -endif() -set(CPACK_OUTPUT_FILE_PREFIX ${PROJECT_SOURCE_DIR}/dist) -set(CPACK_PACKAGE_CONTACT ${MAINTAINER}) -if(VENDOR) - set(CPACK_PACKAGE_VENDOR ${VENDOR}) - string(TOLOWER ${CPACK_PACKAGE_VENDOR} FLAVOR) -endif() -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_BINARY_DIR}/LICENSE.txt") -set(CPACK_RESOURCE_FILE_README "${CMAKE_BINARY_DIR}/README.txt") -#set(CPACK_RESOURCE_FILE_WELCOME "${CMAKE_SOURCE_DIR}/WELCOME.txt") -set(CPACK_PACKAGE_VERSION ${VERSION}) -# Debian based specific -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libode1 (>=0.11), vartypes (>=0.7.0)") -if(FLAVOR) - set(CPACK_PACKAGE_FILE_NAME "${CMAKE_PROJECT_NAME_LOWER}_${CPACK_PACKAGE_VERSION}-${FLAVOR}_${ARCH}") -else() - set(CPACK_PACKAGE_FILE_NAME "${CMAKE_PROJECT_NAME_LOWER}_${CPACK_PACKAGE_VERSION}_${ARCH}") -endif() -include(CPack) +### Packaging +#if(UNIX) +# execute_process(COMMAND uname -p OUTPUT_VARIABLE ARCH) +# string(STRIP ${ARCH} ARCH) +# if(APPLE) +# set(ARCH "osx-universal") +# endif() +#elseif(WIN32) +# set(ARCH "win32") +# set(CPACK_GENERATOR ZIP NSIS) +#endif() +#set(CPACK_OUTPUT_FILE_PREFIX ${PROJECT_SOURCE_DIR}/dist) +#set(CPACK_PACKAGE_CONTACT ${MAINTAINER}) +#if(VENDOR) +# set(CPACK_PACKAGE_VENDOR ${VENDOR}) +# string(TOLOWER ${CPACK_PACKAGE_VENDOR} FLAVOR) +#endif() +#set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_BINARY_DIR}/LICENSE.txt") +#set(CPACK_RESOURCE_FILE_README "${CMAKE_BINARY_DIR}/README.txt") +##set(CPACK_RESOURCE_FILE_WELCOME "${CMAKE_SOURCE_DIR}/WELCOME.txt") +#set(CPACK_PACKAGE_VERSION ${VERSION}) +## Debian based specific +#set(CPACK_DEBIAN_PACKAGE_DEPENDS "libode1 (>=0.11), vartypes (>=0.7.0)") +#if(FLAVOR) +# set(CPACK_PACKAGE_FILE_NAME "${CMAKE_PROJECT_NAME_LOWER}_${CPACK_PACKAGE_VERSION}-${FLAVOR}_${ARCH}") +#else() +# set(CPACK_PACKAGE_FILE_NAME "${CMAKE_PROJECT_NAME_LOWER}_${CPACK_PACKAGE_VERSION}_${ARCH}") +#endif() +#include(CPack) diff --git a/config/ibis.ini b/config/ibis.ini new file mode 100644 index 00000000..43ee2292 --- /dev/null +++ b/config/ibis.ini @@ -0,0 +1,35 @@ +[Geometery] +CenterFromKicker= 0.073 +Radius = 0.089 +Height = 0.090 +RobotBottomZValue = 0.02 +KickerZValue = 0.005 +KickerThickness = 0.005 +KickerWidth = 0.08 +KickerHeight = 0.04 +WheelRadius = 0.027 +WheelThickness = 0.005 +Wheel1Angle = 60 +Wheel2Angle = 135 +Wheel3Angle = 225 +Wheel4Angle = 300 +[Physics] +Bodymass = 2 +Wheelmass = 0.2 +Kickermass =0.02 +KickerDampFactor = 0.2 +RollerTorqueFactor = 0.06 +RollerPerpendicularTorqueFactor = 0.005 +KickerFriction = 0.8 +WheelTangentFriction = 0.8 +WheelPerpendicularFriction = 0.05 +WheelMotorMaximumApplyingTorque= 0.2 + +MaxLinearKickSpeed=10 +MaxChipKickSpeed=10 +AccSpeedupAbsoluteMax=4 +AccSpeedupAngularMax=50 +AccBrakeAbsoluteMax=4 +AccBrakeAngularMax=50 +VelAbsoluteMax=5 +VelAngularMax=20 diff --git a/include/ibis/management.h b/include/ibis/management.h new file mode 100644 index 00000000..42582934 --- /dev/null +++ b/include/ibis/management.h @@ -0,0 +1,276 @@ +/* + * management.h + * + * Created on: Sep 1, 2019 + * Author: okada_tech + */ + +#ifndef MANAGEMENT_H_ +#define MANAGEMENT_H_ + +#include +#include +#include +#include +#include + +//#include "adc.h" +//#include "dma.h" +//#include "fdcan.h" +//#include "gpio.h" +//#include "math.h" +//#include "spi.h" +//#include "stdio.h" +//#include "stm32g4xx_hal.h" +//#include "tim.h" +//#include "usart.h" + +//#define ARM_MATH_CM4 +//#include "arm_math.h" + +enum { + MAIN_MODE_COMBINATION_CONTROL = 0, + MAIN_MODE_SPEED_CONTROL_ONLY, + MAIN_MODE_CMD_DEBUG_MODE, + MAIN_MODE_MOTOR_TEST, + MAIN_MODE_DRIBBLER_TEST, + MAIN_MODE_KICKER_AUTO_TEST, + MAIN_MODE_KICKER_MANUAL, + MAIN_MODE_NONE, + MAIN_MODE_MOTOR_CALIBRATION, + MAIN_MODE_ERROR, +}; + +enum { + BLDC_ERROR_NONE = 0, + BLDC_ERROR_UNDER_VOLTAGE = 0x0001, + BLDC_ERROR_OVER_CURRENT = 0x0002, + BLDC_ERROR_MOTOR_OVER_HEAT = 0x0004, + BLDC_ERROR_OVER_LOAD = 0x0008, + BLDC_ERROR_ENC_ERROR = 0x0010, + BLDC_ERROR_OVER_VOLTAGE = 0x0020, +}; + +enum { + BOOST_ERROR_NONE = 0, + BOOST_ERROR_UNDER_VOLTAGE = 0x0001, + BOOST_ERROR_OVER_VOLTAGE = 0x0002, + BOOST_ERROR_OVER_CURRENT = 0x0004, + BOOST_ERROR_SHORT_CURCUIT = 0x0008, + BOOST_ERROR_CHARGE_TIME = 0x0010, + BOOST_ERROR_CHARGE_POWER = 0x0020, + BOOST_ERROR_DISCHARGE = 0x0040, + BOOST_ERROR_PARAMETER = 0x0080, + BOOST_ERROR_COMMAND = 0x0100, + BOOST_ERROR_NO_CAP = 0x0200, + BOOST_ERROR_DISCHARGE_FAIL = 0x0400, + BOOST_ERROR_GD_POWER_FAIL = 0x0800, + BOOST_ERROR_COIL_OVER_HEAT = 0x1000, + BOOST_ERROR_FET_OVER_HEAT = 0x2000, +}; + + +#define MAIN_LOOP_CYCLE (60) + +#define CAN_RX_DATA_SIZE 8 +#define CAN_TX_DATA_SIZE 8 +#define OMNI_DIAMETER 0.056 +#define ROBOT_RADIUS 0.080 +#define RX_BUF_SIZE_ETHER 64 +#define TX_BUF_SIZE_ETHER 128 + +// logging time : 0.5s -> 2s : without vision mode +#define SPEED_LOG_BUF_SIZE (MAIN_LOOP_CYCLE * 2) +#define SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE (10) + +#define AI_CMD_VEL_MAX_MPS (7.0) + +#define FLAG_SSL_VISION_OK (0x01) +#define FLAG_ENABLE_KEEPER_MODE (0x02) +#define FLAG_STOP_REQUEST (0x04) +#define FLAG_ENABLE_LOCAL_VISION (0x08) +#define FLAG_DRIBBLER_UP (0x10) + +enum { + BOARD_ID_POWER = 0, + BOARD_ID_MOTOR_RIGHT, + BOARD_ID_MOTOR_LEFT, + BOARD_ID_SUB, + BOARD_ID_MAX, +}; + +typedef struct +{ + float * buffer; // float型のデータを格納する配列 + int size; // バッファのサイズ + int front; // データの先頭位置 + int rear; // データの末尾位置 + int count; // データの数 +} RingBuffer; + +typedef struct +{ + float yaw_angle, pre_yaw_angle; + float yaw_angle_rad, pre_yaw_angle_rad; + float yaw_angle_diff_integral; +} imu_t; +typedef struct +{ + uint8_t error_no[8]; + float motor_feedback[5]; // rps + float motor_feedback_velocity[5]; // m/s + float power_voltage[7]; + float temperature[7]; + float current[5]; + uint8_t ball_detection[4]; + uint32_t board_rx_timeout[BOARD_ID_MAX]; +} can_raw_t; + +typedef struct +{ + float target_theta, global_vision_theta; + float dribble_power; + float kick_power; + bool chip_en; + float local_target_speed[2]; + float local_target_speed_scalar; + float global_robot_position[2]; + float global_target_position[2]; + float global_ball_position[2]; + uint8_t allow_local_flags; + int ball_local_x, ball_local_y, ball_local_radius, ball_local_FPS; + bool vision_lost_flag, local_vision_en_flag, keeper_mode_en_flag, stop_request_flag, dribbler_up_flag; + bool pre_vision_lost_flag; + uint32_t latency_time_ms; +} ai_cmd_t; + +typedef struct +{ + float enc_angle[5]; + float pre_enc_angle[5]; + float angle_diff[4]; +} motor_t; + +typedef struct +{ + float velocity[2]; // m/s 速度指令値の入力 + float local_vel[2]; // m/s 上記とほぼ同じ + float local_vel_now[2]; // 台形制御指令値 + float local_vel_ff_factor[2]; // 最終指令速度への追従を高めるためのFF項目 + float global_vel_now[2]; // ターゲットグローバル速度 + float global_pos[2]; // 上記で移動するX,Y座標 +} target_t; + +typedef struct +{ + float vel_error_xy[2]; + float vel_error_scalar, vel_error_rad; + //float vel_error_real_scalar, vel_error_real_rad; +} accel_vector_t; + +typedef struct +{ + float velocity[2]; + float raw_odom[2]; + float floor_odom[2]; + float odom[2]; + int16_t raw[2]; + float raw_diff[2]; + uint16_t quality; + int integral_loop_cnt, loop_cnt_debug; + float pre_yaw_angle_rad, diff_yaw_angle_rad; +} mouse_t; + +typedef struct +{ + float travel_distance[2]; + float global_odom_diff[2], robot_pos_diff[2]; + float odom[2], pre_odom[2], odom_raw[2]; // フィールド + float odom_speed[2]; // フィールド + float local_odom_speed[2]; // ローカル + RingBuffer * local_speed_log[2]; + float local_odom_speed_mvf[2]; +} omni_t; + +typedef struct +{ + RingBuffer * odom_log[2]; + float global_odom_vision_diff[2]; // vision座標を基準にした移動距離(global系) + float vision_based_position[2]; // Visionによって更新された自己位置 + float position_diff[2]; // ai_cmdとvision_based_positionの差分 + float pre_global_target_position[2]; + float move_dist; // Visionとtargetが更新されてからの移動量 + float target_dist_diff; // Visionが更新された時点での現在地とtargetの距離 + float local_target_diff[2]; +} integration_control_t; + +typedef struct +{ + volatile float velocity[2]; + volatile float omega; + volatile float accel[2]; + volatile float motor_voltage[4]; +} output_t; + +typedef struct +{ + bool connected_ai; + bool connected_cm4; + bool already_connected_ai; + uint8_t check_pre; + uint8_t check_ver; + float cmd_rx_frq; + uint32_t vision_update_cycle_cnt; + uint32_t latest_ai_cmd_update_time; + uint32_t latest_cm4_cmd_update_time; +} connection_t; + +typedef struct +{ + bool error_flag, stop_flag; + uint16_t error_id, error_info; + float error_value; + uint32_t error_resume_cnt; + uint8_t main_mode; + uint32_t system_time_ms; + uint32_t stop_flag_request_time; + uint16_t kick_state; + uint32_t sw_data; +} system_t; +typedef struct +{ + volatile uint32_t print_idx; + volatile uint32_t main_loop_cnt, true_cycle_cnt, motor_zero_cnt; + volatile float vel_radian, out_total_spin, fb_total_spin, pre_yaw_angle; + volatile float true_out_total_spi, true_fb_total_spin, true_yaw_speed, limited_output; + volatile bool print_flag, acc_step_down_flag, theta_override_flag; + volatile bool latency_check_enabled; + volatile int latency_check_seq_cnt; + volatile float rotation_target_theta; + volatile uint32_t uart_rx_itr_cnt; +} debug_t; + +typedef union { + uint8_t buf[TX_BUF_SIZE_ETHER]; + + struct + { + uint8_t head[2]; + uint8_t counter, return_counter; //4 + + uint8_t kick_state; + uint8_t temperature[7]; //12 + + uint8_t error_info[8]; //20 + int8_t motor_current[4]; + uint8_t ball_detection[4]; //28 + + float yaw_angle, diff_angle; //36 + float odom[2], odom_speed[2]; // + float voltage[2]; // + } data; +} tx_msg_t; + +//extern float voltage[6]; + +#endif /* MANAGEMENT_H_ */ diff --git a/include/ibis/ring_buffer.h b/include/ibis/ring_buffer.h new file mode 100644 index 00000000..9a3da274 --- /dev/null +++ b/include/ibis/ring_buffer.h @@ -0,0 +1,73 @@ +#ifndef _RING_BUFFER_H_ +#define _RING_BUFFER_H_ + +#include +#include + +#include "management.h" + +// リングバッファの初期化 +inline RingBuffer * initRingBuffer(int size) +{ + RingBuffer * rb = (RingBuffer *)malloc(sizeof(RingBuffer)); + rb->buffer = (float *)malloc(size * sizeof(float)); + rb->size = size; + rb->front = 0; + rb->rear = -1; + rb->count = 0; + return rb; +} + +// リングバッファに要素を追加 +inline void enqueue(RingBuffer * rb, float data) +{ + if (rb->count < rb->size) { + rb->rear = (rb->rear + 1) % rb->size; + rb->buffer[rb->rear] = data; + rb->count++; + } else { + // バッファがいっぱいの場合は古いデータを上書き + rb->rear = (rb->rear + 1) % rb->size; + rb->front = (rb->front + 1) % rb->size; + rb->buffer[rb->rear] = data; + } +} + +// リングバッファから要素を取得 +inline float dequeue(RingBuffer * rb) +{ + if (rb->count > 0) { + float data = rb->buffer[rb->front]; + rb->front = (rb->front + 1) % rb->size; + rb->count--; + return data; + } else { + // バッファが空の場合は0.0を返すなど、適切なエラー処理を追加できます + return 0.0; + } +} + +// リングバッファ上のデータを新しい順にn個加算した結果を取得 +inline float sumNewestN(RingBuffer * rb, int n) +{ + if (n <= 0 || n > rb->count) { + // 無効なnの値の場合はエラーとして0.0を返す + return 0.0; + } + + int index = rb->rear; + float sum = 0.0; + for (int i = 0; i < n; i++) { + sum += rb->buffer[index]; + index = (index - 1 + rb->size) % rb->size; + } + return sum; +} + +// リングバッファの解放 +inline void freeRingBuffer(RingBuffer * rb) +{ + free(rb->buffer); + free(rb); +} +#endif diff --git a/include/ibis/robot_control.h b/include/ibis/robot_control.h new file mode 100644 index 00000000..c3f0fbfd --- /dev/null +++ b/include/ibis/robot_control.h @@ -0,0 +1,257 @@ +/* + * robot_control.h + * + * Created on: May 19, 2024 + * Author: hiroyuki + */ + +#ifndef INC_ROBOT_CONTROL_H_ +#define INC_ROBOT_CONTROL_H_ + +#include "management.h" +#include "util.h" + +// 加速度パラメーター +#define ACCEL_LIMIT (5.0) // m/ss +#define ACCEL_LIMIT_BACK (3.0) // m/ss + +// 速度制御の位置に対するフィードバックゲイン +// ~ m/s / m : -250 -> 4cm : 1m/s +#define OUTPUT_GAIN_ODOM_DIFF_KP (150) +// 上記の出力制限 +#define OUTPUT_OUTPUT_LIMIT_ODOM_DIFF (20) // + +// 0.3はややデカすぎ、0.2は割といい感じ +// accel x KP +#define FF_ACC_OUTPUT_KP (0.2) + +// radに対するゲインなので値がデカい +#define OMEGA_GAIN_KP (160.0) +#define OMEGA_GAIN_KD (4000.0) + +// ドライバ側は 50 rps 制限 +// omegaぶんは考慮しない +#define OUTPUT_XY_LIMIT (40.0) // +// omegaぶんの制限 +#define OUTPUT_OMEGA_LIMIT (20.0) // ~ rad/s + +inline void theta_control(float target_theta, accel_vector_t * acc_vel, output_t * output, imu_t * imu) +{ + // PID + output->omega = (getAngleDiff(target_theta, imu->yaw_angle_rad) * OMEGA_GAIN_KP) - (getAngleDiff(imu->yaw_angle_rad, imu->pre_yaw_angle_rad) * OMEGA_GAIN_KD); + + if (output->omega > OUTPUT_OMEGA_LIMIT) { + output->omega = OUTPUT_OMEGA_LIMIT; + } + if (output->omega < -OUTPUT_OMEGA_LIMIT) { + output->omega = -OUTPUT_OMEGA_LIMIT; + } + //output->omega = 0; +} + +inline void local_feedback(integration_control_t * integ, imu_t * imu, system_t * sys, target_t * target, ai_cmd_t * ai_cmd, omni_t * omni) +{ + const float CMB_CTRL_FACTOR_LIMIT = (3.0); // [m/s] + //const float CMB_CTRL_DIFF_DEAD_ZONE = (0.03); // [m] + const float CMB_CTRL_GAIN = (10.0); + const float CMB_CTRL_DIFF_LIMIT = (CMB_CTRL_FACTOR_LIMIT / CMB_CTRL_GAIN); + + if (sys->main_mode == MAIN_MODE_COMBINATION_CONTROL) { + } else { + for (int i = 0; i < 2; i++) { + integ->vision_based_position[i] = omni->odom[i]; + integ->position_diff[i] = ai_cmd->local_target_speed[i] / 10 - integ->vision_based_position[i]; + } + } + + // グローバル→ローカル座標系 + integ->local_target_diff[0] = integ->position_diff[0] * cos(-imu->yaw_angle_rad) - integ->position_diff[1] * sin(-imu->yaw_angle_rad); + integ->local_target_diff[1] = integ->position_diff[0] * sin(-imu->yaw_angle_rad) + integ->position_diff[1] * cos(-imu->yaw_angle_rad); + + for (int i = 0; i < 2; i++) { + // デバッグ用にomni->odomをそのままと、target_posにspeed使う + // 速度制御はodomベースなのでちょっとおかしなことになる + //integ->local_target_diff[i] = omni->odom[i] - ai_cmd->local_target_speed[i]; + + // 精密性はそれほどいらないので、振動対策に不感帯入れる + // ただの不感帯よりも、スレッショルドとか入れたほうが良いかも + /*if (integ->local_target_diff[i] < CMB_CTRL_DIFF_DEAD_ZONE && integ->local_target_diff[i] > -CMB_CTRL_DIFF_DEAD_ZONE) { + integ->local_target_diff[i] = 0; + }*/ + + // ゲインは x10 + // 吹き飛び対策で+-3.0 m/sを上限にする + if (integ->local_target_diff[i] < -CMB_CTRL_DIFF_LIMIT) { + integ->local_target_diff[i] = -CMB_CTRL_DIFF_LIMIT; + } else if (integ->local_target_diff[i] > CMB_CTRL_DIFF_LIMIT) { + integ->local_target_diff[i] = CMB_CTRL_DIFF_LIMIT; + } + + if (sys->main_mode == MAIN_MODE_COMBINATION_CONTROL) { + // 位置フィードバック項目のx10はゲイン (ベタ打ち) + /*if (ai_cmd->local_target_speed[i] * integ->local_target_diff[i] < 0) { // 位置フィードバック項が制動方向の場合 + target->velocity[i] = ai_cmd->local_target_speed[i] + (integ->local_target_diff[i] * CMB_CTRL_GAIN); //ローカル統合制御あり + } else { + target->velocity[i] = ai_cmd->local_target_speed[i]; // ローカル統合制御なし + }*/ + //target->velocity[i] = (integ->local_target_diff[i] * CMB_CTRL_GAIN); //ローカル統合制御あり + if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { + target->velocity[i] = 0; + // + } else { + target->velocity[i] = integ->local_target_diff[i] * CMB_CTRL_GAIN; + + // 指令値を速度制限として適用 + if (target->velocity[i] > fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = fabs(ai_cmd->local_target_speed[i]); + } else if (target->velocity[i] < -fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = -fabs(ai_cmd->local_target_speed[i]); + } + } + } else { + // 2 x acc x X = V^2 + // acc : ACCEL_LIMIT_BACK * 2 + // ピッタリだとたまにオーバーシュートしてしまうので0.8かける + // 動いてるけど不感帯と相性悪いっぽい。スレッショルドかなにか必要かも + /*if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { + target->velocity[i] = 0; + // + } else { + target->velocity[i] = integ->local_target_diff[i] * CMB_CTRL_GAIN; + + // 指令値を速度制限として適用 + if (target->velocity[i] > fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = fabs(ai_cmd->local_target_speed[i]); + } else if (target->velocity[i] < -fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = -fabs(ai_cmd->local_target_speed[i]); + } + }*/ + target->velocity[i] = ai_cmd->local_target_speed[i]; + } + } +} + +inline void accel_control(accel_vector_t * acc_vel, output_t * output, target_t * target, omni_t * omni) +{ + target->local_vel[0] = target->velocity[0]; + target->local_vel[1] = target->velocity[1]; + + // XY -> rad/scalarに変換 + // 座標次元でのみフィードバックを行う。速度次元ではフィードバックを行わない。 + // ここでは目標加速度のみ決定するため、内部の目標速度のみを使用する。 + for (int i = 0; i < 2; i++) { + acc_vel->vel_error_xy[i] = target->local_vel[i] - target->local_vel_now[i]; + } + acc_vel->vel_error_scalar = pow(pow(acc_vel->vel_error_xy[0], 2) + pow(acc_vel->vel_error_xy[1], 2), 0.5); + if (acc_vel->vel_error_xy[0] != 0 || acc_vel->vel_error_xy[1] != 0) { + acc_vel->vel_error_rad = atan2(acc_vel->vel_error_xy[1], acc_vel->vel_error_xy[0]); + } + + // スカラは使わず、常に最大加速度 + output->accel[0] = cos(acc_vel->vel_error_rad) * ACCEL_LIMIT; + output->accel[1] = sin(acc_vel->vel_error_rad) * ACCEL_LIMIT; + + // バック方向だけ加速度制限 + if (output->accel[0] < -(ACCEL_LIMIT_BACK)) { + output->accel[0] = -(ACCEL_LIMIT_BACK); + } + + // 減速方向は制動力2倍 + // 2倍は流石に無理があるので1.8 → やっぱ2.0 + for (int i = 0; i < 2; i++) { + if (target->local_vel_now[i] * output->accel[i] <= 0) { + output->accel[i] *= 2.0; + } + + // 目標座標を追い越した場合、加速度を2倍にして現実の位置に追従 + // 現在座標も速度制御されたタイヤで見ているので、あまりｱﾃにならない + // これ使える気がする + /*if ((omni->robot_pos_diff[i] > 0 && output->accel[i] > 0) || (omni->robot_pos_diff[i] < 0 && output->accel[i] < 0)) { + //output->accel[i] *= 1.5; + }*/ + } +} + +inline void speed_control(accel_vector_t * acc_vel, output_t * output, target_t * target, imu_t * imu, omni_t * omni) +{ + // 目標速度と差が小さい場合は目標速度をそのまま代入する + // 目標速度が連続的に変化する場合に適切でないかも + if (acc_vel->vel_error_scalar <= ACCEL_LIMIT / MAIN_LOOP_CYCLE) { + target->global_vel_now[0] = (target->local_vel[0]) * cos(imu->yaw_angle_rad) - (target->local_vel[1]) * sin(imu->yaw_angle_rad); + target->global_vel_now[1] = (target->local_vel[0]) * sin(imu->yaw_angle_rad) + (target->local_vel[1]) * cos(imu->yaw_angle_rad); + + output->accel[0] = 0; + output->accel[1] = 0; + } + + // ローカル→グローバル座標系 + // ロボットが回転しても、慣性はグローバル座標系に乗るので、加速度はグローバル座標系に変換してから加算 + // accel + target->global_vel_now[0] += ((output->accel[0]) * cos(imu->yaw_angle_rad) - (output->accel[1]) * sin(imu->yaw_angle_rad)) / MAIN_LOOP_CYCLE; + target->global_vel_now[1] += ((output->accel[0]) * sin(imu->yaw_angle_rad) + (output->accel[1]) * cos(imu->yaw_angle_rad)) / MAIN_LOOP_CYCLE; + + // 次回の計算のためにローカル座標系での速度も更新 + target->local_vel_now[0] = target->global_vel_now[0] * cos(-imu->yaw_angle_rad) - target->global_vel_now[1] * sin(-imu->yaw_angle_rad); + target->local_vel_now[1] = target->global_vel_now[0] * sin(-imu->yaw_angle_rad) + target->global_vel_now[1] * cos(-imu->yaw_angle_rad); + + // 速度次元での位置フィードバックは不要になったので、global_posまわりは使わない + target->global_pos[0] += target->global_vel_now[0] / MAIN_LOOP_CYCLE; // speed to position + target->global_pos[1] += target->global_vel_now[1] / MAIN_LOOP_CYCLE; // speed to position + + // ここから位置制御 + for (int i = 0; i < 2; i++) { + // 応答性を稼ぐためのFF項目 + target->local_vel_ff_factor[i] = output->accel[i] * FF_ACC_OUTPUT_KP; + + // targetとodomの差分に上限をつける(吹っ飛び対策) + // 出力が上限に張り付いたら、出力制限でそれ以上の加速度は出しようがないのでそれに合わせる + float odom_diff_max = (float)OUTPUT_OUTPUT_LIMIT_ODOM_DIFF / OUTPUT_GAIN_ODOM_DIFF_KP; + if (target->global_pos[i] - omni->odom[i] > odom_diff_max) { + target->global_pos[i] = omni->odom[i] + odom_diff_max; + } else if (target->global_pos[i] - omni->odom[i] < -odom_diff_max) { + target->global_pos[i] = omni->odom[i] - odom_diff_max; + } + + omni->global_odom_diff[i] = omni->odom[i] - target->global_pos[i]; + } + + // グローバル→ローカル座標系 + omni->robot_pos_diff[0] = omni->global_odom_diff[0] * cos(-imu->yaw_angle_rad) - omni->global_odom_diff[1] * sin(-imu->yaw_angle_rad); + omni->robot_pos_diff[1] = omni->global_odom_diff[0] * sin(-imu->yaw_angle_rad) + omni->global_odom_diff[1] * cos(-imu->yaw_angle_rad); + + // 加速度と同じぐらいのoutput->velocityを出したい + output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[0]; + output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[1]; +} + +inline void output_limit(output_t * output, debug_t * debug) +{ + if (debug->acc_step_down_flag) { + debug->limited_output = 0; //スリップしてたら移動出力を0にする(仮) + } else { + debug->limited_output = OUTPUT_XY_LIMIT; + } + + float limit_gain = 0; + if (output->velocity[0] > debug->limited_output) { + limit_gain = output->velocity[0] / debug->limited_output; + output->velocity[0] = debug->limited_output; + output->velocity[1] /= limit_gain; + } else if (output->velocity[0] < -debug->limited_output) { + limit_gain = -output->velocity[0] / debug->limited_output; + output->velocity[0] = -debug->limited_output; + output->velocity[1] /= limit_gain; + } + + if (output->velocity[1] > debug->limited_output) { + limit_gain = output->velocity[1] / debug->limited_output; + output->velocity[1] = debug->limited_output; + output->velocity[0] /= limit_gain; + } else if (output->velocity[1] < -debug->limited_output) { + limit_gain = -output->velocity[1] / debug->limited_output; + output->velocity[1] = -debug->limited_output; + output->velocity[0] /= limit_gain; + } +} + +#endif /* INC_ROBOT_CONTROL_H_ */ diff --git a/include/ibis/util.h b/include/ibis/util.h new file mode 100644 index 00000000..7e56a64c --- /dev/null +++ b/include/ibis/util.h @@ -0,0 +1,62 @@ +#pragma once +/******************************************* + * util ver1.0 2015/1/11 + * This is a program made for mathematically calculation. + * + * [Dependency] + * none + * + * [Note] + * Coordinate functions will not support well in the future. + * To operate coordinate, using "utilplus" is recommended. + * + * [Author] + * Tomoki Nagatani + * + * [Change history] + * ver1.1 2015/ 3/ 4 Add converter for int,unsigned short,short + * ver1.1 2015/ 2/ 7 Add weak to sign, for utilplus1.0. + * ver1.0 2015/ 1/11 C++ include capable. + ******************************************/ + +#ifdef __cplusplus +extern "C" { +#endif +#include + +float dtor(float x); +float rtod(float x); +__attribute__ ((weak)) int sign(int val); +int fsign(float val); +float constrain(float x,float a,float b); +float area(float value,float shita,float ue); +float max(float a,float b); +float min(float a,float b); +float abs_max(float a,float b); +float abs_min(float a,float b); +float floatlimit(float mae,float val,float ato); + +int uchar4_to_int(unsigned char *value); +void int_to_uchar4(unsigned char *value,int int_value); + +unsigned short uchar2_to_ushort(unsigned char *value); +void ushort_to_uchar2(unsigned char *value,unsigned short short_value); + +float uchar4_to_float(unsigned char *value); +void float_to_uchar4(unsigned char *value,float float_value); + +//added +float deg_to_radian(float deg); +float radian_to_deg(float radian); + +// add(2) +long map(long x, long in_min, long in_max, long out_min, long out_max); +float getAngleDiff(float angle_rad1, float angle_rad2); +float normalizeAngle(float angle_rad); +uint8_t decode_SW(uint16_t sw_raw_data); +float two_to_float(uint8_t data[2]); +float two_to_int(uint8_t data[2]); + +#ifdef __cplusplus +} +#endif diff --git a/include/net/ibis_robot_packet.hpp b/include/net/ibis_robot_packet.hpp new file mode 100644 index 00000000..a6bed325 --- /dev/null +++ b/include/net/ibis_robot_packet.hpp @@ -0,0 +1,194 @@ +// Copyright (c) 2023 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_LOCAL_PLANNER_ROBOT_PACKET_HPP +#define CRANE_LOCAL_PLANNER_ROBOT_PACKET_HPP +#include + +namespace crane { +inline auto convertFloatToTwoByte(float val, float range) + -> std::pair { + uint16_t two_byte = + static_cast(32767 * static_cast(val / range) + 32767); + uint8_t byte_low, byte_high; + byte_low = two_byte & 0x00FF; + byte_high = (two_byte & 0xFF00) >> 8; + return std::make_pair(byte_high, byte_low); +} + +inline auto convertTwoByteToFloat(uint8_t byte_high, uint8_t byte_low, float range) + -> float { + uint16_t two_byte = (byte_high << 8) | byte_low; + float val = static_cast(two_byte - 32767) / 32767 * range; + return val; +} + +struct RobotCommandSerialized; + +struct RobotCommand { + uint8_t HEADER; + uint8_t CHECK; + float VEL_LOCAL_SURGE; + float VEL_LOCAL_SWAY; + float VISION_GLOBAL_X; + float VISION_GLOBAL_Y; + float VISION_GLOBAL_THETA; + float TARGET_GLOBAL_X; + float TARGET_GLOBAL_Y; + float BALL_GLOBAL_X; + float BALL_GLOBAL_Y; + float TARGET_GLOBAL_THETA; + + bool LOCAL_FEEDBACK_ENABLE; + bool LOCAL_KEEPER_MODE_ENABLE; + bool IS_ID_VISIBLE; + bool STOP_FLAG; + bool IS_DRIBBLER_UP; + float KICK_POWER; + float DRIBBLE_POWER; + bool CHIP_ENABLE; + operator RobotCommandSerialized() const; +}; + +struct RobotCommandSerialized { + enum class Address { + // HEADER, + CHECK, + VEL_LOCAL_SURGE_HIGH, + VEL_LOCAL_SURGE_LOW, + VEL_LOCAL_SWAY_HIGH, + VEL_LOCAL_SWAY_LOW, + VISION_GLOBAL_THETA_HIGH, + VISION_GLOBAL_THETA_LOW, + TARGET_GLOBAL_THETA_HIGH, + TARGET_GLOBAL_THETA_LOW, + KICK_POWER, + DRIBBLE_POWER, + LOCAL_FLAGS, + BALL_GLOBAL_X_HIGH, + BALL_GLOBAL_X_LOW, + BALL_GLOBAL_Y_HIGH, + BALL_GLOBAL_Y_LOW, + VISION_GLOBAL_X_HIGH, + VISION_GLOBAL_X_LOW, + VISION_GLOBAL_Y_HIGH, + VISION_GLOBAL_Y_LOW, + TARGET_GLOBAL_X_HIGH, + TARGET_GLOBAL_X_LOW, + TARGET_GLOBAL_Y_HIGH, + TARGET_GLOBAL_Y_LOW, + SIZE + }; + + operator RobotCommand() const { + RobotCommand packet; + +#define FLOAT_FROM_2BYTE(name, range) \ + packet.name = convertTwoByteToFloat( \ + data[static_cast(Address::name##_HIGH)], data[static_cast(Address::name##_LOW)], range) + +#define FLOAT_FROM_1BYTE(name, range) \ + packet.name = data[static_cast(Address::name)] / 255.0f * range + + // packet.HEADER = data[static_cast(Address::HEADER)]; + packet.CHECK = data[static_cast(Address::CHECK)]; + FLOAT_FROM_2BYTE(VEL_LOCAL_SURGE, 7.0); + FLOAT_FROM_2BYTE(VEL_LOCAL_SWAY, 7.0); + FLOAT_FROM_2BYTE(VISION_GLOBAL_X, 32.767); + FLOAT_FROM_2BYTE(VISION_GLOBAL_Y, 32.767); + FLOAT_FROM_2BYTE(VISION_GLOBAL_THETA, M_PI); + FLOAT_FROM_2BYTE(TARGET_GLOBAL_X, 32.767); + FLOAT_FROM_2BYTE(TARGET_GLOBAL_Y, 32.767); + FLOAT_FROM_2BYTE(BALL_GLOBAL_X, 32.767); + FLOAT_FROM_2BYTE(BALL_GLOBAL_Y, 32.767); + FLOAT_FROM_2BYTE(TARGET_GLOBAL_THETA, M_PI); + const uint8_t kick_raw = data[static_cast(Address::KICK_POWER)]; + if (kick_raw >= 101) { + packet.CHIP_ENABLE = true; + packet.KICK_POWER = (kick_raw - 101) / 20.0f; + } else { + packet.CHIP_ENABLE = false; + packet.KICK_POWER = kick_raw / 20.0f; + } + packet.DRIBBLE_POWER = + data[static_cast(Address::DRIBBLE_POWER)] / 20.0f; + + uint8_t local_flags = data[static_cast(Address::LOCAL_FLAGS)]; + packet.IS_ID_VISIBLE = local_flags & 0x01; + packet.LOCAL_KEEPER_MODE_ENABLE = local_flags & 0x02; + packet.STOP_FLAG = local_flags & 0x04; + packet.LOCAL_FEEDBACK_ENABLE = local_flags & 0x08; + packet.IS_DRIBBLER_UP = local_flags & 0x10; + +#undef FLOAT_FROM_1BYTE +#undef FLOAT_FROM_2BYTE + + return packet; + } + + uint8_t data[static_cast(Address::SIZE)]; +}; + +inline RobotCommand::operator RobotCommandSerialized() const { + RobotCommandSerialized serialized; + +#define FLOAT_TO_2BYTE(name, range) \ + std::pair name##_two_byte = convertFloatToTwoByte(name, range); \ + serialized.data[static_cast(RobotCommandSerialized::Address::name##_HIGH)] = \ + name##_two_byte.first; \ + serialized.data[static_cast(RobotCommandSerialized::Address::name##_LOW)] = \ + name##_two_byte.second + +#define FLOAT_TO_1BYTE(name, range) \ + uint8_t name##_one_byte = static_cast(name / range * 255.0f); \ + serialized.data[static_cast(RobotCommandSerialized::Address::name)] = name##_one_byte + + // serialized.data[static_cast(RobotCommandSerialized::Address::HEADER)] = HEADER; + serialized.data[static_cast(RobotCommandSerialized::Address::CHECK)] = + CHECK; + + FLOAT_TO_2BYTE(VEL_LOCAL_SURGE, 7.0); + FLOAT_TO_2BYTE(VEL_LOCAL_SWAY, 7.0); + FLOAT_TO_2BYTE(VISION_GLOBAL_X, 32.767); + FLOAT_TO_2BYTE(VISION_GLOBAL_Y, 32.767); + FLOAT_TO_2BYTE(VISION_GLOBAL_THETA, M_PI); + FLOAT_TO_2BYTE(TARGET_GLOBAL_X, 32.767); + FLOAT_TO_2BYTE(TARGET_GLOBAL_Y, 32.767); + FLOAT_TO_2BYTE(BALL_GLOBAL_X, 32.767); + FLOAT_TO_2BYTE(BALL_GLOBAL_Y, 32.767); + FLOAT_TO_2BYTE(TARGET_GLOBAL_THETA, M_PI); + serialized + .data[static_cast(RobotCommandSerialized::Address::DRIBBLE_POWER)] = + static_cast(DRIBBLE_POWER * 20); + serialized + .data[static_cast(RobotCommandSerialized::Address::KICK_POWER)] = + [&]() -> uint8_t { + if (CHIP_ENABLE) { + return static_cast((std::round(20 * KICK_POWER) + 101)); + } else { + return static_cast(std::round(20 * KICK_POWER)); + } + }(); + + uint8_t local_flags = 0x00; + local_flags |= (IS_ID_VISIBLE << 0); + local_flags |= (LOCAL_KEEPER_MODE_ENABLE << 1); + local_flags |= (STOP_FLAG << 2); + local_flags |= (LOCAL_FEEDBACK_ENABLE << 3); + local_flags |= (IS_DRIBBLER_UP << 4); + + serialized + .data[static_cast(RobotCommandSerialized::Address::LOCAL_FLAGS)] = + local_flags; + +#undef FLOAT_TO_1BYTE +#undef FLOAT_TO_2BYTE + + return serialized; +} +} // namespace crane + +#endif //CRANE_LOCAL_PLANNER_ROBOT_PACKET_HPP diff --git a/include/net/ibis_ssl_client.h b/include/net/ibis_ssl_client.h new file mode 100644 index 00000000..43d0dbcc --- /dev/null +++ b/include/net/ibis_ssl_client.h @@ -0,0 +1,380 @@ +//======================================================================== +// This software is free: you can redistribute it and/or modify +// it under the terms of the GNU General Public License Version 3, +// as published by the Free Software Foundation. +// +// This software is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// Version 3 in the file COPYING that came with this distribution. +// If not, see . +//======================================================================== +/*! + \file ibis_ssl_client.h + \brief C++ Interface: ibis_ssl_client + \author Stefan Zickler, 2009 + \author Jan Segre, 2012 +*/ +//======================================================================== +#ifndef IBIS_SSL_CLIENT_H +#define IBIS_SSL_CLIENT_H +// #include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "robot.h" +#include "robot_control.h" +#include "ring_buffer.h" + +#include "ibis_robot_packet.hpp" + +// #include "grSim_Packet.pb.h" + +class QUdpSocket; +class QHostAddress; +class QNetworkInterface; + +class PIDController { +public: + PIDController() = default; + + void setGain(double p, double i, double d) { + kp = p; + ki = i; + kd = d; + error_prev = 0.0f; + } + + double update(double error, double dt) { + double p = kp * error; + double i = ki * (error + error_prev) * dt / 2.0f; + double d = kd * (error - error_prev) / dt; + error_prev = error; + return p + i + d; + } + +private: + double kp, ki, kd; + + double error_prev; +}; + +class RobotClient : public QObject{ + Q_OBJECT +public: + RobotClient(QObject* parent = nullptr) : QObject(parent) + { + } + + ~RobotClient() + { + if(has_setup) + { + _socket->close(); + delete _socket; + } + } + + void setup(uint8_t id) { + theta_controller.setGain(3.0, 0.0, 0.0); + _socket = new QUdpSocket(this); + _port = 50100 + id; + _net_address = QHostAddress::LocalHost; + if(not _socket->bind(QHostAddress::LocalHost, _port)) + { + std::cout << "Failed to bind to port " << _port << std::endl; + }else + { + connect(_socket, SIGNAL(readyRead()), this, SLOT(receiveAndProcess())); + } + has_setup = true; + } + + std::optional receive() { + if (_socket->hasPendingDatagrams()) { + QByteArray packet_data; + packet_data.resize(_socket->pendingDatagramSize()); + _socket->readDatagram(packet_data.data(), packet_data.size()); + + crane::RobotCommandSerialized raw; + for (int i = 0; i < packet_data.size(); ++i) { + if (i < static_cast(crane::RobotCommandSerialized::Address::SIZE)) { + raw.data[i] = packet_data[i]; + } + } + return raw; + } else { + return std::nullopt; + } + } + + double normalizeAngle(double angle_rad) + { + while (angle_rad > M_PI) { + angle_rad -= 2.0f * M_PI; + } + while (angle_rad < -M_PI) { + angle_rad += 2.0f * M_PI; + } + return angle_rad; + } + + double getAngleDiff(double angle_rad1, double angle_rad2) + { + angle_rad1 = normalizeAngle(angle_rad1); + angle_rad2 = normalizeAngle(angle_rad2); + if (abs(angle_rad1 - angle_rad2) > M_PI) { + if (angle_rad1 - angle_rad2 > 0) { + return angle_rad1 - angle_rad2 - 2.0f * M_PI; + } else { + return angle_rad1 - angle_rad2 + 2.0f * M_PI; + } + } else { + return angle_rad1 - angle_rad2; + } + } + + double getOmega(double current_theta, double target_theta, double dt) { + return -theta_controller.update(getAngleDiff(current_theta, target_theta), dt); + } + + void setRobot(Robot *robot) + { + _robot = robot; + } + +private slots: + void receiveAndProcess(){ +// std::cout << "receiveAndProcess" << std::endl; + + const double MAX_KICK_SPEED = 8.0; // m/s + while(auto packet = receive()) + { + if(_robot == nullptr) + { + std::cout << "Robot not set" << std::endl; + return; + } + + { + orion.connection.vision_update_cycle_cnt++; + if (orion.connection.vision_update_cycle_cnt > 1000) { + orion.connection.vision_update_cycle_cnt = 0; + } + } + + { // IMU update + orion.imu.yaw_angle = _robot->getDir(); + orion.imu.yaw_angle_rad = orion.imu.yaw_angle * M_PI / 180.0; + orion.imu.yaw_angle_diff_integral += orion.imu.yaw_angle - orion.imu.pre_yaw_angle; + orion.imu.pre_yaw_angle = orion.imu.yaw_angle; + orion.imu.pre_yaw_angle_rad = orion.imu.yaw_angle_rad; + } + + { // System update + } + + { // AI command update + orion.ai_cmd.local_target_speed[0] = packet->VEL_LOCAL_SURGE; + orion.ai_cmd.local_target_speed[1] = packet->VEL_LOCAL_SWAY; + + orion.ai_cmd.local_target_speed_scalar = sqrt( + pow(packet->VEL_LOCAL_SURGE, 2.) + pow(packet->VEL_LOCAL_SWAY, 2.)); + orion.ai_cmd.global_vision_theta = packet->VISION_GLOBAL_THETA; + orion.ai_cmd.target_theta = packet->TARGET_GLOBAL_THETA; + orion.ai_cmd.chip_en = packet->CHIP_ENABLE; + orion.ai_cmd.kick_power = packet->KICK_POWER; + orion.ai_cmd.dribble_power = packet->DRIBBLE_POWER; + + auto raw_packet = static_cast(*packet); + orion.ai_cmd.allow_local_flags = raw_packet.data[static_cast(crane::RobotCommandSerialized::Address::LOCAL_FLAGS)]; + + orion.integ.pre_global_target_position[0] = packet->TARGET_GLOBAL_X; + orion.integ.pre_global_target_position[1] = packet->TARGET_GLOBAL_Y; + + orion.ai_cmd.global_ball_position[0] = packet->BALL_GLOBAL_X; + orion.ai_cmd.global_ball_position[1] = packet->BALL_GLOBAL_Y; + orion.ai_cmd.global_robot_position[0] = packet->VISION_GLOBAL_X; + orion.ai_cmd.global_robot_position[1] = packet->VISION_GLOBAL_Y; + orion.ai_cmd.global_target_position[0] = packet->TARGET_GLOBAL_X; + orion.ai_cmd.global_target_position[1] = packet->TARGET_GLOBAL_Y; + + orion.ai_cmd.vision_lost_flag = !packet->IS_ID_VISIBLE; +// orion.ai_cmd.local_vision_en_flag = packet->LOCAL_VISION_EN; + orion.ai_cmd.local_vision_en_flag = false; + orion.ai_cmd.keeper_mode_en_flag = packet->LOCAL_KEEPER_MODE_ENABLE; + orion.ai_cmd.stop_request_flag = packet->STOP_FLAG; + orion.ai_cmd.dribbler_up_flag = packet->IS_DRIBBLER_UP; + + orion.integ.vision_based_position[0] = packet->VISION_GLOBAL_X; + orion.integ.vision_based_position[1] = packet->VISION_GLOBAL_Y; + } + + { // Odom update + // https://github.com/ibis-ssl/G474_Orion_main/blob/main/Core/Src/odom.c#L32 + for(int i = 0; i < 4; i++) { + auto & wheel = _robot->wheels[i]; + if (isnan(orion.motor.enc_angle[i])) { + orion.motor.enc_angle[i] = 0; + } + orion.motor.enc_angle[i] = static_cast(dJointGetAMotorAngle(wheel->motor, 0)); + orion.motor.angle_diff[i] = orion.motor.enc_angle[i] - orion.motor.pre_enc_angle[i]; + orion.motor.pre_enc_angle[i] = orion.motor.enc_angle[i]; + } + + const double omni_diameter = [&](){ + dReal radius, length; + dGeomCylinderGetParams(_robot->wheels[0]->cyl->geom, &radius, &length); + return radius * 2.0; + }(); + orion.omni.travel_distance[0] = orion.motor.angle_diff[1] * omni_diameter; + orion.omni.travel_distance[1] = orion.motor.angle_diff[2] * omni_diameter; + + // right front & left front + orion.omni.odom_raw[0] += orion.omni.travel_distance[0] * cos(orion.imu.yaw_angle_rad) + orion.omni.travel_distance[1] * sin(orion.imu.yaw_angle_rad); + orion.omni.odom_raw[1] += orion.omni.travel_distance[0] * sin(orion.imu.yaw_angle_rad) - orion.omni.travel_distance[1] * cos(orion.imu.yaw_angle_rad); + + orion.omni.pre_odom[0] = orion.omni.odom[0]; + orion.omni.pre_odom[1] = orion.omni.odom[1]; + + orion.omni.odom[0] = ((orion.omni.odom_raw[0] * cos(M_PI * 3 / 4) - orion.omni.odom_raw[1] * sin(M_PI * 3 / 4)) / 2) + (0.107 * cos(orion.imu.yaw_angle_rad) - 0.107); + orion.omni.odom[1] = ((orion.omni.odom_raw[0] * sin(M_PI * 3 / 4) + orion.omni.odom_raw[1] * cos(M_PI * 3 / 4)) / 2) + (0.107 * sin(orion.imu.yaw_angle_rad)); + + // omni->odom_speed[0] = (omni->odom[0] - omni->pre_odom[0]) * MAIN_LOOP_CYCLE; + // omni->odom_speed[1] = (omni->odom[1] - omni->pre_odom[1]) * MAIN_LOOP_CYCLE; + orion.omni.odom_speed[0] = (orion.omni.odom[0] - orion.omni.pre_odom[0]) / 0.01; + orion.omni.odom_speed[1] = (orion.omni.odom[1] - orion.omni.pre_odom[1]) / 0.01; + + // omni->local_odom_speed[0] = omni->odom_speed[0] * cos(-imu->yaw_angle_rad) - omni->odom_speed[1] * sin(-imu->yaw_angle_rad); + // omni->local_odom_speed[1] = omni->odom_speed[0] * sin(-imu->yaw_angle_rad) + omni->odom_speed[1] * cos(-imu->yaw_angle_rad); + orion.omni.local_odom_speed[0] = orion.omni.odom_speed[0] * cos(-orion.imu.yaw_angle_rad) - orion.omni.odom_speed[1] * sin(-orion.imu.yaw_angle_rad); + orion.omni.local_odom_speed[1] = orion.omni.odom_speed[0] * sin(-orion.imu.yaw_angle_rad) + orion.omni.odom_speed[1] * cos(-orion.imu.yaw_angle_rad); + + // for (int i = 0; i < 2; i++) { + // enqueue(omni->local_speed_log[i], omni->local_odom_speed[i]); + // omni->local_odom_speed_mvf[i] = sumNewestN(omni->local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; + // } + for (int i = 0; i <2 ; ++i) { + enqueue(orion.omni.local_speed_log[i], orion.omni.local_odom_speed[i]); + orion.omni.local_odom_speed_mvf[i] = sumNewestN(orion.omni.local_speed_log[i], SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE) / SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE; + } + + // local座標系で入れているodom speedを,global系に修正する + // vision座標だけ更新されているが、vision_update_cycle_cntが0になっていない場合に、1cycleだけpositionが飛ぶ + float latency_cycle = orion.ai_cmd.latency_time_ms / (1000 / MAIN_LOOP_CYCLE); + for (int i = 0; i < 2; i++) { + enqueue(orion.integ.odom_log[i], orion.omni.odom_speed[i]); + // メモ：connection.vision_update_cycle_cntは更新できていない + orion.integ.global_odom_vision_diff[i] = sumNewestN(orion.integ.odom_log[i], latency_cycle + orion.connection.vision_update_cycle_cnt) / MAIN_LOOP_CYCLE; + orion.integ.vision_based_position[i] = orion.ai_cmd.global_robot_position[i] + orion.integ.global_odom_vision_diff[i]; + orion.integ.position_diff[i] = orion.ai_cmd.global_target_position[i] - orion.integ.vision_based_position[i]; + } + float target_diff[2], move_diff[2]; + for (int i = 0; i < 2; i++) { + target_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.ai_cmd.global_target_position[i]; // Visionが更新された時点での現在地とtargetの距離 + move_diff[i] = orion.ai_cmd.global_robot_position[i] - orion.integ.vision_based_position[i]; // Visionとtargetが更新されてからの移動量 + } + + orion.integ.target_dist_diff = sqrt(pow(target_diff[0], 2) + pow(target_diff[1], 2)); + orion.integ.move_dist = sqrt(pow(orion.integ.position_diff[0], 2) + pow(orion.integ.position_diff[1], 2)); + } + + dReal x,y; + _robot->getXY(x, y); +// orion.omni.robot_position[0] = x; + const double last_dt = 0.01; + double omega = getOmega( + _robot->getDir() * M_PI / 180.0, packet->TARGET_GLOBAL_THETA, last_dt); + double kick_speed = packet->KICK_POWER * MAX_KICK_SPEED; + _robot->kicker->kick(kick_speed, + packet->CHIP_ENABLE ? kick_speed : 0.0); + // TODO: これらは本来別のメインループで回さないといけない（実機では500Hz） + local_feedback(&orion.integ, &orion.imu, &orion.sys, &orion.target, &orion.ai_cmd, &orion.omni); + accel_control(&orion.acc_vel, &orion.output, &orion.target, &orion.omni); + speed_control(&orion.acc_vel, &orion.output, &orion.target, &orion.imu, &orion.omni); + output_limit(&orion.output, &orion.debug); + // 出力がやけにでかいので一回1/100にしている +// _robot->setSpeed(orion.output.velocity[0] * 0.01, orion.output.velocity[1] * 0.01,orion.output.omega); +// _robot->setSpeed(orion.target.velocity[0] * 1000, orion.target.velocity[1] * 1000, omega); + // ひとまずAIコマンドをそのまま入れている。 + _robot->setSpeed(orion.ai_cmd.local_target_speed[0], orion.ai_cmd.local_target_speed[1], omega); + _robot->kicker->setRoller(packet->DRIBBLE_POWER > 0.0); + + if(_port == 50100) + { + std::stringstream ss; + ss << "vx: " << orion.output.velocity[0] << " vy: " << orion.output.velocity[1] << " theta: " << orion.output.omega << " actual theta: " << _robot->getDir(); + std::cout << ss.str() << std::endl; + } + } + } + +public: + PIDController theta_controller; + + QUdpSocket *_socket; + QMutex mutex; + quint16 _port; + QHostAddress _net_address; + Robot * _robot = nullptr; + bool has_setup = false; + + struct OrionInternal{ + OrionInternal(){ + integ.odom_log[0] = initRingBuffer(SPEED_LOG_BUF_SIZE); + integ.odom_log[1] = initRingBuffer(SPEED_LOG_BUF_SIZE); + omni.local_speed_log[0] = initRingBuffer(SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE); + omni.local_speed_log[1] = initRingBuffer(SPEED_MOVING_AVERAGE_FILTER_BUF_SIZE); + } + integration_control_t integ; + imu_t imu; + system_t sys; + target_t target; + ai_cmd_t ai_cmd; + accel_vector_t acc_vel; + output_t output; + omni_t omni; + debug_t debug; + motor_t motor; + connection_t connection; + } orion; + + // QNetworkInterface *_net_interface; +}; + +class IbisRobotCommunicator { +public: + IbisRobotCommunicator(bool is_yellow = false) : is_yellow(is_yellow) { + for (int i = 0; i < 20; ++i) { + _clients[i].setup(i); + } + } + + bool isYellow() const { + return is_yellow; + } + + std::optional receive(uint8_t id) { + return _clients[id].receive(); + } + + double getOmega(double current_theta, double target_theta, double dt, + uint8_t id) { + return _clients[id].getOmega(current_theta, target_theta, dt); + } + + std::array _clients; + + bool is_yellow = true; +}; + +#endif diff --git a/include/sslworld.h b/include/sslworld.h index 60915835..bab1a5db 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -35,6 +35,8 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include "net/robocup_ssl_server.h" +#include "net/ibis_ssl_client.h" + #include "robot.h" #include "configwidget.h" @@ -106,6 +108,7 @@ class SSLWorld : public QObject dReal cursor_x{},cursor_y{},cursor_z{}; dReal cursor_radius{}; RoboCupSSLServer *visionServer{}; + IbisRobotCommunicator *ibisRobotCommunicator{}; QUdpSocket *commandSocket{}; QUdpSocket *blueStatusSocket{},*yellowStatusSocket{}; QUdpSocket *simControlSocket; diff --git a/src/configwidget.cpp b/src/configwidget.cpp index c88d732e..a8861d29 100644 --- a/src/configwidget.cpp +++ b/src/configwidget.cpp @@ -89,7 +89,9 @@ ConfigWidget::ConfigWidget() { ADD_VALUE(div_b_vars, Double, DivB_Goal_Height,0.160,"Goal height") ADD_ENUM(StringEnum,YellowTeam,"Parsian","Yellow Team"); + ADD_ENUM(StringEnum,YellowTeam,"ibis","Yellow Team"); END_ENUM(geo_vars,YellowTeam) + ADD_ENUM(StringEnum,BlueTeam,"ibis","Blue Team"); ADD_ENUM(StringEnum,BlueTeam,"Parsian","Blue Team"); END_ENUM(geo_vars,BlueTeam) VarListPtr camera_vars(new VarList("Cameras")); diff --git a/src/ibis/ring_buffer.c b/src/ibis/ring_buffer.c new file mode 100644 index 00000000..92940e8f --- /dev/null +++ b/src/ibis/ring_buffer.c @@ -0,0 +1,68 @@ +#include "ring_buffer.h" + + + +// リングバッファの初期化 +RingBuffer * initRingBuffer(int size) +{ + RingBuffer * rb = (RingBuffer *)malloc(sizeof(RingBuffer)); + rb->buffer = (float *)malloc(size * sizeof(float)); + rb->size = size; + rb->front = 0; + rb->rear = -1; + rb->count = 0; + return rb; +} + +// リングバッファに要素を追加 +void enqueue(RingBuffer * rb, float data) +{ + if (rb->count < rb->size) { + rb->rear = (rb->rear + 1) % rb->size; + rb->buffer[rb->rear] = data; + rb->count++; + } else { + // バッファがいっぱいの場合は古いデータを上書き + rb->rear = (rb->rear + 1) % rb->size; + rb->front = (rb->front + 1) % rb->size; + rb->buffer[rb->rear] = data; + } +} + +// リングバッファから要素を取得 +float dequeue(RingBuffer * rb) +{ + if (rb->count > 0) { + float data = rb->buffer[rb->front]; + rb->front = (rb->front + 1) % rb->size; + rb->count--; + return data; + } else { + // バッファが空の場合は0.0を返すなど、適切なエラー処理を追加できます + return 0.0; + } +} + +// リングバッファ上のデータを新しい順にn個加算した結果を取得 +float sumNewestN(RingBuffer * rb, int n) +{ + if (n <= 0 || n > rb->count) { + // 無効なnの値の場合はエラーとして0.0を返す + return 0.0; + } + + int index = rb->rear; + float sum = 0.0; + for (int i = 0; i < n; i++) { + sum += rb->buffer[index]; + index = (index - 1 + rb->size) % rb->size; + } + return sum; +} + +// リングバッファの解放 +void freeRingBuffer(RingBuffer * rb) +{ + free(rb->buffer); + free(rb); +} diff --git a/src/ibis/robot_control.c b/src/ibis/robot_control.c new file mode 100644 index 00000000..37c32bb9 --- /dev/null +++ b/src/ibis/robot_control.c @@ -0,0 +1,253 @@ +/* + * robot_control.c + * + * Created on: May 19, 2024 + * Author: hiroyuki + */ + +#include "robot_control.h" + +#include "util.h" + +// 加速度パラメーター +#define ACCEL_LIMIT (5.0) // m/ss +#define ACCEL_LIMIT_BACK (3.0) // m/ss + +// 速度制御の位置に対するフィードバックゲイン +// ~ m/s / m : -250 -> 4cm : 1m/s +#define OUTPUT_GAIN_ODOM_DIFF_KP (150) +// 上記の出力制限 +#define OUTPUT_OUTPUT_LIMIT_ODOM_DIFF (20) // + +// 0.3はややデカすぎ、0.2は割といい感じ +// accel x KP +#define FF_ACC_OUTPUT_KP (0.2) + +// radに対するゲインなので値がデカい +#define OMEGA_GAIN_KP (160.0) +#define OMEGA_GAIN_KD (4000.0) + +// ドライバ側は 50 rps 制限 +// omegaぶんは考慮しない +#define OUTPUT_XY_LIMIT (40.0) // +// omegaぶんの制限 +#define OUTPUT_OMEGA_LIMIT (20.0) // ~ rad/s + +void theta_control(float target_theta, accel_vector_t * acc_vel, output_t * output, imu_t * imu) +{ + // PID + output->omega = (getAngleDiff(target_theta, imu->yaw_angle_rad) * OMEGA_GAIN_KP) - (getAngleDiff(imu->yaw_angle_rad, imu->pre_yaw_angle_rad) * OMEGA_GAIN_KD); + + if (output->omega > OUTPUT_OMEGA_LIMIT) { + output->omega = OUTPUT_OMEGA_LIMIT; + } + if (output->omega < -OUTPUT_OMEGA_LIMIT) { + output->omega = -OUTPUT_OMEGA_LIMIT; + } + //output->omega = 0; +} + +void local_feedback(integration_control_t * integ, imu_t * imu, system_t * sys, target_t * target, ai_cmd_t * ai_cmd, omni_t * omni) +{ + const float CMB_CTRL_FACTOR_LIMIT = (3.0); // [m/s] + //const float CMB_CTRL_DIFF_DEAD_ZONE = (0.03); // [m] + const float CMB_CTRL_GAIN = (10.0); + const float CMB_CTRL_DIFF_LIMIT = (CMB_CTRL_FACTOR_LIMIT / CMB_CTRL_GAIN); + + if (sys->main_mode == MAIN_MODE_COMBINATION_CONTROL) { + } else { + for (int i = 0; i < 2; i++) { + integ->vision_based_position[i] = omni->odom[i]; + integ->position_diff[i] = ai_cmd->local_target_speed[i] / 10 - integ->vision_based_position[i]; + } + } + + // グローバル→ローカル座標系 + integ->local_target_diff[0] = integ->position_diff[0] * cos(-imu->yaw_angle_rad) - integ->position_diff[1] * sin(-imu->yaw_angle_rad); + integ->local_target_diff[1] = integ->position_diff[0] * sin(-imu->yaw_angle_rad) + integ->position_diff[1] * cos(-imu->yaw_angle_rad); + + for (int i = 0; i < 2; i++) { + // デバッグ用にomni->odomをそのままと、target_posにspeed使う + // 速度制御はodomベースなのでちょっとおかしなことになる + //integ->local_target_diff[i] = omni->odom[i] - ai_cmd->local_target_speed[i]; + + // 精密性はそれほどいらないので、振動対策に不感帯入れる + // ただの不感帯よりも、スレッショルドとか入れたほうが良いかも + /*if (integ->local_target_diff[i] < CMB_CTRL_DIFF_DEAD_ZONE && integ->local_target_diff[i] > -CMB_CTRL_DIFF_DEAD_ZONE) { + integ->local_target_diff[i] = 0; + }*/ + + // ゲインは x10 + // 吹き飛び対策で+-3.0 m/sを上限にする + if (integ->local_target_diff[i] < -CMB_CTRL_DIFF_LIMIT) { + integ->local_target_diff[i] = -CMB_CTRL_DIFF_LIMIT; + } else if (integ->local_target_diff[i] > CMB_CTRL_DIFF_LIMIT) { + integ->local_target_diff[i] = CMB_CTRL_DIFF_LIMIT; + } + + if (sys->main_mode == MAIN_MODE_COMBINATION_CONTROL) { + // 位置フィードバック項目のx10はゲイン (ベタ打ち) + /*if (ai_cmd->local_target_speed[i] * integ->local_target_diff[i] < 0) { // 位置フィードバック項が制動方向の場合 + target->velocity[i] = ai_cmd->local_target_speed[i] + (integ->local_target_diff[i] * CMB_CTRL_GAIN); //ローカル統合制御あり + } else { + target->velocity[i] = ai_cmd->local_target_speed[i]; // ローカル統合制御なし + }*/ + //target->velocity[i] = (integ->local_target_diff[i] * CMB_CTRL_GAIN); //ローカル統合制御あり + if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { + target->velocity[i] = 0; + // + } else { + target->velocity[i] = integ->local_target_diff[i] * CMB_CTRL_GAIN; + + // 指令値を速度制限として適用 + if (target->velocity[i] > fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = fabs(ai_cmd->local_target_speed[i]); + } else if (target->velocity[i] < -fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = -fabs(ai_cmd->local_target_speed[i]); + } + } + } else { + // 2 x acc x X = V^2 + // acc : ACCEL_LIMIT_BACK * 2 + // ピッタリだとたまにオーバーシュートしてしまうので0.8かける + // 動いてるけど不感帯と相性悪いっぽい。スレッショルドかなにか必要かも + /*if (fabs(2 * ACCEL_LIMIT_BACK * 2 * 1.0 * integ->local_target_diff[i]) < target->local_vel_now[i] * target->local_vel_now[i] || integ->local_target_diff[i] == 0) { + target->velocity[i] = 0; + // + } else { + target->velocity[i] = integ->local_target_diff[i] * CMB_CTRL_GAIN; + + // 指令値を速度制限として適用 + if (target->velocity[i] > fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = fabs(ai_cmd->local_target_speed[i]); + } else if (target->velocity[i] < -fabs(ai_cmd->local_target_speed[i])) { + target->velocity[i] = -fabs(ai_cmd->local_target_speed[i]); + } + }*/ + target->velocity[i] = ai_cmd->local_target_speed[i]; + } + } +} + +void accel_control(accel_vector_t * acc_vel, output_t * output, target_t * target, omni_t * omni) +{ + target->local_vel[0] = target->velocity[0]; + target->local_vel[1] = target->velocity[1]; + + // XY -> rad/scalarに変換 + // 座標次元でのみフィードバックを行う。速度次元ではフィードバックを行わない。 + // ここでは目標加速度のみ決定するため、内部の目標速度のみを使用する。 + for (int i = 0; i < 2; i++) { + acc_vel->vel_error_xy[i] = target->local_vel[i] - target->local_vel_now[i]; + } + acc_vel->vel_error_scalar = pow(pow(acc_vel->vel_error_xy[0], 2) + pow(acc_vel->vel_error_xy[1], 2), 0.5); + if (acc_vel->vel_error_xy[0] != 0 || acc_vel->vel_error_xy[1] != 0) { + acc_vel->vel_error_rad = atan2(acc_vel->vel_error_xy[1], acc_vel->vel_error_xy[0]); + } + + // スカラは使わず、常に最大加速度 + output->accel[0] = cos(acc_vel->vel_error_rad) * ACCEL_LIMIT; + output->accel[1] = sin(acc_vel->vel_error_rad) * ACCEL_LIMIT; + + // バック方向だけ加速度制限 + if (output->accel[0] < -(ACCEL_LIMIT_BACK)) { + output->accel[0] = -(ACCEL_LIMIT_BACK); + } + + // 減速方向は制動力2倍 + // 2倍は流石に無理があるので1.8 → やっぱ2.0 + for (int i = 0; i < 2; i++) { + if (target->local_vel_now[i] * output->accel[i] <= 0) { + output->accel[i] *= 2.0; + } + + // 目標座標を追い越した場合、加速度を2倍にして現実の位置に追従 + // 現在座標も速度制御されたタイヤで見ているので、あまりｱﾃにならない + // これ使える気がする + /*if ((omni->robot_pos_diff[i] > 0 && output->accel[i] > 0) || (omni->robot_pos_diff[i] < 0 && output->accel[i] < 0)) { + //output->accel[i] *= 1.5; + }*/ + } +} + +void speed_control(accel_vector_t * acc_vel, output_t * output, target_t * target, imu_t * imu, omni_t * omni) +{ + // 目標速度と差が小さい場合は目標速度をそのまま代入する + // 目標速度が連続的に変化する場合に適切でないかも + if (acc_vel->vel_error_scalar <= ACCEL_LIMIT / MAIN_LOOP_CYCLE) { + target->global_vel_now[0] = (target->local_vel[0]) * cos(imu->yaw_angle_rad) - (target->local_vel[1]) * sin(imu->yaw_angle_rad); + target->global_vel_now[1] = (target->local_vel[0]) * sin(imu->yaw_angle_rad) + (target->local_vel[1]) * cos(imu->yaw_angle_rad); + + output->accel[0] = 0; + output->accel[1] = 0; + } + + // ローカル→グローバル座標系 + // ロボットが回転しても、慣性はグローバル座標系に乗るので、加速度はグローバル座標系に変換してから加算 + // accel + target->global_vel_now[0] += ((output->accel[0]) * cos(imu->yaw_angle_rad) - (output->accel[1]) * sin(imu->yaw_angle_rad)) / MAIN_LOOP_CYCLE; + target->global_vel_now[1] += ((output->accel[0]) * sin(imu->yaw_angle_rad) + (output->accel[1]) * cos(imu->yaw_angle_rad)) / MAIN_LOOP_CYCLE; + + // 次回の計算のためにローカル座標系での速度も更新 + target->local_vel_now[0] = target->global_vel_now[0] * cos(-imu->yaw_angle_rad) - target->global_vel_now[1] * sin(-imu->yaw_angle_rad); + target->local_vel_now[1] = target->global_vel_now[0] * sin(-imu->yaw_angle_rad) + target->global_vel_now[1] * cos(-imu->yaw_angle_rad); + + // 速度次元での位置フィードバックは不要になったので、global_posまわりは使わない + target->global_pos[0] += target->global_vel_now[0] / MAIN_LOOP_CYCLE; // speed to position + target->global_pos[1] += target->global_vel_now[1] / MAIN_LOOP_CYCLE; // speed to position + + // ここから位置制御 + for (int i = 0; i < 2; i++) { + // 応答性を稼ぐためのFF項目 + target->local_vel_ff_factor[i] = output->accel[i] * FF_ACC_OUTPUT_KP; + + // targetとodomの差分に上限をつける(吹っ飛び対策) + // 出力が上限に張り付いたら、出力制限でそれ以上の加速度は出しようがないのでそれに合わせる + float odom_diff_max = (float)OUTPUT_OUTPUT_LIMIT_ODOM_DIFF / OUTPUT_GAIN_ODOM_DIFF_KP; + if (target->global_pos[i] - omni->odom[i] > odom_diff_max) { + target->global_pos[i] = omni->odom[i] + odom_diff_max; + } else if (target->global_pos[i] - omni->odom[i] < -odom_diff_max) { + target->global_pos[i] = omni->odom[i] - odom_diff_max; + } + + omni->global_odom_diff[i] = omni->odom[i] - target->global_pos[i]; + } + + // グローバル→ローカル座標系 + omni->robot_pos_diff[0] = omni->global_odom_diff[0] * cos(-imu->yaw_angle_rad) - omni->global_odom_diff[1] * sin(-imu->yaw_angle_rad); + omni->robot_pos_diff[1] = omni->global_odom_diff[0] * sin(-imu->yaw_angle_rad) + omni->global_odom_diff[1] * cos(-imu->yaw_angle_rad); + + // 加速度と同じぐらいのoutput->velocityを出したい + output->velocity[0] = -omni->robot_pos_diff[0] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[0]; + output->velocity[1] = -omni->robot_pos_diff[1] * OUTPUT_GAIN_ODOM_DIFF_KP + target->local_vel_ff_factor[1]; +} + +void output_limit(output_t * output, debug_t * debug) +{ + if (debug->acc_step_down_flag) { + debug->limited_output = 0; //スリップしてたら移動出力を0にする(仮) + } else { + debug->limited_output = OUTPUT_XY_LIMIT; + } + + float limit_gain = 0; + if (output->velocity[0] > debug->limited_output) { + limit_gain = output->velocity[0] / debug->limited_output; + output->velocity[0] = debug->limited_output; + output->velocity[1] /= limit_gain; + } else if (output->velocity[0] < -debug->limited_output) { + limit_gain = -output->velocity[0] / debug->limited_output; + output->velocity[0] = -debug->limited_output; + output->velocity[1] /= limit_gain; + } + + if (output->velocity[1] > debug->limited_output) { + limit_gain = output->velocity[1] / debug->limited_output; + output->velocity[1] = debug->limited_output; + output->velocity[0] /= limit_gain; + } else if (output->velocity[1] < -debug->limited_output) { + limit_gain = -output->velocity[1] / debug->limited_output; + output->velocity[1] = -debug->limited_output; + output->velocity[0] /= limit_gain; + } +} diff --git a/src/ibis/util.c b/src/ibis/util.c new file mode 100644 index 00000000..b953d10d --- /dev/null +++ b/src/ibis/util.c @@ -0,0 +1,220 @@ +#include "util.h" + +#include +#include +#include + + +float dtor(float x) +{ + return (2 * M_PI * x / 360); //x/360*2*PI +} + +float rtod(float x) +{ + return (x * 360 / (2 * M_PI)); //x/(2*PI)*360 +} + +int sign(int val) +{ + if (val >= 0) + return 1; + else + return -1; +} + +int fsign(float val) +{ + if (val >= 0.0) + return 1; + else + return -1; +} + +float floatlimit(float max, float val, float min) +{ + if (min >= val) return min; + if (max <= val) return max; + return val; +} + +float constrain(float x, float a, float b) +{ + if (x < a) + return a; + else if (b < x) + return b; + else + return x; +} + +float area(float value, float shita, float ue) +{ + int i = 0, limit = 100; + if (shita >= ue) return 0; + for (i = 0; value < shita && i < limit; i++) { + value += (ue - shita); + } + for (i = 0; ue < value && i < limit; i++) { + value -= (ue - shita); + } + return value; +} + +float max(float a, float b) +{ + if (a > b) + return a; + else + return b; +} + +float min(float a, float b) +{ + if (a < b) + return a; + else + return b; +} + +float abs_max(float a, float b) +{ + if (fabs(a) > fabs(b)) + return a; + else + return b; +} + +float abs_min(float a, float b) +{ + if (fabs(a) < fabs(b)) + return a; + else + return b; +} + +typedef union { + int int_value; + unsigned char char4_value[4]; +} Int_char4; + +int uchar4_to_int(unsigned char * value) +{ + Int_char4 tmp; + tmp.char4_value[0] = value[0]; + tmp.char4_value[1] = value[1]; + tmp.char4_value[2] = value[2]; + tmp.char4_value[3] = value[3]; + return tmp.int_value; +} + +void int_to_uchar4(unsigned char * value, int int_value) +{ + Int_char4 tmp; + tmp.int_value = int_value; + value[0] = tmp.char4_value[0]; + value[1] = tmp.char4_value[1]; + value[2] = tmp.char4_value[2]; + value[3] = tmp.char4_value[3]; +} + +typedef union { + unsigned short short_value; + unsigned char char2_value[2]; +} UShort_char2; + +unsigned short uchar2_to_ushort(unsigned char * value) +{ + UShort_char2 tmp; + tmp.char2_value[0] = value[0]; + tmp.char2_value[1] = value[1]; + return tmp.short_value; +} + +void ushort_to_uchar2(unsigned char * value, unsigned short short_value) +{ + UShort_char2 tmp; + tmp.short_value = short_value; + value[0] = tmp.char2_value[0]; + value[1] = tmp.char2_value[1]; +} + +typedef union { + float float_value; + unsigned char char4_value[4]; +} Float_char4; + +float uchar4_to_float(unsigned char * value) +{ + Float_char4 tmp; + tmp.char4_value[0] = value[0]; + tmp.char4_value[1] = value[1]; + tmp.char4_value[2] = value[2]; + tmp.char4_value[3] = value[3]; + return tmp.float_value; +} + +void float_to_uchar4(unsigned char * value, float float_value) +{ + Float_char4 tmp; + tmp.float_value = float_value; + value[0] = tmp.char4_value[0]; + value[1] = tmp.char4_value[1]; + value[2] = tmp.char4_value[2]; + value[3] = tmp.char4_value[3]; +} + +float deg_to_radian(float deg) { return (deg / 180) * M_PI; } + +float radian_to_deg(float radian) { return (radian * 180) / M_PI; } + +long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } + +float normalizeAngle(float angle_rad) +{ + while (angle_rad > M_PI) { + angle_rad -= 2.0f * M_PI; + } + while (angle_rad < -M_PI) { + angle_rad += 2.0f * M_PI; + } + return angle_rad; +} + +float getAngleDiff(float angle_rad1, float angle_rad2) +{ + angle_rad1 = normalizeAngle(angle_rad1); + angle_rad2 = normalizeAngle(angle_rad2); + if (fabs(angle_rad1 - angle_rad2) > M_PI) { + if (angle_rad1 > angle_rad2) { + return angle_rad1 - (angle_rad2 + 2 * M_PI); + } else { + return (angle_rad1 + 2 * M_PI) - angle_rad2; + } + } else { + return angle_rad1 - angle_rad2; + } +} + +uint8_t decode_SW(uint16_t sw_raw_data) +{ + int data; + sw_raw_data = sw_raw_data & 0xFFFF; + if (sw_raw_data < 100) { + data = 0b00010000; // C + } else if (sw_raw_data < 500 && sw_raw_data > 100) { + data = 0b00000010; // B + } else if (sw_raw_data < 2000 && sw_raw_data > 500) { + data = 0b00000100; // R + } else if (sw_raw_data < 3000 && sw_raw_data > 2000) { + data = 0b00000001; // F + } else if (sw_raw_data < 4000 && sw_raw_data > 3000) { + data = 0b00001000; // L + } else { + data = 0b00000000; + } + return data; +} + +float two_to_float(uint8_t data[2]) { return (float)((data[0] << 8 | data[1]) - 32767.0) / 32767.0; } +float two_to_int(uint8_t data[2]) { return ((data[0] << 8 | data[1]) - 32767.0); } diff --git a/src/net/ibis_ssl_client.cpp b/src/net/ibis_ssl_client.cpp new file mode 100644 index 00000000..b2bbb37c --- /dev/null +++ b/src/net/ibis_ssl_client.cpp @@ -0,0 +1,4 @@ +#include "ibis_ssl_client.h" +#include "robot_control.h" + + diff --git a/src/robot.cpp b/src/robot.cpp index 252128ee..d12b73d2 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -192,7 +192,12 @@ void Robot::Kicker::holdBall(){ dReal xx = fabs((kx-bx)*vx + (ky-by)*vy); dReal yy = fabs(-(kx-bx)*vy + (ky-by)*vx); if(holdingBall || xx-rob->cfg->BallRadius() < 0) return; - dBodySetLinearVel(rob->getBall()->body,0,0,0); + if(dribble_feedback == nullptr) { + dBodySetLinearVel(rob->getBall()->body,0,0,0); + }else{ + dBodySetLinearVel(rob->getBall()->body,0,-dribble_feedback->f1[0]*200,0); + } + robot_to_ball = dJointCreateHinge(rob->getWorld()->world,0); dJointAttach (robot_to_ball,box->body,rob->getBall()->body); if(dribble_feedback == nullptr) { @@ -208,7 +213,7 @@ bool Robot::Kicker::checkDribbleFeedback() { if(dribble_feedback != nullptr){ // かかっている力の大きさを計算 double norm_force_f1 = sqrt(pow(dribble_feedback->f1[0], 2) + pow(dribble_feedback->f1[1], 2) + pow(dribble_feedback->f1[2], 2)); - if(norm_force_f1 > 0.5) { + if(norm_force_f1 > 1.0) { unholdBall(); return true; } diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 2cb626e8..3a3709a9 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -313,6 +313,15 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 } restartRequired = false; + ibisRobotCommunicator = new IbisRobotCommunicator(cfg->v_YellowTeam.get()->getString() == "ibis"); + std::cout << "ibisRobotCommunicator created" << std::endl; + for (int k = 0; k < cfg->Robots_Count(); k++) { + if (ibisRobotCommunicator->isYellow()) { + ibisRobotCommunicator->_clients[k].setRobot(robots[k + cfg->Robots_Count()]); + } else { + ibisRobotCommunicator->_clients[k].setRobot(robots[k]); + } + } elapsedLastPackageBlue.start(); elapsedLastPackageYellow.start(); @@ -739,7 +748,6 @@ void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, Simul auto teleBall = simulatorCommand.control().teleport_ball(); processTeleportBall(simulatorResponse, teleBall); } - for(const auto &teleBot : simulatorCommand.control().teleport_robot()) { int id = robotIndex(teleBot.id().id(), teleBot.id().team() == YELLOW ? 1 : 0); if (id < 0) {