サッカーロボ解体新書第二弾!

2014/11/11 0:56 に 山田憲 が投稿
今回はサッカーロボに使ったプログラムの内容を解説していきたいと思います。
(cppファイルとそれを左右マイコン用にコンパイルしたhexファイルを添付しています)

#define F_CPU    9600000UL

#include <avr/io.h>
#include <util/delay.h>

#define RIGHT        0
#define LEFT        1

#define M_TYPE        LEFT      //書き込むマイコンの種類(RIGHT=右モーターマイコン LEFT=左モーターマイコン)

#define  ROOM          25           //ジョイスティックの中心と判定する範囲


#define TYPE_X        0
#define TYPE_Y        1

int8_t joyS(uint8_t type);                 //ジョイスティックの情報読み込み。戻り値に-128~128の値を挿入(誤差がなければ理論上中心=0)
int8_t joy_pos[2];                             //ジョイスティックの情報保存用変数。1ループ毎にjoyS関数を使って更新する。
int8_t joy_pos_abs [2];                    //ジョイスティックの絶対値を格納するための配列

int main(void)
{
    DDRB=0b00000111;             //出力をPB0、PB1、PB2に設定。その他は入力
   

    TCCR0A = 0b10000011;       //PB0にPWMを設定
    TCCR0B = 0b00000011;       //64分周に設定
  
    while(1)
    {
        //joyS関数を使って更新する。
        joy_pos[TYPE_X]=joyS(TYPE_X);
        joy_pos[TYPE_Y]=joyS(TYPE_Y);
        /*-128~127を-127~127になおす*/
        if(joy_pos[TYPE_X]<0)
        joy_pos[TYPE_X]++;
        if(joy_pos[TYPE_Y]<0)
        joy_pos[TYPE_Y]++;

        //取得したjoy_posをもとにMDICを制御
        if((joy_pos[TYPE_X]<ROOM)&&(joy_pos[TYPE_X]>-ROOM)&&(joy_pos[TYPE_Y]<ROOM)&&(joy_pos[TYPE_Y]>-ROOM))
        {
                 /*ジョイスティックが中心にある場合*/
                PORTB=_BV(PB2) | _BV(PB1);
                OCR0A=255;
               
        }else{

                /*joy_posの絶対値を取る*/

            if(joy_pos[TYPE_X]<0)
                joy_pos_abs [TYPE_X]=-joy_pos[TYPE_X];
                else
                joy_pos_abs [TYPE_X]= joy_pos[TYPE_X];
            if(joy_pos[TYPE_Y]<0)
                joy_pos_abs [TYPE_Y]=-joy_pos[TYPE_Y];
                else
                joy_pos_abs [TYPE_Y]= joy_pos[TYPE_Y];

            if(joy_pos[TYPE_X]<0)
            joy_pos_abs[TYPE_X]--;
            if(joy_pos[TYPE_Y]<0)
            joy_pos_abs[TYPE_Y]--;      
     

            /*
            //操作性の都合によりこのロボットは最高速度を封印してあります。
            //それでもこのロボットの真のパワーを見たい方はここを開放してください
            joy_pos_abs[TYPE_Y]*=2;
            joy_pos_abs[TYPE_X]*=2;
            */



            if(joy_pos_abs[TYPE_X]<=joy_pos_abs[TYPE_Y])//縦軸絶対値が横軸絶対値より大きい時
            {
                //前進or後退処理
                if(joy_pos[TYPE_Y]>0)
                {
                    //前進処理(両モーター前転)
                    PORTB=_BV(PB1);
                  //↑イコールでピンを設定すれば排他的にPB2を0にしてくれるため2行書くコストを省略できる(PB0はPWMとして設定してるので影響を受けない)

                }else{
                     //後退処理(両モーター後転)
                    PORTB=_BV(PB2);
                }
                OCR0A=joy_pos_abs[TYPE_Y];//PWMでモータースピードを指定
            }else{//縦軸絶対値が横軸絶対値より小さい時
                if(joy_pos[TYPE_X]>0)
                {
                    //右折処理(左モーター前転右モーター後転)
                    if(M_TYPE==RIGHT)
                        PORTB=_BV(PB2);
                    else
                        PORTB=_BV(PB1);

                }else{

                    //左折処理(左モーター後転右モーター前転)
                    if(M_TYPE==RIGHT)
                        PORTB=_BV(PB1);
                    else
                        PORTB=_BV(PB2);
                }
                OCR0A=joy_pos_abs[TYPE_X];//PWMでモータースピードを指定
               
            }
        }
       
    }
    return 0;
}
int8_t joyS(uint8_t type)//ジョイスティックの値をAD変換で読み取る
{   
    if(type==M_TYPE)//オーダーが縦軸値の場合
    ADMUX = 3 | _BV(ADLAR); // 電圧基準はVCC。PB3に設定|ビット左揃え適用|横軸入力
    else                        //オーダーが横軸値の場合
    ADMUX = 2 | _BV(ADLAR); // 電圧基準はVCC。PB4に設定|ビット左揃え適用|縦軸入力
   

    ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF)|_BV(ADPS2)|_BV(ADPS1);
    loop_until_bit_is_clear(ADCSRA,ADSC);//AD変換待ち

     /*0~255のAD変換値を-128~127に変えて返す*/
    if(type==TYPE_X)//オーダーが縦軸値の場合
    return  127-ADCH;
    else                         //オーダーが横軸値の場合
    return  ADCH-128;
}
ċ
scrobo(left).hex
(1k)
山田憲,
2014/11/11 0:56
ċ
scrobo(right).hex
(1k)
山田憲,
2014/11/11 0:56
ċ
山田憲,
2014/11/11 0:56
Comments