# AUV Control on STM32
written by 周文荏
contact: [e-mail]

---
### Basic Concept
**1. 誤差來源:**
- 位置誤差＆速度誤差 會從 Rpi 得到
- 深度誤差會由 Bar02 得到
- 姿態誤差會由 IMU ＆ Rpi (yaw angle) 得到

**2. 控制原理:**
- 得到誤差後，由 **controller** 乘以調參係數得到 **control input**
- 再由 **control input** 透過 **allocation matrix** 得到**每個馬達的推力**完成輸出

---
### Important Datatype

#### Dynamics

Dynamics 儲存 AUV 的：
- quaternion: 四元素 (w,x,y,z)
- position: (x,y,z) 位置
- velocity: 三個方向之速度&角速度
- acceleration: 三方向之家速度&角加速度

@OPhoat6bQ06L3fD9DBb-8Q
> I suggest naming this as 'state' here instead of 'dynamics' because 'dynamics' is associated with force input.
> [name=eason27271563]

/*---- in Inc/Datatype/dynamics.h ----*/


struct Dynamics
{
    geometry::Vector position;
    Quaternion orientation;
    
    Kinematics velocity;
    Kinematics acceleration;
}

>### Notes: Other Datatypes in Dynamics
>#### 1. **geometry::Vector**
> cf: [geometry_msgs/Vector3.msg](http://docs.ros.org/en/api/geometry_msgs/html/msg/Vector3.html)
> ```cpp
> /*---- in Inc/Datatype/vector.h ----*/
>
> namespace geometry
> {
> struct Vector
> {
> float x;
> float y;
> float z;
> }
> }
> ```
>
>#### 2. Kinematics
> ```cpp!
> /*--- in Inc/Datatype/dynamics.h ---*/
>
> struct Kinematics
> {
> geometry::Vector linear;
> geometry::Vector angular;
> }
> ```
>
>#### 3. Quaternion
> ```cpp!
> /*--- in Inc/Datatype/quaternion.h ---*/
>
> struct Quaternion
> {
> float x,y,z,w;
>
> normalize();
> // operator overloading functions
> // ....(略)
> }
> ```
---
### In main.cpp

/*-- parameters --*/
float desired_depth = 0.5;
float yaw_sonar = 0; // yaw angle from sonar (from rpi)

geometry::Vector ex = {2,0,0}; // position error
geometry::Vector ev = {0};     // velocity error

// arm motor rotate angle
int arm_angle[3] = {0,0,0}; //-90 ~ 90

/*-- in main --*/
//sensors
Mpu9250 imu;
Bar02 depth_sensor;

//Dynamics
Dynamics state = {0};
Kinematics control_input = {0}; //x,y,z direction force and moment

//Controller 
Controller controller({1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}, {1.5, 1, 1.5}, {1, 1, 0}, 0);
// set Kx, Kv, KR value (tuning) and Omega(how much we trust sonar yaw angle)

// Motor Propulsion
Propulsion_Sys propulsion_sys;

// Robot arm
Robot_Arm arm;


//Sensor
imu.set(&hspi2, GPIOB, GPIO_PIN_12);
if (!depth_sensor.set(&hi2c1))
    return -1;
  
//Controller
imu.update(state);
controller.set(state.orientation);

//Output
propulsion_sys.set_timer(&htim2, &htim8);

arm.set(&htim4, arm_angle);

/*-- in while loop --*/
imu.update(state);

//depth sensor
depth_sensor.read_value();
ex.z = desired_depth - depth_sensor.depth();

//controller
controller.update(state, ex, ev, yaw_sonar, control_input);

// allocate and output
propulsion_sys.allocate(control_input);

in while loop:
- imu update 角速度 & orientation（目前姿態 in quaternion)
- depth sensor update 深度 (ex.z)
- controller update: 更新 state & 計算 control_input
- 透過 allocation matrix 將 control input 得出的推力 輸出到馬達

> #### **imu update 角速度**
> - use Madgwick filter to filter raw data (get angular velocity)
> - use gradient decent to calculate quaternion
[Madgwick filter + gradient decent code](https://github.com/NCTU-AUV/SAUVC2022_STM32/blob/db67a90fe94ebd8ff7e9ab33753e01a6a03ac308/Core/Src/Sensor/Adafruit_AHRS_Madgwick.cpp#L190)
> > Note: if imu change direction => change q_ItoE in constructor
>
> #### depth sensor update
> - update depth error (desired depth predefined)
> [more depth sensor details](https://hackmd.io/HYQ-6N19SjCVHs9AClE5Mw?view)
> #### controller update
> - Details: [geometry controller](https://hackmd.io/yPEXwDbLRkayKPhvLadm6Q?view)
> > Note: ex(位置),ev(速度), eR(姿態), eOmega(角速度) 誤差
> 類似：
> 

---

Notes：
- ex, ev, yaw from rpi

Problems:
- 目前 state 的即時位置(座標)沒有更新 (因為ex, ev從 Rpi 來）
- ev 的來源 (暫無DVL)