SG90サーボ　2軸カメラジンバルのテスト制御

# カメラトラッキングに使えそうなので購入してみました。ジンバル用カメラ台。
<iframe width="560" height="315" src="https://www.youtube.com/embed/XB6zazuCQKY?si=0plXTMBVtu7VQVj6" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

![IMG_5170.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/96b781c9-ea97-56a2-7bf9-fa530e3d8043.jpeg)

<img width="400" alt="SG90" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/411d26a9-7d60-17cd-f3ea-19332f7e2c68.jpeg">

:::note 
SG90 360°回転サーボ　2軸ジンバル
Aliexpress で700円ぐらい。
180度回転サーボだと思ったら、360度回転型でした。
:::

:::note warn
# 組み立て
<img width="300" alt="取り付け１" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/4d9ec877-8a42-26c6-3071-a73c7e724cda.jpeg">

<img width="300" alt="底面固定具.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/39ac914b-503f-e6e5-2a1b-35e7d2b13cda.jpeg">


白い固定具のプラスティックが、付属品で同梱されていますが、そのままでは、取り付けられません。
ニッパーと、カッターと、やすりで、本体の取り付けサイズに合わせて、固定部品を加工しました。
ネジは、一番小さいもので固定しました。

組み立てに必要な、ネジは数種類あります。選択に迷うことはないとおもいます。

<img width="300" alt="ネジ用途.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/a8531afb-6fa7-11e5-5a24-29a179b2a170.jpeg">

使わなかったネジもありました。
それほど、時間はかからないと思います。

:::

# SG90 360度回転　PWM制御タイミング
![SG90_360duty比.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/8fcf244c-3312-044b-d531-c3ac44723812.jpeg)

SG90のラベルを見て、180度回転だと思い込んでしまったまま、コーディング。
動かしてみると、360度回転するので、間違えに気が付きました。

ただ、CW、CCW回転、そして停止は、勘違いコードのまま、2つのボリューム(可変抵抗値)を上げ下げすると、回転したり、停止したりしたので、そのままで、しばらく様子をみました。

動かしているうちに、データシートを観なくても何とかなりそうだったので、実際に動かしてみて、停止するON時間、CW,CCWするON時間を調べました。

最初は、ボリュームをQ43マイコンのRA0,RA1につなぎ、ADC値を使って、デューティー比を変化させ、回転の様子をみてみました。
![ADC調整.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/726478e7-eb46-620f-6ae6-efb622d6c1e8.jpeg)



同時に、ON時間をLCDに表示させてみて、上図のようなON時間で動いていることを確認しました。

:::note warn
停止のON時間の幅が、かなり微妙なので、
CW、CCWのON時間を1200us、1350us、停止を1280usと固定しました。
これで、”定速”でサーボを動かせます。
:::

:::note
**サーボは、スーファミコンコントローラの十字キーで、上下、CW、CCW旋回をさせています。**
:::

```C:サーボ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信号の作成

![SG90_360duty比.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/8fcf244c-3312-044b-d531-c3ac44723812.jpeg)

さて、この**20msのだいぶ長い周期をつくるPWMモジュールを搭載しているマイコンがなかなかない**、という問題があります。

今回は、できるだけ簡単に実装したかったので、10usでカウントアップするTimerモジュールを２つ使用して、上記波形をつくりました。

Q43のTimer0とTimer3を使用します。10usインターバルを作り出せるタイマーならOKです。

タイマーモジュールは、どのマイコンにも搭載されているので、18F27Q43以外のマイコンでも動かせます。

:::note warn
インターバルは、20msきっちり取らなくても動作しました。最終的に3msインターバルで動かしています。
:::

```C: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;
}

```

```C:サーボ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;
    }
}

```

```C:サーボ制御用変数 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 */
```
```C:SG90_360.c
#include "SG90.h"

volatile _sg90 SG90[2];
```

ピックマイコン(16F18326,16F18857etc..）だと、CLCロジックセルを使ってサーボモータを駆動する方法もあるのですが、こちらのほうが簡単でした。

スーファミコントローラでサーボを動かしています。
信号線が3本だけで、なおかつ、ただのIOピンの入出力だけをつかい、ペリフェラルモジュールを使用しないで動いてくれるので、どんなマイコンでも動きます。

以下、スーファミコントローラの資料の記事。

https://qiita.com/etoolsLab369/items/88e2a0c690d2798b2f97

https://qiita.com/etoolsLab369/items/5452540e2a12386beda9

```C: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 */
```
```C: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;
}

```