おびやまロボット研究所

HOME > センサ・エンコーダ

　センサ・エンコーダ　

今日は，超音波センサの動作確認->OK
ジャイロの動作確認->調整必要
サーボ動作確認->ちょっとバタつく，どうする？
GPS読み込み->ソフト的な不具合見つかり修正途中（手間取りそう）

ジャイロの動作について，ジャイロ取り込みの調整できた．要は，８ビットや１６ビット単位で丸めるのではなく，A/Dの誤差も含めて最大のビット長でとって積算しながらデータを持っておき，必要な時だけ丸めて使うえばいいようだ．
サーボ動作について，ばたつく原因究明はあと回しにして，各サーボに個別に信号を与えることができるテストプログラムを入れて稼動範囲とセンターの値を調べた．
GPS読み込みの不具合は，分かってしまえば単純なミス，
・タイマー割込み中で定義したローカル変数をカウンタに使っておかしくなってた
・LCD表示に適当に待ちを入れてやらないと表示が狂う
・文字列->数値化関数の処理に時間がかかってて，割込み中に入れると負荷が大　きすぎた

LCDのメニューがほぼ完成．
メニュー構造を単純化することでボタン２つでロボットの設定・動作が操作できた．
黄色：次へ，　青：実行
メインメニュー中で黄色ボタンでメインメニュー切り替え，
青ボタンで表示されたメインメニュー中のサブメニューへ．
サブメニュー中でも黄色ボタンで次のサブメニューへ．
青ボタンで表示されているサブメニューの処理．
学生さんに頼んでいたボディが完成．基板をのせて写真を撮ってみた↓

1mmアルミ板でボディの加工，スペーサで基板をのせ，その前にGPSモジュールを両面テープで貼り付け（予定），先頭には超音波センサ，サーボで首振り可能．
GPSの左側の支柱部分に垂直にジャイロ（KRG-3）貼り付け．
あと，ロータリーエンコーダを作って前輪に仕込んだら想定しているハードウェアは完成．

今日は家にいないといけない用事があったので，自宅でプログラム作成．
IRQ割込みでエンコーダのソフトウェアカウントのプログラム作成．
といってもカウンタ本体は数行なのですぐできた．
前進/バックのフラグの値でアップ/ダウンカウントするだけ．
動作検証プログラムも作ってマイコンに送って動作確認．
ただし，エンコーダは正負の値を取っていいように書いてるので，貧弱な表示ルーチンではエンコーダが負の値になったとき暴走する．でもメモリ足りないので表示に凝る必要なし．

プログラム

こんな感じ↓

//-------------------------------------------------
//	IRQ割り込み処理 (IRQ2:右前輪，IRQ3:左前輪)    		                                     
//-------------------------------------------------
void int_irq2(void){                  // 右前輪エンコーダ処理
  DI;
  IRR1.BIT.IRRI2=0; IENR1.BIT.IEN2=0; // IRQ2割込み禁止 
  if (fFR==0) encR++; else encR--;     // フラグでエンコーダカウントアップ/ダウン切り替え
  IENR1.BIT.IEN2=1;                   // IRQ2割込み許可 
  EI;
}
void int_irq3(void){                  // 左前輪エンコーダ処理
  DI;
  IRR1.BIT.IRRI3=0; IENR1.BIT.IEN3=0; // IRQ3割込み禁止 
  if (fFR==0) encL++; else encL--;     // フラグでエンコーダカウントアップ/ダウン切り替え
  IENR1.BIT.IEN3=1;                   // IRQ3割込み許可 
  EI;
}


//----------------------------------------------- 
//	エンコーダ動作チェック	       
//----------------------------------------------- 
void check_encoder(void){
  LCD_clear();
  LCD_disp("CHECK ENCODER",1);
  LCD_disp("    L:    R:    ",2);
  while (SW_START==1 && SW_SET==1){
    LCD_locate(2,0);
    if(DIP_SW==4 || DIP_SW==0) 
          {fFR=0; LCD_msg("FWD");} 
    else  {fFR=1; LCD_msg("REV");}
    LCD_locate(2, 6); LCD_10(encL);
    LCD_locate(2,12); LCD_10(encR);     
    pause(100); 
  }
}


※割込みの部分は，「ONOの電脳壁新聞」様の以下のURLを参考にしました．
http://0n0.fc2web.com/h83664/irq/irq01.html