碩零 - HackMD

碩零 === ## *HT-04 Firmware Migration* + ### Mbed Project #### ==*Ben Katz Original Version*== https://os.mbed.com/users/benkatz/code/HKC_MiniCheetah/ or on Github repository: https://github.com/bgkatz/3phase_integrated.git #### ==*學姊更改過的版本*== https://drive.google.com/file/d/1TEMXmkZibL7_KZf5RU2D48In_lsauoMT/view?usp=share_link + ### STM32CubeIDE Project #### ==*Download Page*== https://www.st.com/en/development-tools/stm32cubeide.html#overview #### ==*MCU(Stm32 F446RE) information*== https://www.st.com/en/evaluation-tools/nucleo-f446re.html #### ==*Ben Katz New Project*== Ben Katz new project (Migrate to CubeIDE environment) https://github.com/bgkatz/motorcontrol.git #### ==*Debug Board Firmware Code*== On Biorola NAS (Path = **/d/文件區/實驗室機器人資料_/Corgi/code/碩零任務包**) --- ## *Webots* + ### *目標：搭建一個Webots環境提供模擬與實機直接的對口。* > > 實機與模擬需要更動的部分大致有二，一個是感測器的回授，二是控制的制動器，目前這些訊息在實機上都是以類似ROS的通訊架構在傳輸的，即是說如果我有一個模擬環境，基本上只需要把感測器的輸入、以及控制器的輸出重新導向模擬器就好。 > + ### *進程：* 1. 熟悉Webot環境，下載目前實驗室使用的環境與模型用C++建立馬達角度控制器。 2. 找到目前使用的機器人檔案，整理一下各個檔案的用途，寫一個文件說明一下。 3. 把馬達的控制器架設的更完整接近實機，實機的馬達控制可以控制力矩，並且把實機有的感測器可以裝設的都裝設上去，例如imu等。 + ### *資料：* 1. [實機程式說明](https://hackmd.io/DiZOqF0NSkW6siLSguldMQ) 2. [Webot的資料](https://drive.google.com/file/d/1ywb6GKOOEhR3WKSJwaxmN3taMdOogBOU/view?usp=share_link) --- ## *進度紀錄* ### *HT-04 Firmware Migration* (2023.04.21 更新) #### 修改serial print 1. 註解 `usart.c` `line 118-121` ```C=118 /* USER CODE BEGIN 1 */ //int __io_putchar(int ch) { //HAL_UART_Transmit(&huart2, (uint8_t*)&ch, 1, 0xffff); //return 0; // ``` 2. 在 `main.c` `line 104-114` 增加 ```C=104 /* Private function prototypes -----------------------------------------------*/ #ifdef __GNUC__ #define PUTCHAR_PROTOTYPE int __io_putchar(int ch) #else #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) #endif PUTCHAR_PROTOTYPE { HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, HAL_MAX_DELAY); return ch; } ``` **** #### 接收CAN 且測試MODE轉換 ##### 設定 expression 中監測MODE轉換的變數 1. 在`main.c` `line 81`增加 ```C=104 int state_can = 0; ``` 2. 在`main.c` `line 274`增加 ```C=274 printf("state test start!\r\n "); printf("state = %d \r\n ",state_can); ``` ##### 監測MODE轉換 1. 在`stm32f4xx_it.c` `line 278` 可以看到呼叫主要CAN訊號主要辨別的函示 ```C=277 /* Check for CAN messages */ can_tx_rx(); ``` 2. 更改函式 `void can_tx_rx(void)` 如下 ```C=308 /* USER CODE BEGIN 1 */ void can_tx_rx(void){ extern int state_can; int no_mesage = HAL_CAN_GetRxMessage(&CAN_H, CAN_RX_FIFO0, &can_rx.rx_header, can_rx.data); // Read CAN if(!no_mesage){ uint32_t TxMailbox; pack_reply(&can_tx, CAN_ID, comm_encoder.angle_multiturn[0]/GR, comm_encoder.velocity/GR, controller.i_q_filt*KT*GR); // Pack response HAL_CAN_AddTxMessage(&CAN_H, &can_tx.tx_header, can_tx.data, &TxMailbox); // Send response /* Check for special Commands */ if(((can_rx.data[0]==0xFF) & (can_rx.data[1]==0xFF) & (can_rx.data[2]==0xFF) & (can_rx.data[3]==0xFF) & (can_rx.data[4]==0xFF) & (can_rx.data[5]==0xFF) & (can_rx.data[6]==0xFF) & (can_rx.data[7]==0xFC))){ update_fsm(&state, MOTOR_CMD); printf("enter MOTOR_CMD"); state_can = 1; printf("state = %d \r\n ",state_can); } else if(((can_rx.data[0]==0xFF) & (can_rx.data[1]==0xFF) & (can_rx.data[2]==0xFF) & (can_rx.data[3]==0xFF) * (can_rx.data[4]==0xFF) & (can_rx.data[5]==0xFF) & (can_rx.data[6]==0xFF) & (can_rx.data[7]==0xFD))){ update_fsm(&state, MENU_CMD); printf("enter MENU_CMD"); state_can = 2; printf("state = %d \r\n ",state_can); } else if(((can_rx.data[0]==0xFF) & (can_rx.data[1]==0xFF) & (can_rx.data[2]==0xFF) & (can_rx.data[3]==0xFF) * (can_rx.data[4]==0xFF) & (can_rx.data[5]==0xFF) & (can_rx.data[6]==0xFF) & (can_rx.data[7]==0xFE))){ update_fsm(&state, ZERO_CMD); printf("enter ZERO_CMD"); state_can = 3; printf("state = %d \r\n ",state_can); } else{ unpack_cmd(can_rx, controller.commands); // Unpack commands controller.timeout = 0; // Reset timeout counter printf("enter error"); state_can = 4; printf("state = %d \r\n ",state_can); } } } /* USER CODE END 1 */ ``` 3. 按照學姊的Mbed開始更改 `<fsm.h>` ```C=19 #define MENU_MODE 0 #define CALIBRATION_MODE 1 #define MOTOR_MODE 2 #define SETUP_MODE 4 #define ENCODER_MODE 5 #define INIT_TEMP_MODE 6 ``` 改成 ```C=19 #define MENU_MODE 0 #define HALL_CALIBRATION 1 #define CALIBRATION_MODE 5 #define MOTOR_MODE 2 #define SET_ZERO 3 #define SETUP_MODE 4 #define ENCODER_MODE 7 #define INIT_TEMP_MODE 6 #define PRINT_UART ``` #### Problems 1. 使用debug board 找不到`#include <mcp_can.h>` 2. ![](https://hackmd.io/_uploads/HyB41f7rh.png) 3. 學姊裡面的`main.cpp`的(如下圖) 在cubeide 有找到應用但找步道define在哪裡 ![](https://hackmd.io/_uploads/H1pkIMmB2.png) --- ### *Webots* (2023.04.16更新) ==開啟*Quadruped*專案== 1. 在Webot中匯入[Quadruped專案](https://drive.google.com/file/d/1ywb6GKOOEhR3WKSJwaxmN3taMdOogBOU/view?usp=share_link) 2. 開啟 `Quadruped/worlds/four leg.wbt` 檔案 3. 若地板物件方向錯誤，則在左側 `Scene Tree` 中點開 `Floor "floor"` 調整 `rotation` 為 `1 0 0 -1.57` 4. 在上方 `Tool Bar` 點擊 :arrow_forward: 按鈕，即可讓機器人向前運動。 ==取得 robot proto file== 1. 確認Webot專案能夠順利開啟後，進入 `Quadruped/worlds/` ，並用滑鼠右鍵點擊 `four leg.wbt` 以文字檔開啟。 2. 打開檔案後會在 `line:39` 至 `line:2476` 看到以下內容: ```=39 Robot { translation 0 0.2 0 children [ DEF ... ``` ```=2470 boundingObject USE BASE physics Physics { density -1 mass 10 } controller "fourleg" } ``` 3. 在 `Quadruped/protos/` 路徑下建立 `FourLegsRobot.proto`，並以文字檔開啟，再將以上所有內容複製到該檔案。 4. 由於從 `.wbt` 檔案中得到的機器人描述，並非完整的 `.proto` 檔案格式，因此需在 3. 建立的 `.proto` 檔起始處加入以下內容: ```=1 #VRML_SIM R2023a utf8 PROTO FourLegsRobot[ field SFVec3f translation 0 0.2 0 field SFRotation rotation 0 0 1 0 field SFFloat bodyMass 10 field SFString controller "fourleg" ] { Robot { ... ``` ```=2440 ... boundingObject USE BASE physics Physics { density -1 mass 10 } controller "fourleg" } ``` 5. 若先前建立的 `.proto` 檔案名稱並非 `FourLegsRobot.proto`，則需要在 `line:2` 改為 `PROTO [檔案名稱]`。 ```=2 PROTO [YourFileName][... ``` 6. 而在 `line:10` 、 `line:11` 、 `line:2444` 、 `line:2446` 需分別改成: ```=10 translation IS translation rotation IS rotation ``` ```=2444 mass IS bodyMass ``` ```=2446 controller IS controller ``` 7. 需注意的是，若將參數宣告於 `PROTO [檔案名稱]` 內 (其位置如下)，便可以在 `Scene Tree` 中依據其宣告格式調控，以改變物件狀態與行為。 ```=2 PROTO FourLegsRobot[ field SFVec3f translation 0 0.2 0 field SFRotation rotation 0 0 1 0 field SFFloat bodyMass 10 field SFString controller "fourleg" ] ``` ==匯入 robot proto file== 1. 建立完 `.proto` 檔之後，重新開啟 (或複製) `four leg.wbt` 檔案，若點擊左上方 :heavy_plus_sign: 即會看到不同node的選單；其中 `USE` 選單即為原本 `.wbt` 檔案中已寫入的機器人描述，而 `PROTO nodes (Current Project)` 選單則是根據 `Quadruped/protos/` 路徑下的 `.proto` 檔所產生的node. 2. 將選單關閉，並於左側 `Scene Tree` 將 `Robot "robot"`物件刪除，接著點擊 :heavy_plus_sign: ，在 `PROTO nodes (Current Project)` 中選取先前建立的機器人加入場景，便會看到與原本相同的機器人在場景中。 3. 在上方 `Tool Bar` 點擊 :arrow_forward: 按鈕，即可讓機器人向前運動。而此時在 `Scene Tree` 中點擊機器人會看到的參數，便會是先前在 `.proto` 檔開頭所設定之參數。 ==使用 *.cpp controller* 取代 *.m controller*== 1. 若在Webot中欲建立新的c+\+控制器，請點擊Webot視窗左上方之 `File/New/New Robot Controller...` ，並在接下來的設定中依序選擇 `c++` -> `Webots (gcc/Makefile)` -> `輸入 fourleg_cpp (或其他名稱)` ，系統便會在 `Quadruped/controllers/` 中建立一個新的資料夾，並包含控制器之主要程式檔。 2. 由於原先機器人的控制器為 `Quadruped/controllers/fourleg/` 中的 `fourleg.m`，其中使用到了 `R100_crawl_4leg.mat` 之步態資料，因此需先撰寫一個 `mat_2_csv.m` 程式，將 `.mat` 檔之資料轉換至 `.csv` 檔供c++程式讀取。其程式碼如下: ```== % Load the .mat file matFileName = 'R100_crawl_4leg.mat'; load(matFileName); % Convert the variable(s) to a table LF_phi_rad_table = table(LF_phi_rad); LH_phi_rad_table = table(LH_phi_rad); RF_phi_rad_table = table(RF_phi_rad); RH_phi_rad_table = table(RH_phi_rad); % Save the table to a .csv file csvFileName = 'LF_phi_rad.csv'; writetable(LF_phi_rad_table, csvFileName); csvFileName = 'LH_phi_rad.csv'; writetable(LH_phi_rad_table, csvFileName); csvFileName = 'RF_phi_rad.csv'; writetable(RF_phi_rad_table, csvFileName); csvFileName = 'RH_phi_rad.csv'; writetable(RH_phi_rad_table, csvFileName); ``` 3. 完成 `.mat` 檔與 `.csv` 檔的轉換後，將 `LF_phi_read.csv` 、 `LH_phi_read.csv` 、 `RF_phi_read.csv` 、 `RH_phi_read.csv` 檔案從 `Quadruped/controllers/fourleg/` 移至 `Quadruped/controllers/fourleg_cpp/` (或先前建立之c+\+控制器資料夾)。 4. 根據 `fourleg.m` 之程式碼內容，將其轉換至c+\+語法，並寫入 `fourleg_cpp.cpp` (或先前建立之c+\+程式檔)，其程式碼如下: ```cpp=1 // File: fourleg_cpp // Date: 2023/04/15 // Description: cpp version of fourleg.m // Author: b08502148 // Modifications: #include <webots/Motor.hpp> #include <webots/Robot.hpp> #include <fstream> #include <iostream> #include <sstream> #include <string> #include <vector> #define TIME_STEP 64 using namespace webots; std::vector<std::string> _csv(std::string s); int main() { // append files converted from R100_crawl_4leg.mat std::vector<std::string> filename; filename.push_back("LF_phi_rad.csv"); filename.push_back("LH_phi_rad.csv"); filename.push_back("RF_phi_rad.csv"); filename.push_back("RH_phi_rad.csv"); // store all the data from csv files std::vector<std::vector<std::vector<double>>> data; for (int i=0; i<4; i++){ std::ifstream inFile(filename[i], std::ios::in); if (!inFile) { std::cout << "Open file failed..." << std::endl; std::exit(1); } // read the title (the first line of the csv file) std::string line; std::getline(inFile, line); std::vector<std::string> title = _csv(line); // append all the values from csv files to the data data.push_back(std::vector<std::vector<double>>()); while (std::getline(inFile, line)) { std::vector<std::string> pos = _csv(line); data[i].push_back(std::vector<double>()); data[i].push_back(std::vector<double>()); data[i][0].push_back(std::stod(pos[0])); data[i][1].push_back(std::stod(pos[1])); } } // define the robot object Robot *robot = new Robot(); // add all motors to the robot Motor *lf_right_motor = robot->getMotor("lf_right_motor"); Motor *lf_left_motor = robot->getMotor("lf_left_motor"); Motor *lh_right_motor = robot->getMotor("lh_right_motor"); Motor *lh_left_motor = robot->getMotor("lh_left_motor"); Motor *rf_right_motor = robot->getMotor("rf_right_motor"); Motor *rf_left_motor = robot->getMotor("rf_left_motor"); Motor *rh_right_motor = robot->getMotor("rh_right_motor"); Motor *rh_left_motor = robot->getMotor("rh_left_motor"); // set motor positions iteratively and repeatedly to drive the robot size_t index = 0; while (robot->step(TIME_STEP) != -1) { lf_right_motor->setPosition(data[0][0][index]); lf_left_motor->setPosition(data[0][1][index]); lh_right_motor->setPosition(data[1][0][index]); lh_left_motor->setPosition(data[1][1][index]); rf_right_motor->setPosition(data[2][1][index]); rf_left_motor->setPosition(data[2][0][index]); rh_right_motor->setPosition(data[3][1][index]); rh_left_motor->setPosition(data[3][0][index]); index = index + 1; if (index > data[0][0].size()-1) index = 0; } delete robot; return 0; } // the function of reading csv files std::vector<std::string> _csv(std::string s) { std::vector<std::string> arr; std::istringstream delim(s); std::string token; int c = 0; while (std::getline(delim, token, ',')) { arr.push_back(token); c++; } return arr; } ``` 5. 完成程式後，進入 `Scene Tree` 點擊機器人物件，並將 `controller` 選取為該 `.cpp` 檔。最後，在上方 `Tool Bar` 點擊 :arrow_forward: 按鈕，即可讓機器人向前運動。 --- (2023.05.06更新) ==CMake== 基本上和一般的CMake一樣，就是找到Webot的函式庫位置就好，原生的編譯生成如果函式庫用的版本比較新有時候會比較難設定，用CMake比較方便，例如NLOPT、Eigen，或者自己寫的函示庫。 ```cmake= cmake_minimum_required(VERSION 3.0) get_filename_component(PROJECT ${CMAKE_SOURCE_DIR} NAME) project(${PROJECT}) set(CMAKE_C_COMPILER "/usr/bin/gcc") set(CMAKE_CXX_COMPILER "/usr/bin/g++") set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_FLAGS_DEBUG "-g") set(CMAKE_CXX_FLAGS_RELEASE "-O3") set (CMAKE_EXE_LINKER_FLAGS) set (CMAKE_CXX_STANDARD 17) set(CMAKE_C_FLAGS "-std=c99") set(CMAKE_CXX_FLAGS "-std=c99 -Wno-error=deprecated-declarations -Wno-deprecated-declarations ") # Get C or C++ sources in the current directory (only). file(GLOB C_SOURCES *.c) file(GLOB CPP_SOURCES *.cpp) set(SOURCES ${C_SOURCES} ${CPP_SOURCES}) #find package you need find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(NLopt REQUIRED) # Set the Webots home path (change it according to your installation method) set(WEBOTS_HOME "/Applications/Webots.app") #set(WEBOTS_HOME "/snap/webots/current/usr/share/webots") # Link with the Webots controller library. link_directories(${WEBOTS_HOME}/lib/controller) set (LIBRARIES m ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX}) include_directories(${WEBOTS_HOME}/include/controller/c ${WEBOTS_HOME}/include/controller/cpp) # Setup the target executable. add_executable(${PROJECT} ${SOURCES}) target_link_libraries(${PROJECT} ${LIBRARIES} Eigen3::Eigen) # Copy the target executable at the right location. add_custom_command(TARGET ${PROJECT} POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_BINARY_DIR}/${PROJECT} ${CMAKE_SOURCE_DIR} ) ```