久しぶりにraspberry piを動かしてみる。
今回はサーボモーター制御に挑戦。
ちょっと実験死体だけなのでサーボモーターは安いものを
物色した結果、Amazonから
SG90を購入。
2個で\980 でとってもリーズナブル。
サーボモーターは信号のONの時間により位置合わせをしてくれるモーターで、
今回のSG90は 回転角 0°～180°で対応する信号幅は
0.5ms～2.5msである。
本当はPWMを使うといいのだが、とりあえずソフトウェアでGPIOを叩いて
制御してみることにする
前にLED点滅制御で作ったプログラムをちょっと変更して
作ってみる
なんか動きがぎこちないがとりあえず思ったとおりに動いた。
MotionのWebカメラ制御と組み合わせて、ネットワーク越しに
カメラの向きを制御できると面白そうだ。
ソース・コード
------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
// レジスタブロックの物理アドレス
#define PERI_BASE 0x20000000
#define GPIO_BASE (PERI_BASE + 0x200000)
#define BLOCK_SIZE 4096
// ピン機能（BCM2835）
#define GPIO_INPUT 0x0 // 入力
#define GPIO_OUTPUT 0x1 // 出力
#define GPIO_ALT0 0x4
#define GPIO_ALT1 0x5
#define GPIO_ALT2 0x6
#define GPIO_ALT3 0x7
#define GPIO_ALT4 0x3
#define GPIO_ALT5 0x2
// gpio[n]: GPIO 関連レジスタ (volatile＝最適化によって消滅させない→必ず実メモリにアクセスさせる)
static volatile unsigned int *gpio;
// gpio_init: GPIO 初期化（最初に１度だけ呼び出すこと）
void gpio_init ()
{
int fd;
void *gpio_map;
// /dev/mem（物理メモリデバイス）を開く（sudo が必要）
fd = open("/dev/mem", O_RDWR | O_SYNC);
if (fd == -1) {
printf("error: cannot open /dev/mem (gpio_setup)\n");
exit(-1);
}
// mmap で GPIO（物理メモリ）を gpio_map（仮想メモリ）に対応づける
gpio_map = mmap(NULL, BLOCK_SIZE,
PROT_READ | PROT_WRITE, MAP_SHARED,
fd, GPIO_BASE );
if ((int) gpio_map == -1) {
printf("error: cannot map /dev/mem on the memory (gpio_setup)\n");
exit(-1);
}
// mmap 後は不要な fd をクローズ
close(fd);
// gpio[index]: 整数 uint32 の配列としてレジスタへのアクセスを確立
gpio = (unsigned int *) gpio_map;
}
// gpio_configure: ピン機能を設定する（ピンを使用する前に必ず設定）
// pin : (P1) 2,3,4,7,8,9,10,11,14,15,17,18,22,23,24,25,27
// (P5) 28,29,30,31
// mode: GPIO_INPUT, _OUTPUT, _ALT0, _ALT1, _ALT2, _ALT3, _ALT4, _ALT5
void gpio_configure (int pin, int mode)
{
// ピン番号チェック
if (pin < 0 || pin > 31) {
printf("error: pin number out of range (gpio_configure)\n");
exit(-1);
}
// レジスタ番号（index）と３ビットマスクを生成
int index = pin / 10;
unsigned int mask = ~(0x7 << ((pin % 10) * 3));
// GPFSEL0/1 の該当する FSEL (3bit) のみを書き換え
gpio[index] = (gpio[index] & mask) | ((mode & 0x7) << ((pin % 10) * 3));
}
// gpio_set/clear: ピンをセット (3.3V)，クリア (0V)
void gpio_set (int pin)
{
// ピン番号チェック（スピードを追求するなら省略してもよい）
if (pin < 0 || pin > 31) {
printf("error: pin number out of range (gpio_set)\n");
exit(-1);
}
// ピンに１を出力（3.3V 出力）
gpio[7] = 0x1 << pin; // GPSET0
}
void gpio_clear (int pin)
{
// ピン番号チェック（スピードを追求するなら省略してもよい）
if (pin < 0 || pin > 31) {
printf("error: pin number out of range (gpio_clear)\n");
exit(-1);
}
// ピンに０を出力（0V 出力）
gpio[10] = 0x1 << pin; // GPCLR0
}
#define SERVO01 17
#define SERVO02 18
#define INTERVAL_TIME 22000
#define SERVO_MOVE_MAX 180
#define SERVO_DEGREE_0_TIME 500
static void servo(unsigned long int PinNo,unsigned long int Angle);
static void servo(unsigned long int PinNo,unsigned long int Angle)
{
unsigned long int Time_H;
unsigned long int Time_L;
/* 制御角度は0～180度に限定 */
Angle = Angle % (SERVO_MOVE_MAX+1);
Time_H = SERVO_DEGREE_0_TIME+((Angle*2000)/SERVO_MOVE_MAX);
Time_L = INTERVAL_TIME - Time_H;
printf("PinNo(%02d):Time:%09d[us]\n",PinNo,Time_H);
gpio_set(PinNo);
usleep(Time_H);
gpio_clear(PinNo);
usleep(Time_L);
}
#define WAIT_TIME (2*1000*1000)
int main ()
{
signed long int j;
gpio_init(); // オマジナイ
gpio_configure(SERVO01, GPIO_OUTPUT);
gpio_configure(SERVO02, GPIO_OUTPUT);
/*
[INITIALIZE]
角度0℃に設定
*/
printf("TEST000\n");
for(j=0;j<10;j++){
servo(SERVO01,0);
servo(SERVO02,0);
}
usleep(WAIT_TIME);
while (1) {
/*
0->90へ移動
*/
printf("TEST 0->90\n");
for( j=0;j<=90;j=j+2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);
/*
90->180へ移動
*/
printf("TEST 90->180\n");
for( j=90;j<=180;j=j+2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);
/*
180->90へ移動
*/
printf("TEST 180->90\n");
for( j=180;j>=90;j=j-2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);
/*
90->0へ移動
*/
printf("TEST 90->0\n");
for( j=90;j>0;j=j-2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);
}
return 0;
}
------------------------------------------------------------------
