カメラトラッキングに使えそうなので購入してみました。ジンバル用カメラ台。
SG90 360°回転サーボ 2軸ジンバル
Aliexpress で700円ぐらい。
180度回転サーボだと思ったら、360度回転型でした。
SG90 360度回転 PWM制御タイミング
SG90のラベルを見て、180度回転だと思い込んでしまったまま、コーディング。
動かしてみると、360度回転するので、間違えに気が付きました。
ただ、CW、CCW回転、そして停止は、勘違いコードのまま、2つのボリューム(可変抵抗値)を上げ下げすると、回転したり、停止したりしたので、そのままで、しばらく様子をみました。
動かしているうちに、データシートを観なくても何とかなりそうだったので、実際に動かしてみて、停止するON時間、CW,CCWするON時間を調べました。
最初は、ボリュームをQ43マイコンのRA0,RA1につなぎ、ADC値を使って、デューティー比を変化させ、回転の様子をみてみました。
同時に、ON時間をLCDに表示させてみて、上図のようなON時間で動いていることを確認しました。
停止のON時間の幅が、かなり微妙なので、
CW、CCWのON時間を1200us、1350us、停止を1280usと固定しました。
これで、”定速”でサーボを動かせます。
サーボは、スーファミコンコントローラの十字キーで、上下、CW、CCW旋回をさせています。
サーボPWMのON時間を変えているコード
famiconAnswer = getButton();//ファミコンのボタン押下状況の取得。
if(famiconAnswer!=0xFFF0)//何か押されていたら、サーボを動かす。
{
//上のサーボ用----------------------------
if((famiconAnswer&0x0800)==0x0)
{//十字キーの上
//printf("UP ");
SG90[0].countTerm = 120;//1200usにセット
}
if((famiconAnswer&0x0400)==0x0)
{//十字キーの下
//printf("DOWN ");
SG90[0].countTerm = 135;//1350usにセット
}
//下のサーボ用---------------------------
if((famiconAnswer&0x0200)==0x0)
{//十字キーの左
//printf("LEFT ");
SG90[1].countTerm = 135;//1350usにセット
}
if((famiconAnswer&0x0100)==0x0)
{//十字キーの右
//printf("RIGHT ");
SG90[1].countTerm = 120;//1200usにセット
}
}else{//なにも押されていないとき
//停止
SG90[0].countTerm=128;//1280usにセット
SG90[1].countTerm=128;
}
__delay_ms(3);//これがないと、速く更新されすぎて、がたつく。
SG90のPWM信号の作成
さて、この20msのだいぶ長い周期をつくるPWMモジュールを搭載しているマイコンがなかなかない、という問題があります。
今回は、できるだけ簡単に実装したかったので、10usでカウントアップするTimerモジュールを２つ使用して、上記波形をつくりました。
Q43のTimer0とTimer3を使用します。10usインターバルを作り出せるタイマーならOKです。
タイマーモジュールは、どのマイコンにも搭載されているので、18F27Q43以外のマイコンでも動かせます。
インターバルは、20msきっちり取らなくても動作しました。最終的に3msインターバルで動かしています。
18F27Q43 Fosc=32Mhz 10usインターバルタイマーの初期化
//*****************************************************//
//Timer0初期化
//*****************************************************//
_tm tm0;
void timer0Init(void)
{
//Timer0初期化--------------------------------------------------
T0CON1bits.CS=0b010;//クロックソースFosc/4
T0CON1bits.CKPS=0b0011;//プリスケ1:8
T0CON0bits.MD16=1;//16bitsタイマー
T0CON0bits.OUTPS=0b0000;//ﾎﾟｽﾄｽｹｰﾗ1:1
TMR0H=0xFF;//10us Fosc=32Mhz
TMR0L=0xF6;//10us
T0CON0bits.EN=1;
PIE3bits.TMR0IE=1;
PIR3bits.TMR0IF=0;
}
/*---------------------------------------------------
Timer3
---------------------------------------------------*/
_tm tm3;
void timer3Init(void)
{
T3CONbits.CKPS=0b11; //1:8
T3CONbits.RD16=1;//16bits.Timer
T3CLK=0b00001;//Fosc/4
TMR3H=0xFF;
TMR3L=0xF6;
T3CONbits.ON=1;
PIR5bits.TMR3IF=0;
PIE5bits.TMR3IE=1;
}
サーボPWM波をタイマー割り込み内で作成
//****************割り込み関数***********************//
// Timer0割込み
//**************************************************//
void __interrupt(irq(IRQ_TMR0)) Timer0_ISR(void)
{
PIR3bits.TMR0IF=0;
SG90[0].countTotal++;
TMR0L=0xF6;//10us:Timer0カウントアップ時間
if(SG90[0].countTotal== SG90[0].countTerm)
{//ON時間の終了
LATBbits.LATB0=0;
}
if(SG90[0].countTotal==2000)
{//一周期終了
SG90[0].countTotal=0;
LATBbits.LATB0=1;//ON開始
}
}
//****************割り込み関数***********************//
// Timer3割込み
//**************************************************//
void __interrupt(irq(IRQ_TMR3)) Timer3_ISR(void)
{
PIR5bits.TMR3IF=0;
SG90[1].countTotal++;
TMR3L=0xF6;//10us:Timer3カウントアップ時間
if(SG90[1].countTotal== SG90[1].countTerm)
{
LATBbits.LATB2=0;
}
if(SG90[1].countTotal==2000)
{//一周期終了
SG90[1].countTotal=0;
LATBbits.LATB2=1;
}
}
サーボ制御用変数 SG90_360.h
#ifndef SG90_H
#define SG90_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
//SG90_360用構造体
typedef struct{
uint16_t countTotal;//長周期カウント(単位：10us)
uint16_t countTerm;//ON時間をセット(単位：10us)
}_sg90;
extern volatile _sg90 SG90[2];
#ifdef __cplusplus
}
#endif
#endif /* SG90_H */
SG90_360.c
#include "SG90.h"
volatile _sg90 SG90[2];
ピックマイコン(16F18326,16F18857etc..）だと、CLCロジックセルを使ってサーボモータを駆動する方法もあるのですが、こちらのほうが簡単でした。
スーファミコントローラでサーボを動かしています。
信号線が3本だけで、なおかつ、ただのIOピンの入出力だけをつかい、ペリフェラルモジュールを使用しないで動いてくれるので、どんなマイコンでも動きます。
以下、スーファミコントローラの資料の記事。
famicon.h
#ifndef FAMICON_H
#define FAMICON_H
#ifdef __cplusplus
extern "C" {
#endif
#include <xc.h>
#include "Q_peripheral27Q43.h"
//PS:オレンジ
//CLK:黄色
//DATA1:赤
//この信号線のディファイン定義さえ変えてしまえば、どのPICマイコンでも簡単に動きます。
//arduinoは、digitalWriteにかえる必要がありそうですが。
#define PS PORTAbits.RA3
#define CLK PORTAbits.RA4
#define DATA1 PORTAbits.RA5
extern uint16_t famiconButtonState;
extern void famiconInit(void);
extern uint16_t getButton(void);
#ifdef __cplusplus
}
#endif
#endif /* FAMICON_H */
famicon.c
#include "famicon.h"
uint16_t famiconButtonState;//button押下状況を格納
///@brief コントローラーの初期化
void famiconInit(void)
{
TRISAbits.TRISA5=1;//DATA1線だけは、入力設定。
}
///@brief ボタン押下状況取得関数
///return ボタン押下16ビットデータ
uint16_t getButton(void)
{
uint16_t w_val;
uint8_t cnt;
w_val=0x0000;
cnt=0;
//最初の1ビット
PS=1;
CLK=1;
__delay_us(10);
PS=0;
CLK=0;
__delay_us(10);
w_val|=DATA1;
w_val<<=1;
cnt++;
//残り15ビットの取得
do{
CLK=1;
__delay_us(10);
CLK=0;
// __delay_us(5);
w_val|=DATA1;
if(cnt!=15)
{
w_val<<=1;
}
cnt++;
}while(cnt!=16);
//受信完了
w_val&=0xFFF0;//ボタンデータのみを、抽出
/*if(w_val!=0xFFF0)
{
if((w_val&0x8000)==0x0)
{
printf("B ");
}
if((w_val&0x4000)==0x0)
{
printf("Y ");
}
if((w_val&0x2000)==0x0)
{
printf("SELECT ");
}
if((w_val&0x1000)==0x0)
{
printf("START ");
}
if((w_val&0x0800)==0x0)
{
printf("UP ");
}
if((w_val&0x0400)==0x0)
{
printf("DOWN ");
}
if((w_val&0x0200)==0x0)
{
printf("LEFT ");
}
if((w_val&0x0100)==0x0)
{
printf("RIGHT ");
}
if((w_val&0x0080)==0x0)
{
printf("A ");
}
if((w_val&0x0040)==0x0)
{
printf("X ");
}
if((w_val&0x0020)==0x0)
{
printf("L ");
}
if((w_val&0x0010)==0x0)
{
printf("R ");
}
printf("%04x\r",w_val);
}*/
return w_val;
}