From e64a0e8d3e6ad31a7d79142e2d9cafa598bd437e Mon Sep 17 00:00:00 2001 From: SeirenTDoi Date: Thu, 5 Oct 2023 15:15:53 +0900 Subject: [PATCH] Add and modify Cubewheel files (#232) * Add and modify Cubewheel files * Modify indent and character code * Modify character code * add a space after // * modify for coding rule error * change unused type to used type * modify as coding rule * modify as coding rule * modify as coding rule * modify as coding rule * rename filename to devide cube and wheel * rename text in file to devide cube and wheel --- .../DriverInstances/di_cube_wheel.c | 437 ++++++++++++++ .../DriverInstances/di_cube_wheel.h | 55 ++ .../ActuatorControllers/rw_controller.c | 49 ++ src/src_user/Drivers/Aocs/cube_wheel.c | 545 ++++++++++++++++++ src/src_user/Drivers/Aocs/cube_wheel.h | 108 ++++ 5 files changed, 1194 insertions(+) create mode 100644 src/src_user/Applications/DriverInstances/di_cube_wheel.c create mode 100644 src/src_user/Applications/DriverInstances/di_cube_wheel.h create mode 100644 src/src_user/Drivers/Aocs/cube_wheel.c create mode 100644 src/src_user/Drivers/Aocs/cube_wheel.h diff --git a/src/src_user/Applications/DriverInstances/di_cube_wheel.c b/src/src_user/Applications/DriverInstances/di_cube_wheel.c new file mode 100644 index 00000000..abc272be --- /dev/null +++ b/src/src_user/Applications/DriverInstances/di_cube_wheel.c @@ -0,0 +1,437 @@ +#pragma section REPRO +/** +* @file di_cube_wheel.c +* @brief CUBE_WHEELのDriver Instance +*/ + +#include "di_cube_wheel.h" + +#include +#include +#include +#include +#include "../UserDefined/anomaly_control.h" +#include "../UserDefined/AOCS/aocs_manager.h" +#include +#include +#include "../../Library/vector.h" +#include "../../Library/c2a_math.h" + + +static CUBE_WHEEL_Driver cube_wheel_driver_[CUBE_WHEEL_IDX_MAX]; +const CUBE_WHEEL_Driver* const cube_wheel_driver[CUBE_WHEEL_IDX_MAX] = { &cube_wheel_driver_[CUBE_WHEEL_IDX_ON_X], + &cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Y], + &cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Z] }; + +static float DI_CUBE_WHEEL_rw_speed_rad_s_[CUBE_WHEEL_IDX_MAX] = { 0.0f, 0.0f, 0.0f }; +static uint8_t DI_CUBE_WHEEL_is_initialized_[3] = { 0, 0, 0 }; //!< 0 = not initialized, 1 = initialized +static uint8_t DI_CUBE_WHEEL_is_enabled_[3] = { 0, 0, 0 }; //!< 0 = not initialized, 1 = initialized + +static void DI_CUBE_WHEEL_init_(void); +static void DI_CUBE_WHEEL_update_(void); + +/** + * @brief AOCS MANAGERに方向行列をセットする + * @return AOCS_MANAGER_ERRORに準拠 + */ +static AOCS_MANAGER_ERROR DI_CUBE_WHEEL_set_direction_matrix_to_aocs_manager_(void); + +AppInfo DI_CUBE_WHEEL_update(void) +{ + return create_app_info("update_DI_CUBE_WHEEL", DI_CUBE_WHEEL_init_, DI_CUBE_WHEEL_update_); +} + + +static void DI_CUBE_WHEEL_init_(void) +{ + CUBE_WHEEL_init(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_X], PORT_CH_I2C_2, I2C_DEVICE_ADDR_RW_X, PORT_CH_GPIO_OUT_RWX_EN, RW_AXIS_ID_X); + CUBE_WHEEL_init(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Y], PORT_CH_I2C_2, I2C_DEVICE_ADDR_RW_Y, PORT_CH_GPIO_OUT_RWY_EN, RW_AXIS_ID_Y); + CUBE_WHEEL_init(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Z], PORT_CH_I2C_2, I2C_DEVICE_ADDR_RW_Z, PORT_CH_GPIO_OUT_RWZ_EN, RW_AXIS_ID_Z); + + // 回転方向設定 + float direction_x[] = { -1.0f, 0.0f, 0.0f }; + CUBE_WHEEL_set_rotation_direction_b(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_X], direction_x); + float direction_y[] = { 0.0f, -1.0f, 0.0f }; + CUBE_WHEEL_set_rotation_direction_b(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Y], direction_y); + float direction_z[] = { 0.0f, 0.0f, -1.0f }; + CUBE_WHEEL_set_rotation_direction_b(&cube_wheel_driver_[CUBE_WHEEL_IDX_ON_Z], direction_z); + + AOCS_MANAGER_ERROR ret_manager = DI_CUBE_WHEEL_set_direction_matrix_to_aocs_manager_(); + if (ret_manager != AOCS_MANAGER_ERROR_OK) Printf("RW direction setting Failed ! %d \n", ret_manager); +} + + +static void DI_CUBE_WHEEL_update_(void) +{ + uint8_t i; + + // 電源OFFの場合、角速度は0に設定する + if (power_switch_control->switch_state_unreg[APP_PSC_UNREG_IDX_RW] == APP_PSC_STATE_OFF) + { + for (i = 0; i < CUBE_WHEEL_IDX_MAX; i++) + { + DI_CUBE_WHEEL_is_initialized_[i] = 0; + DI_CUBE_WHEEL_rw_speed_rad_s_[i] = 0.0f; + } + return; + } + + for (i = 0; i < CUBE_WHEEL_IDX_MAX; i++) + { + if (cube_wheel_driver[i]->info.valid) + { + uint8_t last_result; + + if (cube_wheel_driver[i]->info.backup_mode_state == 1) + { + last_result = CUBE_WHEEL_GetWheelDuty(&cube_wheel_driver_[i]); + DI_CUBE_WHEEL_rw_speed_rad_s_[i] = PHYSICAL_CONST_rpm_to_rad_sec(cube_wheel_driver_[i].info.speed_backup_in_rpm); + } + else + { + last_result = CUBE_WHEEL_GetWheelData(&cube_wheel_driver_[i]); + DI_CUBE_WHEEL_rw_speed_rad_s_[i] = PHYSICAL_CONST_rpm_to_rad_sec(cube_wheel_driver_[i].info.speed_in_rpm); + } + AOCS_MANAGER_set_rw_angular_velocity_rad_s(DI_CUBE_WHEEL_rw_speed_rad_s_); + + // アノマリ処理 + APP_ANOMALY_IDX idx = (APP_ANOMALY_IDX)(APP_ANOMALY_IDX_COMM_RWX + i); + if (APP_ANOMALY_SET_LAST_RESULT(idx, (DS_CMD_ERR_CODE)last_result) == APP_ANOMALY_STATE_TO_ON) + { + // Printf("to initial by RW(%d) comm error.\n", i);App内のPrintは避ける + APP_ANOMALY_TRANSITION_TO_INITIAL(); + } + } + } +} + +static AOCS_MANAGER_ERROR DI_CUBE_WHEEL_set_direction_matrix_to_aocs_manager_(void) +{ + float rw_rotation_direction_matrix[AOCS_MANAGER_NUM_OF_RW][PHYSICAL_CONST_THREE_DIM]; + for (int idx = 0; idx < AOCS_MANAGER_NUM_OF_RW; idx++) + { + VECTOR_copy(rw_rotation_direction_matrix[idx], cube_wheel_driver_[idx].info.rotation_direction_b); + } + AOCS_MANAGER_ERROR ret = AOCS_MANAGER_set_rw_rotation_direction_matrix(rw_rotation_direction_matrix); + return ret; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_Enable(const CommonCmdPacket* packet) +{ + DS_CMD_ERR_CODE ret; + + // TODO: 処理を書く + for (int idx = 0; idx < CUBE_WHEEL_IDX_MAX; idx++) + { + ret = CUBE_WHEEL_Enable(&cube_wheel_driver_[idx]); + if (ret == DS_CMD_OK) + { + DI_CUBE_WHEEL_is_enabled_[idx] = 1; + } + } + + return DS_conv_cmd_err_to_ccp_exec_sts(ret); +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_Init(const CommonCmdPacket* packet) +{ + DS_CMD_ERR_CODE ret; + + // TODO: 処理を書く + for (int idx = 0; idx < CUBE_WHEEL_IDX_MAX; idx++) + { + if (DI_CUBE_WHEEL_is_enabled_[idx] == 1) + { + ret = CUBE_WHEEL_Startup(&cube_wheel_driver_[idx]); + if (ret == DS_CMD_OK) + { + DI_CUBE_WHEEL_is_initialized_[idx] = 1; + } + } + } + + return DS_conv_cmd_err_to_ccp_exec_sts(ret); +} + + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EndProcess(const CommonCmdPacket* packet) +{ + DS_CMD_ERR_CODE ret; + + // TODO: 処理を書く + for (int idx = 0; idx < CUBE_WHEEL_IDX_MAX; idx++) + { + ret = CUBE_WHEEL_Disable(&cube_wheel_driver_[idx]); + DI_CUBE_WHEEL_is_enabled_[idx] = 0; + } + + return DS_conv_cmd_err_to_ccp_exec_sts(ret); +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetSpeed(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + uint8_t raw_val[4] = { 0, 0, 0, 0 }; + float f_speed = 0.0f; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + raw_val[0] = param[1]; + raw_val[1] = param[2]; + + int16_t int16_val = 0; + *(((unsigned char*)&int16_val) + 0) = raw_val[1]; + *(((unsigned char*)&int16_val) + 1) = raw_val[0]; + + int16_t speed = int16_val; + + CUBE_WHEEL_SetReferenceSpeed(&cube_wheel_driver_[cube_wheel_idx], speed); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; + +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetDutyCycle(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + uint8_t raw_val[4] = { 0, 0, 0, 0 }; + float f_duty = 0.0f; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + raw_val[0] = param[1]; + raw_val[1] = param[2]; + + int16_t int16_val = 0; + *(((unsigned char*)&int16_val) + 0) = raw_val[1]; + *(((unsigned char*)&int16_val) + 1) = raw_val[0]; + + int16_t duty = int16_val; + + CUBE_WHEEL_SetDutyCycle(&cube_wheel_driver_[cube_wheel_idx], duty); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetControlMode(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + uint8_t control_mode = param[1]; + + CUBE_WHEEL_SetControlMode(&cube_wheel_driver_[cube_wheel_idx], control_mode); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetPwmGain(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + int16_t Ki = (int16_t)(param[1] << 8 | param[2]); + uint8_t KiMultiplier = param[3]; + + CUBE_WHEEL_SetPwmGain(&cube_wheel_driver_[cube_wheel_idx], Ki, KiMultiplier); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetMainGain(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + uint16_t Ki = (uint16_t)(param[1] << 8 | param[2]); + uint8_t KiMultiplier = param[3]; + uint16_t Kd = (uint16_t)(param[4] << 8 | param[5]); + uint8_t KdMultiplier = param[6]; + + CUBE_WHEEL_SetMainGain(&cube_wheel_driver_[cube_wheel_idx], Ki, KiMultiplier, Kd, KdMultiplier); + + + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_GetStatus(const CommonCmdPacket* packet) +{ + DS_CMD_ERR_CODE ret; + + // TODO: 処理を書く + for (int idx = 0; idx < CUBE_WHEEL_IDX_MAX; idx++) + { + ret = CUBE_WHEEL_GetStatus(&cube_wheel_driver_[idx]); + } + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetBackupmode(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + uint8_t enable_status = 0; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + enable_status = param[1]; + + CUBE_WHEEL_SetBackupmode(&cube_wheel_driver_[cube_wheel_idx], enable_status); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EnableEncoder(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + uint8_t enable_status = 0; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + enable_status = param[1]; + + CUBE_WHEEL_EnableEncoder(&cube_wheel_driver_[cube_wheel_idx], enable_status); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EnableHallsensor(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX cube_wheel_idx; + uint8_t enable_status = 0; + + cube_wheel_idx = (CUBE_WHEEL_IDX)param[0]; + if (cube_wheel_idx > CUBE_WHEEL_IDX_MAX) + { + return CCP_EXEC_ILLEGAL_PARAMETER; + } + + enable_status = param[1]; + + CUBE_WHEEL_EnableHallsensor(&cube_wheel_driver_[cube_wheel_idx], enable_status); + + // return DS_conv_cmd_err_to_ccp_exec_sts(ret); + return CCP_EXEC_SUCCESS; +} + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SET_ROTATION_DIRECTION_VECTOR(const CommonCmdPacket* packet) +{ + const uint8_t* param = CCP_get_param_head(packet); + CUBE_WHEEL_IDX idx = (CUBE_WHEEL_IDX)param[0]; + if (idx >= CUBE_WHEEL_IDX_MAX) return CCP_EXEC_ILLEGAL_PARAMETER; + + float rotation_direction_b[PHYSICAL_CONST_THREE_DIM]; + for (int body_axis = 0; body_axis < PHYSICAL_CONST_THREE_DIM; body_axis++) + { + endian_memcpy(&rotation_direction_b[body_axis], param + 1 + body_axis * sizeof(float), sizeof(float)); + } + + C2A_MATH_ERROR ret_math = CUBE_WHEEL_set_rotation_direction_b(&cube_wheel_driver_[idx], rotation_direction_b); + if (ret_math != C2A_MATH_ERROR_OK) return CCP_EXEC_ILLEGAL_PARAMETER; + + AOCS_MANAGER_ERROR ret_manager = DI_CUBE_WHEEL_set_direction_matrix_to_aocs_manager_(); + if (ret_manager != AOCS_MANAGER_ERROR_OK) return CCP_EXEC_ILLEGAL_CONTEXT; + + return CCP_EXEC_SUCCESS; +} + +int DI_CUBE_WHEEL_set_torque_Nm(CUBE_WHEEL_IDX idx, float torque_rw_Nm) +{ + if (idx >= CUBE_WHEEL_IDX_MAX) return AOCS_ERROR_RANGE_OVER; + if (DI_CUBE_WHEEL_is_initialized_[idx] != 1) return AOCS_ERROR_OTHERS; + + float f_duty = 0.0; + int duty = 0; + CUBE_WHEEL_IDX cube_wheel_idx = idx; + + float max_torque_rw_Nm = (float)(cube_wheel_driver_[cube_wheel_idx].info.max_torque / 1000.0); + f_duty = (float)(torque_rw_Nm / max_torque_rw_Nm); + + duty = (int)(f_duty * 100); + + // set max/min limit to duty value + if (duty > 100) + { + duty = 100; + } + else if (duty < -100) + { + duty = -100; + } + + CUBE_WHEEL_SetDutyCycle(&cube_wheel_driver_[cube_wheel_idx], duty); + return 0; +} + +int DI_CUBE_WHEEL_set_speed_rpm(CUBE_WHEEL_IDX idx, float speed_rw_rpm) +{ + if (idx >= CUBE_WHEEL_IDX_MAX) return AOCS_ERROR_RANGE_OVER; + if (DI_CUBE_WHEEL_is_initialized_[idx] != 1) return AOCS_ERROR_OTHERS; + + // speed制限 + float max_rpm = 8000; + float min_rpm = -max_rpm; + if (max_rpm < speed_rw_rpm) + { + speed_rw_rpm = max_rpm; + } + else if (speed_rw_rpm < min_rpm) + { + speed_rw_rpm = min_rpm; + } + + int16_t speed_int = (int16_t)speed_rw_rpm; + + CUBE_WHEEL_SetReferenceSpeed(&cube_wheel_driver_[idx], speed_int); + return 0; +} + +#pragma section diff --git a/src/src_user/Applications/DriverInstances/di_cube_wheel.h b/src/src_user/Applications/DriverInstances/di_cube_wheel.h new file mode 100644 index 00000000..c11f9f86 --- /dev/null +++ b/src/src_user/Applications/DriverInstances/di_cube_wheel.h @@ -0,0 +1,55 @@ +/** +* @file di_cube_wheel.h +* @brief CUBE_WHEELのDriver Instance +*/ +#ifndef DI_CUBE_WHEEL_H_ +#define DI_CUBE_WHEEL_H_ + +#include "../../Drivers/ADCS/cube_wheel.h" + +#include +#include +#include +#include "../UserDefined/AOCS/aocs_error.h" + +typedef enum +{ + CUBE_WHEEL_IDX_ON_X = 0, + CUBE_WHEEL_IDX_ON_Y, + CUBE_WHEEL_IDX_ON_Z, + CUBE_WHEEL_IDX_MAX, +} CUBE_WHEEL_IDX; + +extern const CUBE_WHEEL_Driver* const cube_wheel_driver[CUBE_WHEEL_IDX_MAX]; + +/** + * @brief CUBE_WHEEL定期実行アプリ生成関数 + * @param void + * @return AppInfo + */ +AppInfo DI_CUBE_WHEEL_update(void); + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_Enable(const CommonCmdPacket* packet); // 旧 RW_startup +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_Init(const CommonCmdPacket* packet); // 旧 RW_startup+enable +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EndProcess(const CommonCmdPacket* packet); + + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetSpeed(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetDutyCycle(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetControlMode(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetPwmGain(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetMainGain(const CommonCmdPacket* packet); + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_GetStatus(const CommonCmdPacket* packet); + + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SetBackupmode(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EnableEncoder(const CommonCmdPacket* packet); +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_EnableHallsensor(const CommonCmdPacket* packet); + +CCP_EXEC_STS Cmd_DI_CUBE_WHEEL_SET_ROTATION_DIRECTION_VECTOR(const CommonCmdPacket* packet); + +int DI_CUBE_WHEEL_set_torque_Nm(CUBE_WHEEL_IDX idx, float torque_rw_Nm); +int DI_CUBE_WHEEL_set_speed_rpm(CUBE_WHEEL_IDX idx, float speed_rw_rpm); + +#endif diff --git a/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/rw_controller.c b/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/rw_controller.c index 6fd97432..18d00a38 100644 --- a/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/rw_controller.c +++ b/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/rw_controller.c @@ -43,6 +43,55 @@ void APP_RW_CONTROLLER_exec_(void) EL_record_event(el_group_error, (uint32_t)idx_rw, EL_ERROR_LEVEL_LOW, (uint32_t)RW0003_EL_NOTE_TORQUE_CMD); } } + + + /* + // 回転数指令でのRW制御 TODO:今後トルク指令との場合分けを行う + static int cnt_come_here = 0; + if (0 < cnt_come_here++ % rw_controller_.set_speed_interval) + { + return; + } + + // 時刻差分計算 + ObcTime current_obc_time = TMGR_get_master_clock(); + float dt_s = (float)OBCT_diff_in_sec(&rw_controller_.previous_obc_time, ¤t_obc_time); + if (dt_s < 0.0f || dt_s > 10.0f) + { + // 時刻差分が異常値の場合は何もせずに終了し、次のタイミングで正常になることを期待する + rw_controller_.previous_obc_time = TMGR_get_master_clock(); + return; + } + + float acceleration_rw_rad_s2[CUBE_WHEEL_IDX_MAX]; + float calc_speed_rw_rad_s[CUBE_WHEEL_IDX_MAX]; + float filtered_target_speed_rw_rad_s[CUBE_WHEEL_IDX_MAX]; + float target_speed_rw_rpm[CUBE_WHEEL_IDX_MAX]; + for (uint8_t idx = 0; idx < CUBE_WHEEL_IDX_MAX; idx++) + { + // トルク制限 + float max_torque = 0.00023f; + float min_torque = -max_torque; + if (max_torque < torque_rw_Nm[idx]) + { + torque_rw_Nm[idx] = max_torque; + } + else if (torque_rw_Nm[idx] < min_torque) + { + torque_rw_Nm[idx] = min_torque; + } + + // 積分計算 + acceleration_rw_rad_s2[idx] = torque_rw_Nm[idx] / aocs_manager->rw_moment_of_inertia_kgm2[idx] * rw_controller_.torque_scaler; + calc_speed_rw_rad_s[idx] = aocs_manager->rw_angular_velocity_rad_s[idx] + acceleration_rw_rad_s2[idx] * dt_s; + // LPF + filtered_target_speed_rw_rad_s[idx] = Z_FILTER_calc_output(&rw_controller_.lpf_ctrl[idx], calc_speed_rw_rad_s[idx]); + // 指令 + target_speed_rw_rpm[idx] = PHYSICAL_CONST_rad_sec_to_rpm(filtered_target_speed_rw_rad_s[idx]); + DI_CUBE_WHEEL_set_speed_rpm((CUBE_WHEEL_IDX)idx, target_speed_rw_rpm[idx]); + } + rw_controller_.previous_obc_time = TMGR_get_master_clock(); + */ } #pragma section diff --git a/src/src_user/Drivers/Aocs/cube_wheel.c b/src/src_user/Drivers/Aocs/cube_wheel.c new file mode 100644 index 00000000..155e77f4 --- /dev/null +++ b/src/src_user/Drivers/Aocs/cube_wheel.c @@ -0,0 +1,545 @@ +#pragma section REPRO +/** +* @file cube_wheel.c +* @brief cube_wheelのDriver +*/ + +#include "./cube_wheel.h" +#include +#include "string.h" // for memcpy +#include +#include +#include "../../Library/vector.h" + +static uint8_t GetStartupStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +static void WriteRegister(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t register_address, uint8_t* send_data, uint8_t length); +static void ReadRegister(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t register_address, uint8_t* receive_data, uint8_t length); +static uint8_t ReadID(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t* data); +static uint8_t ReadExtendedID(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t* data); +static uint8_t ReadErrorFlag(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); + +const static uint8_t CUBE_WHEEL_kCmdIdSetReset_ = 1; +const static uint8_t CUBE_WHEEL_kCmdIdSetWheelSpeed_ = 2; +const static uint8_t CUBE_WHEEL_kCmdIdSetWheelTorque_ = 3; +const static uint8_t CUBE_WHEEL_kCmdIdSetMotorPowerState_ = 7; +const static uint8_t CUBE_WHEEL_kCmdIdSetEncoderPowerState_ = 8; +const static uint8_t CUBE_WHEEL_kCmdIdSetHallPowerState_ = 9; +const static uint8_t CUBE_WHEEL_kCmdIdSetControlMode_ = 10; +const static uint8_t CUBE_WHEEL_kCmdIdSetBackupWheelMode_ = 12; +const static uint8_t CUBE_WHEEL_kCmdIdSetClearErrors_ = 20; +const static uint8_t CUBE_WHEEL_kCmdIdSetI2cAddresss_ = 31; +const static uint8_t CUBE_WHEEL_kCmdIdSetPwmGain_ = 33; +const static uint8_t CUBE_WHEEL_kCmdIdSetMainGain_ = 34; +const static uint8_t CUBE_WHEEL_kCmdIdSetBackupGain_ = 35; + +const static uint8_t CUBE_WHEEL_kCmdIdGetIdentification_ = 128; +const static uint8_t CUBE_WHEEL_kCmdIdGetExIdentification_ = 129; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelStatus_ = 130; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelSpeed_ = 133; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelReference_ = 134; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelCurrent_ = 135; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelData_ = 137; +const static uint8_t CUBE_WHEEL_kCmdIdGetWheelDataAdditional_ = 138; +const static uint8_t CUBE_WHEEL_kCmdIdGetPwmGain_ = 139; +const static uint8_t CUBE_WHEEL_kCmdIdGetMainGain_ = 140; +const static uint8_t CUBE_WHEEL_kCmdIdGetBackupGain_ = 141; +const static uint8_t CUBE_WHEEL_kCmdIdGetStatusErrorFlag_ = 145; + +const static uint8_t CUBE_WHEEL_kCmdLengthSetReset_ = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetWheelSpeed_ = 2; +const static uint8_t CUBE_WHEEL_kCmdLengthSetWheelTorque_ = 2; +const static uint8_t CUBE_WHEEL_kCmdLengthSetMotorPowerState_ = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetEncoderPowerState_ = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetHallPowerState_ = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetControlMode_ = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetBackupWheelMode = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetClearErrors = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetI2cAddress = 1; +const static uint8_t CUBE_WHEEL_kCmdLengthSetPwmGain = 3; +const static uint8_t CUBE_WHEEL_kCmdLengthSetMainGain = 6; +const static uint8_t CUBE_WHEEL_kCmdLengthSetBackupGain = 6; + + +const static uint8_t CUBE_WHEEL_kTlmLengthGetIdentification_ = 8; +const static uint8_t CUBE_WHEEL_kTlmLengthGetExIdentification_ = 4; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelStatus_ = 8; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelSpeed_ = 2; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelReference_ = 2; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelCurrent_ = 2; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelData_ = 6; +const static uint8_t CUBE_WHEEL_kTlmLengthGetWheelDataAdditional_ = 4; +const static uint8_t CUBE_WHEEL_kTlmLengthGetPwmGain_ = 4; +const static uint8_t CUBE_WHEEL_kTlmLengthGetMainGain_ = 7; +const static uint8_t CUBE_WHEEL_kTlmLengthGetBackupGain_ = 7; +const static uint8_t CUBE_WHEEL_kTlmLengthGetStatusErrorFlag_ = 1; + +const static uint8_t CUBE_WHEEL_kConvertValueOfCurrentFactor_ = 0.48828125; + +// Endianがビッグエンディアン + +int CUBE_WHEEL_init(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t ch, uint8_t device_address, uint8_t enable_port_no, uint8_t axis_id) +{ + + CUBE_WHEEL_driver->info.enable_port_no = enable_port_no; + GPIO_set_direction(CUBE_WHEEL_driver->info.enable_port_no, GPIO_OUTPUT); + + CUBE_WHEEL_driver->info.axis_id = axis_id; + CUBE_WHEEL_driver->info.maxspeed_in_rpm = 8000; // rpm + CUBE_WHEEL_driver->info.max_torque = 0.23; // mNm + + CUBE_WHEEL_driver->info.rotation_direction_b[0] = 1.0f; + CUBE_WHEEL_driver->info.rotation_direction_b[1] = 0.0f; + CUBE_WHEEL_driver->info.rotation_direction_b[2] = 0.0f; + + CUBE_WHEEL_driver->driver.i2c_write.ch = ch; + CUBE_WHEEL_driver->driver.i2c_write.frequency_khz = 100; + CUBE_WHEEL_driver->driver.i2c_write.device_address = (device_address << 1); // write + CUBE_WHEEL_driver->driver.i2c_write.stop_flag = 1; + CUBE_WHEEL_driver->driver.i2c_write.timeout_threshold = 500; + CUBE_WHEEL_driver->driver.i2c_write.rx_length = 8; + + // todo: ここはreopen? + if (I2C_init(&CUBE_WHEEL_driver->driver.i2c_write) != 0) + { + Printf("Error: I2C_init in i2c_rw_write_.\n"); + } + + CUBE_WHEEL_driver->driver.i2c_read.ch = ch; + CUBE_WHEEL_driver->driver.i2c_read.frequency_khz = 100; + CUBE_WHEEL_driver->driver.i2c_read.device_address = (device_address << 1) + 1; // read + CUBE_WHEEL_driver->driver.i2c_read.stop_flag = 1; + CUBE_WHEEL_driver->driver.i2c_read.timeout_threshold = 500; + CUBE_WHEEL_driver->driver.i2c_read.rx_length = 8; + + // todo: ここはreopen? + if (I2C_init(&CUBE_WHEEL_driver->driver.i2c_read) != 0) + { + Printf("Error: I2C_init in i2c_rw_read_.\n"); + } + + CUBE_WHEEL_driver->driver.i2c_slave.ch = ch; + CUBE_WHEEL_driver->driver.i2c_slave.frequency_khz = 100; + CUBE_WHEEL_driver->driver.i2c_slave.device_address = device_address; // slave + CUBE_WHEEL_driver->driver.i2c_slave.stop_flag = 1; + CUBE_WHEEL_driver->driver.i2c_slave.timeout_threshold = 500; + CUBE_WHEEL_driver->driver.i2c_slave.rx_length = 8; + + // todo: ここはreopen? + if (I2C_init(&CUBE_WHEEL_driver->driver.i2c_slave) != 0) + { + Printf("Error: I2C_init in i2c_rw_slave_.\n"); + } + + return 0; +} + +DS_CMD_ERR_CODE CUBE_WHEEL_Disable(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + GPIO_set_output(CUBE_WHEEL_driver->info.enable_port_no, GPIO_LOW); + + return DS_CMD_OK; +} + +DS_CMD_ERR_CODE CUBE_WHEEL_Enable(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + + GPIO_set_output(CUBE_WHEEL_driver->info.enable_port_no, GPIO_HIGH); + + if (I2C_reopen(&CUBE_WHEEL_driver->driver.i2c_slave, 0) != 0) + { + Printf("Error: I2C_init in i2c_rw_slave_.\n"); + return DS_CMD_UNKNOWN_ERR; + } + else + { + return DS_CMD_OK; + } + + return DS_CMD_UNKNOWN_ERR; +} + + +DS_CMD_ERR_CODE CUBE_WHEEL_Startup(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + if (GetStartupStatus(CUBE_WHEEL_driver) == 0) + { + // set speed mode in initial state + CUBE_WHEEL_SetControlMode(CUBE_WHEEL_driver, 3); + + Printf("Success Startup: RW id= %d\n", CUBE_WHEEL_driver->info.axis_id); + CUBE_WHEEL_driver->info.valid = true; + return DS_CMD_OK; + } + else + { + Printf("Error Startup: RW id = %d\n", CUBE_WHEEL_driver->info.axis_id); + CUBE_WHEEL_driver->info.valid = false; + return DS_CMD_UNKNOWN_ERR; + } + + // GetStartupStatusからの返り値が戻り値になっている． + return DS_CMD_OK; +} + +DS_CMD_ERR_CODE CUBE_WHEEL_GetStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + if (CUBE_WHEEL_driver->info.valid) + { + CUBE_WHEEL_GetWheelStatus(CUBE_WHEEL_driver); + CUBE_WHEEL_GetWheelDuty(CUBE_WHEEL_driver); + CUBE_WHEEL_GetWheelSpeed(CUBE_WHEEL_driver); + CUBE_WHEEL_GetPwmGain(CUBE_WHEEL_driver); + CUBE_WHEEL_GetMainGain(CUBE_WHEEL_driver); + + } + return DS_CMD_OK; +} + +uint8_t CUBE_WHEEL_SetDutyCycle(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int16_t duty) +{ + int i_ret = 0; + + uint8_t data[2]; + data[1] = (duty >> 8) & 0xFF; + data[0] = duty & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetWheelTorque_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_SetReferenceSpeed(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int16_t speed) +{ + int i_ret = 0; + int16_t raw_value = speed * 2; + + uint8_t data[2] = { 0 }; + data[1] = (raw_value >> 8) & 0xFF; + data[0] = raw_value & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetWheelSpeed_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_SetControlMode(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t value) +{ + int i_ret = 0; + + uint8_t data[1] = { 0 }; + data[0] = value & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetControlMode_, data, sizeof(data)); + + return i_ret; +} +uint8_t CUBE_WHEEL_SetPwmGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int Ki, uint8_t KiMultiplier) +{ + int i_ret = 0; + + uint8_t data[3] = { 0 }; + data[1] = (Ki >> 8) & 0xFF; + data[0] = Ki & 0xFF; + + data[2] = KiMultiplier & 0xFF; + + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetPwmGain_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_SetMainGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint16_t Ki, uint8_t KiMultiplier, uint16_t Kd, uint8_t KdMultiplier) +{ + + int i_ret = 0; + + uint8_t data[6] = { 0 }; + data[1] = (Ki >> 8) & 0xFF; + data[0] = Ki & 0xFF; + data[2] = KiMultiplier & 0xFF; + + data[4] = (Kd >> 8) & 0xFF; + data[3] = Kd & 0xFF; + data[5] = KdMultiplier & 0xFF; + + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetMainGain_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_GetWheelData(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetWheelData_]; + uint8_t s_data[2] = { 0 }; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetWheelData_, data, CUBE_WHEEL_kTlmLengthGetWheelData_); + + short raw_value = 0; + + s_data[0] = data[0]; + s_data[1] = data[1]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.speed_in_rpm = (float)(raw_value / 2); + + s_data[0] = data[2]; + s_data[1] = data[3]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.reference_in_rpm = (float)(raw_value / 2); + + uint16_t uraw_value = 0; + uraw_value = data[5] << 8; + uraw_value += data[4]; + CUBE_WHEEL_driver->info.current = (float)(uraw_value * CUBE_WHEEL_kConvertValueOfCurrentFactor_); + + return i_ret; +} + +uint8_t CUBE_WHEEL_GetWheelStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetWheelStatus_]; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetWheelStatus_, data, CUBE_WHEEL_kTlmLengthGetWheelStatus_); + + uint16_t raw_value = 0; + raw_value = data[1] << 8; + raw_value += data[0]; + CUBE_WHEEL_driver->info.runtime = (uint16_t)raw_value; + + raw_value = data[3] << 8; + raw_value += data[2]; + CUBE_WHEEL_driver->info.runtime_in_msec = (uint16_t)raw_value; + + raw_value = data[5] << 8; // reserve + raw_value += data[4]; // reserve + + raw_value = data[6]; + CUBE_WHEEL_driver->info.motor_mode = (uint8_t)raw_value; + + raw_value = data[7]; + CUBE_WHEEL_driver->info.backup_mode_state = raw_value & 0x01; + CUBE_WHEEL_driver->info.motor_state = (raw_value >> 1) & 0x01; + CUBE_WHEEL_driver->info.hall_sensor_state = (raw_value >> 2) & 0x01; + CUBE_WHEEL_driver->info.encoder_state = (raw_value >> 3) & 0x01; + CUBE_WHEEL_driver->info.error_flag = (raw_value >> 4) & 0x01; + + return i_ret; +} + +uint8_t CUBE_WHEEL_GetWheelSpeed(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + + uint8_t data[CUBE_WHEEL_kTlmLengthGetWheelSpeed_]; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetWheelSpeed_, data, CUBE_WHEEL_kTlmLengthGetWheelSpeed_); + + int raw_value = 0; + uint8_t s_data[2] = { 0 }; + + s_data[0] = data[0]; + s_data[1] = data[1]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.speed_in_rpm = (float)(raw_value / 2); + + return i_ret; +} + +uint8_t CUBE_WHEEL_GetWheelDuty(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + uint8_t data[CUBE_WHEEL_kTlmLengthGetWheelDataAdditional_]; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetWheelDataAdditional_, data, CUBE_WHEEL_kTlmLengthGetWheelDataAdditional_); + + int raw_value = 0; + uint8_t s_data[2] = { 0 }; + + s_data[0] = data[0]; + s_data[1] = data[1]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.duty = (float)(raw_value); + + s_data[0] = data[2]; + s_data[1] = data[3]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.speed_backup_in_rpm = (float)(raw_value / 2); + + return 0; +} + +uint8_t CUBE_WHEEL_GetPwmGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetPwmGain_]; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetPwmGain_, data, CUBE_WHEEL_kTlmLengthGetPwmGain_); + + int raw_value = 0; + uint8_t s_data[2] = { 0 }; + + s_data[0] = data[0]; + s_data[1] = data[1]; + raw_value = raw2signedshort_little(s_data); + CUBE_WHEEL_driver->info.gain_pwm = (float)raw_value; + + + raw_value = data[2]; + CUBE_WHEEL_driver->info.gain_pwm_multiplier = (uint8_t)raw_value; + + return i_ret; +} + +uint8_t CUBE_WHEEL_GetMainGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetMainGain_]; + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetMainGain_, data, CUBE_WHEEL_kTlmLengthGetMainGain_); + + uint16_t raw_value = 0; + raw_value = data[1] << 8; + raw_value += data[0]; + CUBE_WHEEL_driver->info.feedback_gain = raw_value; + + raw_value = data[2]; + CUBE_WHEEL_driver->info.feedback_gain_multiplier = (uint8_t)raw_value; + + raw_value = data[4] << 8; + raw_value += data[3]; + CUBE_WHEEL_driver->info.integrator_gain = raw_value; + + raw_value = data[5]; + CUBE_WHEEL_driver->info.integrator_gain_multiplier = (uint8_t)raw_value; + + return i_ret; + +} + +uint8_t CUBE_WHEEL_SetBackupmode(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t enable_status) +{ + int i_ret = 0; + uint8_t data[1] = { 0 }; + data[0] = enable_status & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetBackupWheelMode_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_EnableEncoder(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t enable_status) +{ + int i_ret = 0; + uint8_t data[1] = { 0 }; + data[0] = enable_status & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetEncoderPowerState_, data, sizeof(data)); + + return i_ret; +} + +uint8_t CUBE_WHEEL_EnableHallsensor(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t enable_status) +{ + int i_ret = 0; + uint8_t data[1] = { 0 }; + data[0] = enable_status & 0xFF; + WriteRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdSetHallPowerState_, data, sizeof(data)); + + return i_ret; +} + +static uint8_t GetStartupStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + uint8_t i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetIdentification_]; + i_ret = ReadID(CUBE_WHEEL_driver, data); + if (i_ret != 0) + { + return i_ret; + } + i_ret = ReadExtendedID(CUBE_WHEEL_driver, data); + return i_ret; +} + +static void WriteRegister(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t register_address, uint8_t* send_data, uint8_t length) +{ + uint8_t send_data_buffer[10] = { 0 }; + + // Make send data + send_data_buffer[0] = register_address; + + // todo error処理 + if (length > 10) + { + return; + } + memcpy(&send_data_buffer[1], &send_data[0], length); + + // Send data + I2C_tx(&CUBE_WHEEL_driver->driver.i2c_slave, &send_data_buffer[0], length + 1); + + return; +} + +static void ReadRegister(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t register_address, uint8_t* receive_data, uint8_t length) +{ + // Resister set + uint8_t send_data_buffer[1] = { 0 }; + send_data_buffer[0] = register_address; + I2C_tx(&CUBE_WHEEL_driver->driver.i2c_slave, &send_data_buffer[0], 1); + + // Read data + CUBE_WHEEL_driver->driver.i2c_slave.rx_length = length; + I2C_rx(&CUBE_WHEEL_driver->driver.i2c_slave, &receive_data[0], length); + return; +} + +static uint8_t ReadID(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t* data) +{ + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetIdentification_, data, CUBE_WHEEL_kTlmLengthGetIdentification_); + + // TLM128のdata[0]は8固定。 + // Printf("ReadID = %d\n", data[0]); + if (data[0] != 0x08) + { + return(1); + } + + return 0; +} + + +static uint8_t ReadExtendedID(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t* data) +{ + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetExIdentification_, data, CUBE_WHEEL_kTlmLengthGetExIdentification_); + + // TLM129のdata[2]はI2Cアドレス + // Printf("ReadExtendedID = %d : %d", data[2], CUBE_WHEEL_driver->driver.i2c_write.device_address); + if (data[2] != CUBE_WHEEL_driver->driver.i2c_write.device_address) + { + return(1); + } + + return 0; +} + +uint8_t ReadErrorFlag(CUBE_WHEEL_Driver* CUBE_WHEEL_driver) +{ + int i_ret = 0; + uint8_t data[CUBE_WHEEL_kTlmLengthGetStatusErrorFlag_]; + + ReadRegister(CUBE_WHEEL_driver, CUBE_WHEEL_kCmdIdGetStatusErrorFlag_, data, CUBE_WHEEL_kTlmLengthGetStatusErrorFlag_); + + + // Printf("Cube Wheel Error flag:"); + int i; + for (i = 0; i < CUBE_WHEEL_kTlmLengthGetStatusErrorFlag_; i++) + { + // Printf(data[i], HEX); Printf(" "); + } + // Printf(""); + + return i_ret; +} + +C2A_MATH_ERROR CUBE_WHEEL_set_rotation_direction_b(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, + const float rotation_direction_b[PHYSICAL_CONST_THREE_DIM]) +{ + C2A_MATH_ERROR is_normalized = VECTOR_is_normalized(rotation_direction_b); + if (is_normalized != C2A_MATH_ERROR_OK) return is_normalized; + VECTOR_copy(CUBE_WHEEL_driver->info.rotation_direction_b, + rotation_direction_b); + return C2A_MATH_ERROR_OK; +} + +#pragma section diff --git a/src/src_user/Drivers/Aocs/cube_wheel.h b/src/src_user/Drivers/Aocs/cube_wheel.h new file mode 100644 index 00000000..3c3806a0 --- /dev/null +++ b/src/src_user/Drivers/Aocs/cube_wheel.h @@ -0,0 +1,108 @@ +#ifndef CUBE_WHEEL_H_ +#define CUBE_WHEEL_H_ + + +#include +#include +#include +#include "../../Library/physical_constants.h" +#include "../../Library/c2a_math.h" +#include +#include + +#define RW_AXIS_ID_X 0 +#define RW_AXIS_ID_Y 1 +#define RW_AXIS_ID_Z 2 + +#define RW_BACKUP_MODE_OFF 0 +#define RW_BACKUP_MODE_ON 1 + +/** + * @struct CUBE_WHEEL_Info + * @brief CUBE_WHEELのテレメトリ情報 + */ +typedef struct +{ + uint8_t enable_port_no; + uint8_t axis_id; + bool valid; + uint16_t runtime; + uint16_t runtime_in_msec; + float speed_in_rpm; + float speed_backup_in_rpm; + float maxspeed_in_rpm; + float reference_in_rpm; + float max_torque; + float current; + float duty; + uint8_t motor_mode; + bool backup_mode_state; + bool motor_state; + bool hall_sensor_state; + bool encoder_state; + bool error_flag; + float gain_pwm; + uint8_t gain_pwm_multiplier; + uint16_t feedback_gain; + uint8_t feedback_gain_multiplier; + uint16_t integrator_gain; + uint8_t integrator_gain_multiplier; + float rotation_direction_b[3]; + +} CUBE_WHEEL_Info; + +/** +* @struct CUBE_WHEEL_Driver +* @brief CUBE_WHEEL_Driver構造体 +*/ +typedef struct +{ + struct + { + I2C_Config i2c_write; + I2C_Config i2c_read; + I2C_Config i2c_slave; //!< I2C class + } driver; + CUBE_WHEEL_Info info; + +} CUBE_WHEEL_Driver; + + +int CUBE_WHEEL_init(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t ch, uint8_t device_address, uint8_t enable_port_no, uint8_t axis_id); + +// これは必要そう +DS_CMD_ERR_CODE CUBE_WHEEL_Disable(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +DS_CMD_ERR_CODE CUBE_WHEEL_Enable(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +DS_CMD_ERR_CODE CUBE_WHEEL_Startup(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +DS_CMD_ERR_CODE CUBE_WHEEL_GetStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); + + +uint8_t CUBE_WHEEL_SetReferenceSpeed(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int16_t speed); +uint8_t CUBE_WHEEL_SetDutyCycle(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int16_t duty); + +uint8_t CUBE_WHEEL_SetControlMode(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t value); +uint8_t CUBE_WHEEL_SetPwmGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, int Ki, uint8_t KiMultiplier); +uint8_t CUBE_WHEEL_SetMainGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint16_t Ki, uint8_t KiMultiplier, uint16_t Kd, uint8_t KdMultiplier); + +uint8_t CUBE_WHEEL_GetWheelData(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +uint8_t CUBE_WHEEL_GetWheelStatus(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +uint8_t CUBE_WHEEL_GetWheelSpeed(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +uint8_t CUBE_WHEEL_GetWheelDuty(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +uint8_t CUBE_WHEEL_GetPwmGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); +uint8_t CUBE_WHEEL_GetMainGain(CUBE_WHEEL_Driver* CUBE_WHEEL_driver); + + +uint8_t CUBE_WHEEL_SetBackupmode(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t en); +uint8_t CUBE_WHEEL_EnableEncoder(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t en); +uint8_t CUBE_WHEEL_EnableHallsensor(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, uint8_t en); + +/** + * @brief 回転方向ベクトル設定関数 + * @param *CUBE_WHEEL_driver : CUBE_WHEEL_Driver構造体へのポインタ + * @param rotation_direction_b : 機体固定座標系での回転方向ベクトル + * @return C2A_MATH_ERRORに準じる + */ +C2A_MATH_ERROR CUBE_WHEEL_set_rotation_direction_b(CUBE_WHEEL_Driver* CUBE_WHEEL_driver, + const float rotation_direction_b[PHYSICAL_CONST_THREE_DIM]); + +#endif