---
# System prepended metadata

title: 碩零

---

碩零
===

## *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環境提供模擬與實機直接的對口。*

> <font size="1"> 
> 實機與模擬需要更動的部分大致有二，一個是感測器的回授，二是控制的制動器，目前這些訊息在實機上都是以類似ROS的通訊架構在傳輸的，即是說如果我有一個模擬環境，基本上只需要把感測器的輸入、以及控制器的輸出重新導向模擬器就好。
    
> </font>

+ ### *進程：*
<font size="1">

1. 熟悉Webot環境，下載目前實驗室使用的環境與模型用C++建立馬達角度控制器。</br>
2. 找到目前使用的機器人檔案，整理一下各個檔案的用途，寫一個文件說明一下。</br>
3. 把馬達的控制器架設的更完整接近實機，實機的馬達控制可以控制力矩，並且把實機有的感測器可以裝設的都裝設上去，例如imu等。

</font>

+ ### *資料：*

<font size="1">

1. [實機程式說明](https://hackmd.io/DiZOqF0NSkW6siLSguldMQ)
2. [Webot的資料](https://drive.google.com/file/d/1ywb6GKOOEhR3WKSJwaxmN3taMdOogBOU/view?usp=share_link)

</font>

---
## *進度紀錄*

### *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}
)
```
