目次
| 章 | タイトル | 内容 |
|---|---|---|
| 1 | はじめに | 今回やることの説明 |
| 2 | モータの設定 | 動かす前のモータの設定 |
| 3 | パワーを設定する | モータのパワーを設定する |
| 4 | 停止する | モータの回転を停止を設定する |
| 5 | スピードを設定する | モータのスピードを設定する |
| 6 | パワー値・スピード値を得る | 現在のパワー値・スピード値を得る |
| 7 | 角度を得る | エンコーダから現在の角度を得る |
| 8 | その他機能 | モータに関するその他の機能について |
| 9 | まとめ | 今回のまとめ |
1. はじめに
前回までで、開発環境を整えることが出来ました。
今回からはいよいよ、ロボットを動かすためのプログラムを紹介していきます。
今回は、モータ系のAPIを紹介します。
モータ系のAPIの説明は以下のページに書かれています。本記事はそれをかみ砕いて、LEGO EducationのSPIKEソフトでの例も交えながら紹介していきます。
SPIKE-RT C API Reference [Japanese] / モータ
2. モータの設定
モータを動かす前に、ハブブロックの「ポート」と「モータの回転方向」を設定する必要があります。
SPIKEソフトで言うところの以下のブロックにあたります。
まず、「ポート」と「PUPモータデバイスポインタ」を結びつけます。
pup_motor_t *pup_motor_get_device(pbio_port_id_t port);
関数名はpup_motor_get_deviceで、指定したポートをもとに「PUPモータデバイスポインタ」を返します。
引数として、「ポート」を指定すると、戻り値として「PUPモータデバイスポインタ」が返ってきます。
ここで得られた「PUPモータデバイスポインタ」を指定することで、モータを制御していきます。
次に、モータを実際に動かせるように設定を行います。
pbio_error_t pup_motor_setup(pup_motor_t *motor, pup_direction_t positive_direction, bool reset_count);
関数名はpup_motor_setupで、モータの初期化(設定)を行います。
第一引数に「PUPモータデバイスポインタ」を入力し、どのポートのモータかを指定します。
第二引数には「モータの回転方向」として、時計回りならPUP_DIRECTION_CLOCKWISEを、反時計回りならPUP_DIRECTION_COUNTERCLOCKWISEを指定します。
第三引数は「エンコーダ(回転角)のリセット」で、trueを指定するとエンコーダがリセット(回転角が0度)されます。
戻り値はエラーコードですので、ここでは省略します。
では、簡単な使い方の例を示します。
ここでは、ポートAとBにモータをつないだロボットを想定します。
Ex2-1. モータのセットアップ
#include <stdlib.h>
#include <stdio.h>
#include <kernel.h>
#include <spike/hub/system.h>
#include <spikert_4.h>
#include "spike/pup/motor.h"
#include "spike/pup/colorsensor.h"
#include "spike/pup/forcesensor.h"
#include "spike/pup/ultrasonicsensor.h"
#include "spike/hub/battery.h"
#include "spike/hub/button.h"
#include "spike/hub/display.h"
#include "spike/hub/imu.h"
#include "spike/hub/light.h"
#include "spike/hub/speaker.h"
#include <pbio/color.h>
#include "kernel_cfg.h"
#include "syssvc/serial.h"
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
}
Main関数に入る前に、「PUPモータデバイスポインタ」をAとBの二つ分宣言しておきます。
Main関数に入ったら、pup_motor_get_deviceにより、「PUPモータデバイスポインタ」と各ポートを結びつけます。
結びつけることが出来たら、pup_motor_setupにより、モータの初期化をします。
これで、モータを動かす準備が完了しました。
尚、ポートを指定する定数PBIO_PORT_ID_AやPBIO_PORT_ID_Bを使用する場合、以下のファイルを編集する必要があります。
【場所】spike-rt/external/libpybricks/lib/pbio/include/pbdrv/config.h
// SPDX-License-Identifier: MIT
// Copyright (c) 2018-2020 The Pybricks Authors
#ifndef _PBDRV_CONFIG_H_
#define _PBDRV_CONFIG_H_
// This file is defined per-platform. It should override the config values below as needed.
#include "pbdrvconfig.h"
// set to (1) if there is a port labeled "A" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_A
- #define PBDRV_CONFIG_HAS_PORT_A (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_A (1) //編集箇所
#endif
// set to (1) if there is a port labeled "B" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_B
- #define PBDRV_CONFIG_HAS_PORT_B (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_B (1) //編集箇所
#endif
// set to (1) if there is a port labeled "C" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_C
- #define PBDRV_CONFIG_HAS_PORT_C (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_C (1) //編集箇所
#endif
// set to (1) if there is a port labeled "D" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_D
- #define PBDRV_CONFIG_HAS_PORT_D (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_D (1) //編集箇所
#endif
// set to (1) if there is a port labeled "E" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_E
- #define PBDRV_CONFIG_HAS_PORT_E (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_E (1) //編集箇所
#endif
// set to (1) if there is a port labeled "F" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_F
- #define PBDRV_CONFIG_HAS_PORT_F (0) //編集箇所
+ #define PBDRV_CONFIG_HAS_PORT_F (1) //編集箇所
#endif
// set to (1) if there is a port labeled "1" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_1
#define PBDRV_CONFIG_HAS_PORT_1 (0)
#endif
// set to (1) if there is a port labeled "2" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_2
#define PBDRV_CONFIG_HAS_PORT_2 (0)
#endif
// set to (1) if there is a port labeled "3" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_3
#define PBDRV_CONFIG_HAS_PORT_3 (0)
#endif
// set to (1) if there is a port labeled "4" on the programmable brick
#ifndef PBDRV_CONFIG_HAS_PORT_4
#define PBDRV_CONFIG_HAS_PORT_4 (0)
#endif
// the number of built-in motor controllers in the programmable brick
#ifndef PBDRV_CONFIG_NUM_MOTOR_CONTROLLER
#define PBDRV_CONFIG_NUM_MOTOR_CONTROLLER (0)
#elif PBDRV_CONFIG_NUM_MOTOR_CONTROLLER != 0
// the pbio_port_id_t enum value of the first motor port
#ifndef PBDRV_CONFIG_FIRST_MOTOR_PORT
#error PBDRV_CONFIG_NUM_MOTOR_CONTROLLER requires that PBDRV_CONFIG_FIRST_MOTOR_PORT is defined
#endif
// the pbio_port_id_t enum value of the last motor port
#ifndef PBDRV_CONFIG_LAST_MOTOR_PORT
#error PBDRV_CONFIG_NUM_MOTOR_CONTROLLER requires that PBDRV_CONFIG_LAST_MOTOR_PORT is defined
#endif
#endif
#endif // _PBDRV_CONFIG_H_
上記の編集が難しい場合は、定数を使用せずに'A'や'B'と入力することで、ポートを指定することも出来ます。
Ex2-2. モータのセットアップ (ポート指定方法)
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device('A');
motorB = pup_motor_get_device('B');
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
}
2023年12月5日現在、ポートFはデバッグ用に使用されているため、モータやセンサ用には使えません。
よって、pup_motor_get_deviceでポートFは指定しないでください。
【2024/03/01 追記】ポートFについて ====================
別の記事にて、SPIKE-RTでもポートFを使用する方法を公開いたしました。
ポートFを使用したい方は👇の記事をご覧ください。
============================================================
余談ですが、SPIKE Prime用のモータには以下の「Lモータ」と「Mモータ」がありますが、設定上の違いはありません。
Lモータの仕様についてはこちら👇
Mモータの仕様についてはこちら👇
3. パワーを設定する
モータの基本的な動かし方として、モータパワーを設定する方法を紹介します。
SPIKEソフトでは以下のブロックにあたります。
pbio_error_t pup_motor_set_power(pup_motor_t *motor, int power);
関数名はpup_motor_set_powerで、指定したポートのモータパワーを設定します。
第一引数には「PUPモータデバイスポインタ」を入力してポートを指定、第二引数にはそのポートのモータのパワーを整数値で指定します。
モータパワー値の範囲は $-100 \sim +100$ です。
使い方については後ほど示します。
4. 停止する
次に、モータの回転を停止する方法です。
SPIKEソフトでは以下のブロックにあたります。
停止系の関数は3つあるので、順に紹介します。
ストップ関数
まずはストップ関数です。
内部のコードを見てみると惰性で止まるための関数のようですが、実際に確認したところ即時停止します。
pbio_error_t pup_motor_stop(pup_motor_t *motor);
関数名はpup_motor_stopで、指定したポートの回転を停止します。
引数には「PUPモータデバイスポインタ」で停止したいモータのポートを指定します。
では使い方を例で確認しましょう。
Ex4-1. モータを1秒回転させる (ストップ関数)
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// パワーの設定
pup_motor_set_power(motorA, 50); // パワーを50%に設定
pup_motor_set_power(motorB, 50); // パワーを50%に設定
dly_tsk(1*1000*1000); // 1s 待機
// モータのストップ
pup_motor_stop(motorA); // モータを停止する
pup_motor_stop(motorB); // モータを停止する
}
「パワーの設定」の部分で、ポートA・Bのパワーを50%に設定し、1秒間の待機を入れた後、「モータのストップ」の部分で両モータを停止します。
ブレーキ関数
二つ目のブレーキ関数の方も紹介します。
こちらはモータにブレーキをかけて回転を停止する関数です。
pbio_error_t pup_motor_brake(pup_motor_t *motor);
関数名はpup_motor_brakeで、指定したポートモータにブレーキをかけて停止します。
引数には「PUPモータデバイスポインタ」で停止したいモータのポートを指定します。
では使い方を例で確認しましょう。
Ex4-2. モータを1秒回転させる (ブレーキ関数)
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// パワーの設定
pup_motor_set_power(motorA, 50); // パワーを50%に設定
pup_motor_set_power(motorB, 50); // パワーを50%に設定
dly_tsk(1*1000*1000); // 1s 待機
// モータにブレーキ
pup_motor_brake(motorA); // モータにブレーキをかける
pup_motor_brake(motorB); // モータにブレーキをかける
}
動きとしては先ほどと全く同じで、「パワーの設定」の部分で、ポートA・Bのパワーを50%に設定し、1秒間の待機を入れた後、「モータにブレーキ」の部分で両モータを停止します。
執筆時点で、「ストップ関数」と「ブレーキ関数」に違いは見られないので、使いたい方の関数を使えば良いかと思います。
ただし、アップデートにより仕様が変わる可能性はあるので注意してください。
角度維持関数
最後に、モータを停止した後に、その角度で維持し続ける関数を紹介します。
pbio_error_t pup_motor_hold(pup_motor_t *motor);
関数名はpup_motor_holdで、指定したポートの回転を停止し、停止したときの角度を維持し続けます。
引数には「PUPモータデバイスポインタ」で停止したいモータのポートを指定します。
こちらも、使い方を例で確認しましょう。
Ex4-3. モータを1秒回転させた後、角度を維持する
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// パワーの設定
pup_motor_set_power(motorA, 50);
pup_motor_set_power(motorB, 50);
dly_tsk(1*1000*1000); // 1s 待機
// 角度維持
pup_motor_hold(motorA);
pup_motor_hold(motorB);
dly_tsk(10*1000*1000); // 10s 待機
}
先ほどまでとほぼ同じで、「パワーの設定」の部分で、ポートA・Bのパワーを50%に設定し、1秒間の待機を入れた後、「角度維持」の部分で両モータを停止しますが、この後10秒間にわたり、停止した時の角度を維持し続けます。
角度を維持し続けている間は、モータを無理やり手で回したりしたとしても、元の位置に帰ってきます。
以上が、モータの回転を停止する関数です。
5. スピードを設定する
モータを回転する方法として「パワーの設定」を紹介しましたが、SPIKE-RTにはもう一つモータを回転させる方法があります。
それが「スピード関数」を用いる方法です。
pbio_error_t pup_motor_set_speed(pup_motor_t *motor, int speed);
関数名はpup_motor_set_speedで、指定したポートのモータスピードを設定します。
ここで言う「スピード」とは、「1秒間に何度回転するか」の割合です。
第一引数には「PUPモータデバイスポインタ」を入力してポートを指定、第二引数にはそのポートのモータのスピードを整数値で指定します。
では使い方を例で確認しましょう。
Ex5-1. モータを1秒回転させる (スピード関数)
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// スピードの設定
pup_motor_set_speed(motorA, 360); // 「1秒あたり360度」の回転率で回す
pup_motor_set_speed(motorB, 360); // 「1秒あたり360度」の回転率で回す
dly_tsk(1*1000*1000); // 1s 待機
// モータのストップ
pup_motor_brake(motorA); // モータにブレーキをかける
pup_motor_brake(motorB); // モータにブレーキをかける
}
「スピードの設定」の部分で、モータA・Bのスピードを「1秒あたり360度」の回転率に設定しています。
この状態で1秒間待機を入れると、モータは1秒後には360度、すなわち1回転します。
待機後にはブレーキをかけてモータを停止しています。
距離が分かっている特定の区間を、指定した時間で走行したい時などはこの関数を使うと良いかもしれません。
6. パワー値・スピード値を得る
先ほど、パワー値やスピード値を設定しモータを回転させましたが、今後は実際に出力されているパワー値やスピード値を取得してみましょう。
パワー値の取得
まずはパワー値の取得方法です。
int32_t pup_motor_get_power(pup_motor_t *motor);
関数名はpup_motor_get_powerで、指定したポートのモータパワーを取得します。
引数には「PUPモータデバイスポインタ」を入力しポートを指定します。
すると、戻り値として現在のパワー値が整数値で返されます。
ここで、パワー値は出力電圧ベースで計算されているようなので、バッテリー残量により変動があるものと思われます。
又、実際に「パワー50%」と設定したからといって、現在値も50%と出力されるということでもなさそうです。
簡単な確認方法だけ記しておきます。
Ex6-1. 現在のパワー値を確認する
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// パワーの設定
pup_motor_set_power(motorA, 50);
pup_motor_set_power(motorB, 50);
while(1){
int now_power_A = pup_motor_get_power(motorA); // 今のパワーを得る
hub_display_number(now_power_A); // ディスプレイに表示する
}
}
セットアップを行った後、モータA・Bのパワーを50%に設定します。
その後無限ループに入り、モータAの現在のパワー値を取得し、変数now_power_Aに格納します。
格納した後は、まだ紹介をしていないですがhub_display_number関数により、ハブのディスプレイに現在のパワー値を出力します。
私が試した際には、「パワーの設定」で50%を指定したものの、実際のパワー値は34%くらいでした。
スピード値の取得
続いて、スピード値の取得方法です。
int32_t pup_motor_get_speed(pup_motor_t *motor);
関数名はpup_motor_get_speedで、指定したポートのモータのスピードを取得します。
引数には「PUPモータデバイスポインタ」を入力しポートを指定します。
すると、戻り値として現在のスピード値が整数値で返されます。
こちらで得られるスピード値は、角度情報から計算しているものと思われ、スピードを360[°/秒]
と指定すれば、戻り値としても360付近の値が返ってきます。
こちらも、簡単な使い方を示しておきます。
Ex6-2. 現在のスピード値を確認する
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// スピードの設定
pup_motor_set_speed(motorA, 360); // 「1秒あたり360度」の回転率で回す
pup_motor_set_speed(motorB, 360); // 「1秒あたり360度」の回転率で回す
while(1){
int now_speed_A = pup_motor_get_speed(motorA); // 今のパワーを得る
hub_display_number(now_speed_A/10); // ディスプレイに表示する
}
}
先ほど同様、セットアップを行った後、モータA・Bのスピードを50%に設定します。
その後無限ループに入り、モータAの現在のスピード値を取得し、変数now_speed_Aに格納します。
そして、hub_display_number関数により、ハブのディスプレイに現在のスピード値を出力します。ただし、hub_display_number関数は2ケタまでしか表示できないことから、1/10倍して表示しています。
実際に試してみたところ、回転の途中にモータAの回転を手で止めたりすると、値が0になることが確認できました。
よって、リアルタイムでのスピードが正しく取得出来ているように思われます。
7. 角度を得る
SPIKE用のモータには「ロータリーエンコーダ」という回転角度を測るセンサが内蔵されています。
これにより回転角度を指定した回転命令などが出来るわけですが、一つのセンサとして値を取得することも出来ます。
回転角度のリセット
角度を取得したい時は、取得関数実行前に回転角度のリセットをしておきましょう。
角度を求めるには基準点が必要だからです。
pbio_error_t pup_motor_reset_count(pup_motor_t *motor);
関数名はpup_motor_reset_countで、指定したポートのモータの回転角をリセットします。
引数には「PUPモータデバイスポインタ」を入力しポートを指定します。
これにより、回転角をリセットし、関数実行時からどれだけ回転したかを次の回転角取得関数で求めることが出来るようになりました。
尚先述のように、pup_motor_setup関数を実行する時にも、第三引数にtrueを指定すると回転角はリセットされます。
回転角度を取得する
続いて、回転角度を取得する関数です。
SPIKEソフトでは以下のブロックにあたります。
ただし、SPIKE Primeのモータおよび標準ソフトでの特徴的な機能である「絶対位置」とは異なります。
int32_t pup_motor_get_count(pup_motor_t *motor);
関数名はpup_motor_get_countで、指定したポートのモータの回転角を取得します。
引数には「PUPモータデバイスポインタ」を入力しポートを指定します。
すると、戻り値として現在の回転角が整数値で返されます。
では、これら二つの関数を用いて、モータを指定の回転角まで回す制御を行ってみましょう。
Ex7-1. 指定の回転角までモータを回転させる
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
pup_motor_reset_count(motorA); // モータA の角度をリセット
while(abs(pup_motor_get_count(motorA)) < 500){
// パワーの設定
pup_motor_set_power(motorA, 50);
pup_motor_set_power(motorB, 50);
}
// モータのストップ
pup_motor_brake(motorA); // モータにブレーキをかける
pup_motor_brake(motorB); // モータにブレーキをかける
}
モータのセットアップを行った後、モータAの角度をリセットします。
(pup_motor_setup関数によりリセットされているので、今回の場合はpup_motor_reset_count関数は無くても動きます。)
その後、モータAの角度の絶対値(abs関数)が、500より小さい間という条件でループを行います。ループ中はモータパワーを50%に設定しています。
モータAの角度の絶対値が500を超えるとループを抜け、モータにブレーキがかかります。
これにより、「モータA(およびB)を500度回転させる」という制御が出来るようになりました。
8. その他機能
ここからは、SPIKE-RTのAPIとして用意されているこの他の機能(関数)について紹介していきます。
ストールしているか
モータがストール、つまり回転の指示を与えたにも関わらず何らかの原因で回転がストップしているかを調べる事が出来ます。
例えば、壁にぶつかっていてモータが回転しない、といったシチュエーションなどです。
bool pup_motor_is_stalled(pup_motor_t *motor);
関数名はpup_motor_is_stalledで、指定したポートがストールしているかを取得します。
引数には「PUPモータデバイスポインタ」を入力しポートを指定します。
モータがストールしている場合、戻り値としてTrueが返ってきます。
これを用いた簡単な例を以下に示します。
Ex8-1. モータがストールしているか取得する
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
// パワーの設定
pup_motor_set_power(motorA, 50);
pup_motor_set_power(motorB, 50);
while(1){
if(pup_motor_is_stalled(motorA)){
hub_light_on_color(PBIO_COLOR_RED); // ボタンのLEDを赤にする
}
else if(pup_motor_is_stalled(motorB)){
hub_light_on_color(PBIO_COLOR_YELLOW); // ボタンのLEDを黄にする
}
else{
hub_light_on_color(PBIO_COLOR_GREEN); // ボタンのLEDを緑にする
}
}
}
モータのセットアップ後、両モータにはパワー50%を与えています。
無限ループに入った後、モータAおよびBがストールしているかを調べます。
モータAがストールしていた場合、hub_light_on_color関数によりハブブロックのボタンのLEDを赤色に、モータBがストールしていた場合黄色に、どちらもストールしていない場合は緑色にします。
実際に動かして、モータを手で止めると分かりやすいかと思います。
デューティ比のリミットの設定
デューティとは一般に、モータのPWM制御で電圧がかかっている時間の事を言いますが、ここでは簡単に「モータパワー」と同値であると考えてください。
このデューティについて、最大値を設定することが出来ます。
その設定値を超えるパワーは出力されないということになります。
int32_t pup_motor_set_duty_limit(pup_motor_t *motor, int limit);
関数名はpup_motor_set_duty_limitで、指定したポートのデューティの最大値を設定します。
第一引数には「PUPモータデバイスポインタ」を入力しポートを指定、第二引数にはデューティの最大値を0~100の整数値で設定します。
戻り値として、現在のデューティの最大値(ただし電圧ベース)が返ってきます。
デューティの最大値を元に戻す
変更したデューティ値は元に戻すことが出来ます。
void pup_motor_restore_duty_limit(pup_motor_t *motor, int old_value);
関数名はpup_motor_restore_duty_limitで、指定したポートのデューティの最大値を元に戻します。
第一引数には「PUPモータデバイスポインタ」を入力しポートを指定、第二引数には元のデューティの最大値を指定します。
この第二引数には、先ほどのpup_motor_set_duty_limit関数の戻り値で得られた値を用います。
では、これらの使い方を示し、およびデューティの最大値が下がっているかの確認も行いたいと思います。
Ex8-2. デューティの最大値の変更
// PUPモータデバイスポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;
void Main(intptr_t exinf)
{
// PUPモータデバイスポインタを取得
motorA = pup_motor_get_device(PBIO_PORT_ID_A);
motorB = pup_motor_get_device(PBIO_PORT_ID_B);
// モータのセットアップ
pup_motor_setup(motorA, PUP_DIRECTION_COUNTERCLOCKWISE, true);
pup_motor_setup(motorB, PUP_DIRECTION_CLOCKWISE, true);
int i;
for(i=0; i<=100; i++){
// パワーの設定
pup_motor_set_power(motorA, i);
pup_motor_set_power(motorB, i);
hub_display_number(i);
dly_tsk(200*1000);
}
// モータの停止
pup_motor_stop(motorA);
pup_motor_stop(motorB);
// モータのデューティ値を下げる
int old_val_A = pup_motor_set_duty_limit(motorA, 50);
int old_val_B = pup_motor_set_duty_limit(motorB, 50);
dly_tsk(1*1000*1000);
for(i=0; i<=100; i++){
// パワーの設定
pup_motor_set_power(motorA, i);
pup_motor_set_power(motorB, i);
hub_display_number(i);
dly_tsk(200*1000);
}
// モータの停止
pup_motor_stop(motorA);
pup_motor_stop(motorB);
// モータのデューティ値を戻す
pup_motor_restore_duty_limit(motorA, old_val_A);
pup_motor_restore_duty_limit(motorB, old_val_B);
dly_tsk(1*1000*1000);
for(i=0; i<=100; i++){
// パワーの設定
pup_motor_set_power(motorA, i);
pup_motor_set_power(motorB, i);
hub_display_number(i);
dly_tsk(200*1000);
}
exit(0);
}
この例では、for文を用いてのモータの加速を3回行っています。
このうち、2回目の加速の前にはpup_motor_set_duty_limit関数によりデューティの最大値を50%に制限、3回目の加速の前にはpup_motor_restore_duty_limit関数によりデューティの最大値を元に戻しています。
実行してみると、2回目の加速のときのみ、ハブディスプレイに写るパワー値が50を超えたあと加速しなくなることが分かります。(モータの音などで確認してみてください。)
9. まとめ
今回はモータの使用方法について解説していきました。
ロボットプログラミングの基本中の基本ですので、ぜひマスターしていきましょう。
次回は、こちらもロボットには欠かせないカラーセンサの使い方について解説していきたいと思います。