この一連の記事ではスマート卓上ファンを作ってみます。このファンにはSTマイクロエレクトロニクスのToF（Time-of-Flight）測距センサを使っています。一部のToF測距センサはst.comで提供されるスマート人感検出（以下SPD : Smart Presence Detection）を使うことで高度な人感センサとして動作します。このSPDの結果を使い、羽の回転や左右の首振りを制御します。
前の記事へのリンク
前回までの記事では、ToF測距離センサとSPDのご紹介から始まり、機構・モータ制御の考察、NUCLEO-H533REのベースとなるプロジェクトの作成、SPDの移植を進めてきました。この記事ではSPD出力に応じてモータを制御していきます！
5. SPDの結果を使ってモータを動かす
ここまででやっとソフトウェア実装の準備が整いました。SPDのサンプルソフトをそのまま使って実装を進めるので、「SPDの結果を使って」の部分はそんなに難しくありません。SPDの結果を角度に変換して、その角度をモータのステップに変換します。
5.1 ステッピングモータを動かす
まずはステッピングモータを動かすコードです。上で書いたようにGPIO４本を制御すると回ります。３つの関数をapp_comm.cに入れておきます。見ての通り、Arduinoのステッピングモータのライブラリを参考にしていきます。
app_comm.cに追加する関数の修正コード詳細
- digitalWrite()はGPIOを制御するコードです。
- Stepper_stepMotor()は位相ごとに波形のパターンを作るコードです。冗長ですが可読性を優先します。
- Stepper_step()は現在の角度から＋か－の方向に与えられたステップの位相パターンを出力していきます。ロジックのシンプルさを優先して同期的なコードになっています。Stepper_step_numberは現在のステップ位置を表していますが、-Stepper_number_of_steps/4～+Stepper_number_of_steps/4の値をとって、マイナスの値を取るところが特徴的です。
app_comm.c
////////////////////////////////////////////////////////////////////
//
// Control Stepping Motor
//
//#define NUMBER_OF_PHASE 4
#define NUMBER_OF_PHASE 8
int Stepper_step_number = 0;//absolute position (step number 0 ~~ 2048 or 4096)
int Stepper_direction = 0;
#if NUMBER_OF_PHASE==8
int Stepper_number_of_steps = 4096;
#else
int Stepper_number_of_steps = 2048;
#endif
unsigned long Stepper_last_step_time = 0;
unsigned long Stepper_step_delay = 10;
//
// Control GPIO Signal
//
//PB1, PB15, PB14, PB13
void digitalWrite(int pin_number, int on_off)
{
GPIO_TypeDef* GPIOx_Array[] = {GPIOA, GPIOA, GPIOC, GPIOC};
uint16_t GPIO_Pin_Array[] = {GPIO_PIN_1, GPIO_PIN_0, GPIO_PIN_12, GPIO_PIN_10};
GPIO_TypeDef* GPIOx = GPIOx_Array[pin_number];
uint16_t GPIO_Pin = GPIO_Pin_Array[pin_number];
GPIO_PinState PinState = (GPIO_PinState)on_off;
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, PinState);
}
//
// Control GPIOs
//
void Stepper_stepMotor(int thisStep)
{
#define HIGH 1
#define LOW 0
int motor_pin_1 = 0;
int motor_pin_2 = 1;
int motor_pin_3 = 2;
int motor_pin_4 = 3;
#if NUMBER_OF_PHASE==8
switch (thisStep) {
case 0: // 1000
digitalWrite(motor_pin_1, HIGH);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, LOW);
break;
case 1: // 1100
digitalWrite(motor_pin_1, HIGH);
digitalWrite(motor_pin_2, HIGH);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, LOW);
break;
case 2: //0100
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, HIGH);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, LOW);
break;
case 3: //0110
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, HIGH);
digitalWrite(motor_pin_3, HIGH);
digitalWrite(motor_pin_4, LOW);
break;
case 4: // 0010
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, HIGH);
digitalWrite(motor_pin_4, LOW);
break;
case 5: // 0011
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, HIGH);
digitalWrite(motor_pin_4, HIGH);
break;
case 6: //0001
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, HIGH);
break;
case 7: //1001
digitalWrite(motor_pin_1, HIGH);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, HIGH);
break;
}
#else
switch (thisStep) {
case 0: // 1010
digitalWrite(motor_pin_1, HIGH);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, HIGH);
digitalWrite(motor_pin_4, LOW);
break;
case 1: // 0110
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, HIGH);
digitalWrite(motor_pin_3, HIGH);
digitalWrite(motor_pin_4, LOW);
break;
case 2: //0101
digitalWrite(motor_pin_1, LOW);
digitalWrite(motor_pin_2, HIGH);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, HIGH);
break;
case 3: //1001
digitalWrite(motor_pin_1, HIGH);
digitalWrite(motor_pin_2, LOW);
digitalWrite(motor_pin_3, LOW);
digitalWrite(motor_pin_4, HIGH);
break;
}
#endif
}
//
// Control Motor Phase
//
void Stepper_step(int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (steps_to_move > 0) { Stepper_direction = 1; }
if (steps_to_move < 0) { Stepper_direction = 0; }
// decrement the number of steps, moving one step each time:
while (steps_left > 0)
{
unsigned long now = HAL_GetTick();
// move only if the appropriate delay has passed:
if (now - Stepper_last_step_time >= Stepper_step_delay)
{
// get the timeStamp of when you stepped:
Stepper_last_step_time = now;
// increment or decrement the step number,
// depending on direction:
if (Stepper_direction == 1)
{
Stepper_step_number++;
//if (Stepper_step_number == Stepper_number_of_steps) {
// Stepper_step_number = 0;
//}
if (Stepper_step_number >= Stepper_number_of_steps/4) {
break;
}
}
else
{
//if (Stepper_step_number == 0) {
// Stepper_step_number = Stepper_number_of_steps;
//}
Stepper_step_number--;
if (Stepper_step_number <= -Stepper_number_of_steps/4) {
break;
}
}
// decrement the steps left:
steps_left--;
// change to plus step number
int Stepper_phase_number = 0;
Stepper_phase_number = Stepper_step_number;
if ( Stepper_phase_number<0 ){
Stepper_phase_number += Stepper_number_of_steps;
}
Stepper_phase_number %= NUMBER_OF_PHASE;
// step the motor to step number 0, 1, ..., {3 or 10}
Stepper_stepMotor(Stepper_phase_number);
} else {
//DELAY_MS(1);
}
}
}
5.2 ステッピングモータで扇風機を左右に動かす
まずはSPDの結果が出てきた後にファンを制御する関数control_fun()を呼び出します。
control_fun()呼び出しの修正コード詳細
main.c
if(Params.InterruptMode == 0) {
/* Run SPD */
start_profiling();
SPD_run(&SPD_Data, &SEN_MeasData, &SEN_Info);
stop_profiling("SPD_run()");
/* Data logging */
print_data();
+ control_fun();
}
ファンを制御するコードはapp_comm.cに置きます。２つの関数を定義します。
- control_fun()では、人が前にいることが検出された場合にx,yの座標を絶対角度に変換します。その角度でFAN_SetAngle()を呼び出します。
- FAN_SetAngle()では、絶対角度を現在の位置からの相対角度に変換してステッピングモータを動かすStepper_step()を呼び出します。
app_comm.cに追加する２つ関数のコード詳細
app_comm.c
////////////////////////////////////////////////////////////////////
//
// Control Fun
//
int FAN_SetAngle(int angle);
int current_angle = 0;
void control_fun()
{
if (SPD_Data.presence){
if (SPD_Data.USR_Data.nb_of_users == 0){
// no one
}
else{
// present
if ( SPD_Data.USR_Data.nb_of_users>=1 ){
double x = (double)SPD_Data.USR_Data.user_properties[0].obj_prop.CoM_pos.x;//+right -left mm
double y = (double)SPD_Data.USR_Data.user_properties[0].obj_prop.CoM_pos.z;//-distance mm
// rotate 90deg and flip
double x_2 = -y + 90/*mm distance from sensor to center of turn table*/;
double y_2 = -x;// flip angle
// calculate angle in degree
int angle = (int)(atan(y_2/x_2)*180.0/3.14156535);
// limit angle
if (angle>40){
angle = 40;
}else if ( angle<-40 ){
angle = -40;
}
uart_printf("\n");
uart_printf("angle = %d \n", angle);
// filter small move
if ( abs(angle-current_angle)>=2 ){
current_angle = FAN_SetAngle(angle);
}
}
}
}
else{
if (SPD_Data.state == SPD_AUTONOMOUS){
// zzz
}
else{
// no one
}
}
}
//
// Control Fan Anagle
//
int FAN_SetAngle(int angle)
{
int Stepper_step_number_dest;
Stepper_step_number_dest = angle * Stepper_number_of_steps / 360;
int steps_to_move;
steps_to_move = Stepper_step_number_dest - Stepper_step_number;
uart_printf("\n");
uart_printf("angle = %d, step_to_move = %d \n", angle, steps_to_move);
uart_printf("org: %d ====> dest: %d \n", Stepper_step_number, Stepper_step_number_dest);
Stepper_step(steps_to_move);
int new_angle;
new_angle = Stepper_step_number * 360 / Stepper_number_of_steps;
return new_angle;
}
5.3 DCモータで扇風機をON/OFFする
SPDで人が前にいるかいないかを判断することができます。これを使って扇風機のON/OFFを制御します。いるときはON、いなくなったらOFFにします。
まずは誰かがセンサの前にいた場合にDCモータをONにするコードです。
app_comm.cのDCモータをONにする修正コード詳細
app_comm.c
void control_fun()
{
if (SPD_Data.presence){
if (SPD_Data.USR_Data.nb_of_users == 0){
// no one
}
else{
// present
+ // start DC motor
+ HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_SET);
if ( SPD_Data.USR_Data.nb_of_users>=1 ){
次に誰もいなくなった時にはDCモータをOFFにします。DCモータを停止して、ファンの角度を戻す処理を入れます。
app_comm.cのDCモータをOFFにする修正コード詳細
app_comm.c
void close_fun()
{
// stop DC motor
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_RESET);
// move back to center position
current_angle = FAN_SetAngle(0);
}
control_fun()の中で誰もいなくなったことを検出した時に、このclose_fun()関数を呼び出します。
app_comm.cの中でclose_fun()呼び出しの修正コード詳細
app_comm.c
else{
if (SPD_Data.state == SPD_AUTONOMOUS){
// zzz
+ close_fun();
}
else{
// no one
}
}
}
app_comm.hにcontrol_fun()とclose_fun()の宣言を追加しておきます。
app_comm.h内の修正コード詳細
app_comm.h
void print_data();
void print_user();
+ void control_fun();
+ void close_fun();
DCモータのENABLE Aは固定でも良いです。評価基板に接続している場合には、これをHIGHにする必要があります。これはmain.cのMX_GPIO_Init()の中で行います。
app_comm.h内の修正コード詳細
main.cのMX_GPIO_INIT(void)の中
/* USER CODE BEGIN MX_GPIO_Init_2 */
+ HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);//ENABLE A
/* USER CODE END MX_GPIO_Init_2 */
5.4 基板上のユーザボタンで扇風機をON/OFFする
基板上のボタンを押して扇風機をON/OFFできるようにします。以下のコードをapp_comm.cに追加します。rangingで現在の状態を見て、CommandDataを通じて、enableやdisableのコマンドを送ります。
app_comm.c内のボタン押下検出に使う修正コード詳細
app_comm.c
////////////////////////////////////////////////////////////////////
//
// User Button
//
extern int ranging;
/**
* @brief BSP Push Button callback
* @param Button Specifies the pin connected EXTI line
* @retval None
*/
void BSP_PB_Callback(Button_TypeDef Button)
{
/* Prevent unused argument(s) compilation warning */
UNUSED(Button);
/* This function should be implemented by the user application.
It is called into this driver when an event on Button is triggered. */
if ( ranging ){
CommandData.disable = 1;
}else{
CommandData.enable = 1;
}
}
今回はSPD出力に応じてモータを制御しました。モータは動きましたでしょうか？次回はプログラムのデバッグをしていきます。次の記事につづきます！
次の記事へのリンク