IMU ストアと接地センサのプログラムは，この SC のプログラムの中につくられる。
<![CDATA[
////////////////////////////////////////////
// store_complex.c
// MCU内ストア群のコンプレクス
////////////////////////////////////////////
#include <stdio.h>
#include <string.h>
#include <stdint.h>
////////////////////////////////////////////
// グローバル変数
////////////////////////////////////////////
/////////////////////////
// IMUストア
/////////////////////////
// タイマ
static uint32_t imu_last_time = 0;
// IMU データの型
// (IMU の出力から項目を選択)
typedef struct
{
float ax;
float ay;
float wx;
float wy;
} IMUData;
// IMU データの変数
// 変数 imu_out は値が変わりやすいので
// コンパイルにおいて最適化をしない
volatile IMUData imu_data;
////////////////
// バッファ imu_store_buffer
// ax, ay, wx, wy で 4 × 4 = 16 byte
volatile uint8_t imu_store_buffer[16];
HAL_I2C_Slave_Transmit_IT(
&hi2c3,
imu_store_buffer,
16
);
// IMUデータを bytes に変換
// imu_store_buffer に書く
void IMU_Data2StoreBuffer(
uint8_t* imu_store_buffer
)
{
memcpy(
imu_store_buffer,
(uint8_t*)&imu_data,
sizeof(IMUData)
);
}
/////////////////////////
// 接地センサストア
/////////////////////////
// タイマ
static uint32_t foot_last_time = 0;
// データ型
// （未定だが，ここでは仮に）
typedef struct
{
uint8_t front_left;
uint8_t front_right;
uint8_t rear_left;
uint8_t rear_right;
} FootData;
// データの変数
volatile FootData foot_data;
////////////////
// バッファ foot_store_buffer
// front_right, front_right, rear_left, rear_right
// で 1 byte × 4 = 8 byte
uint8_t foot_store_buffer[8];
HAL_I2C_Slave(
&hi2c5,
foot_store_buffer,
8
);
// 接地センサデータを bytes に変換
// foot_store_buffer に書く
void FOOT_Data2StoreBuffer(
uint8_t* foot_store_buffer
)
{
memcpy(
foot_store_buffer,
(uint8_t*)&foot_data,
sizeof(foot_store_buffer)
);
}
////////////////////////////////////////////
// ストアのタスク
////////////////////////////////////////////
/////////////////////////
// IMU ストア
// IMU を読み，imu_out に書く
// 周期 10m秒 (100Hz)
/////////////////////////
void IMUStore_Task(void)
{
// タイマ
uint32_t now =
HAL_GetTick();
if((now - imu_last_time) < 10)
{
return;
}
imu_last_time = now;
// IMU データ
MPU6050_t imu;
// IMU (MPU6050) とは I2C 接続
MPU6050_Read_All(
&hi2c2,
&imu
);
// imu_data に書く
imu_data.ax = imu.Ax;
imu_data.ay = imu.Ay;
imu_data.wx = imu.Gx;
imu_data.wy = imu.Gy;
// imu_data を bytes に変換して
// imu_store_buffer に書く
IMU_Data2StoreBuffer(imu_store_buffer);
}
/////////////////////////
// 接地センサストア
// 接地センサを読み，foot_out に書く
// 200Hz
/////////////////////////
void FootStore_Task(void)
{
// タイマ
uint32_t now =
HAL_GetTick();
if((now - foot_last_time) < 5)
{
return;
}
foot_last_time = now;
// 接地センサ データ
// foot_data に書く
// 接地センサ データ
接地センサ_t foot;
// 接地センサとは I2C 接続
接地センサ_Read_All(
&hi2c4,
&foot
);
// foot_data に書く
foot_data.front_left = foot.FL;
foot_data.front_right = foot.FR;
foot_data.rear_left = foot.RL;
foot_data.rear_right = foot.RR;
// foot_data を bytes に変換して
// foot_store_buffer に書く
FOOT_Data2StoreBuffer(foot_store_buffer);
}
////////////////////////////////////////////
// main
////////////////////////////////////////////
int main(void)
{
HAL_Init();
SystemClock_Config();
MPU6050_Init(&hi2c2);
接地センサ_Init(&hi2c4);
while(1)
{
IMUStore_Task();
FootStore_Task();
}
}
]]>
|