「StickController Ver0.1」の編集履歴（バックアップ）一覧はこちら

「StickController Ver0.1」(2013/06/25 (火) 09:44:06) の最新版変更点

追加された行は緑色になります。

削除された行は赤色になります。

*スティックコントローラーの機能
-スティックを倒した方向に応じて、2つのキャタピラー出力に変換し無線送信する
-スティックを押す事で超信地旋回するモードとしないモードに切り替わる(超信地旋回するモードの場合には緑LEDが点灯)
-車体から送られたデータを読み込み、値によって赤LEDを点灯する
-一定時間データが送られてこない場合通信エラーなので赤LEDが点滅する

*使用した部品
-[[Arduino Fio>http://arduino.cc/en/Main/ArduinoBoardFio]]
-XBee
-LiPoバッテリー
-[[アナログジョイスティック>http://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-07RM]]
-LED × 2(赤, 緑)
-ブレッドボード

*ハードウェアの説明
Arduinoのアナログ入力0, 1にジョイスティックのX軸とY軸を接続し、AD変換を行っている。また、D2ピンにボタンを接続し、スイッチが押された場合はGNDレベルに落ち、割り込みが発生するようになっている。D2ピンとスイッチに関しては　10KΩのプルアップ抵抗が取り付けられている。またD3とD5ピンにLEDが取り付けられており、PWMにより明るさの調整が可能になっている。

*ソフトウェアの説明
XY座標により入力されるデータを2つのモーター出力に変換している。変換のために極座標に飛ばし、角度θにより左右のモーター出力の係数を出力する関数を記述し、それをベクトルの大きさRと掛け合わせる事でモーター出力を得ている。またスイッチによりモードの切り替えを行い、モード0(超信地旋回あり)とモード1(超信地旋回しない)で別の関数を呼び出している。データは現在100ms毎に送信を行っている。チャタリング対策に関してもソフトウェア的に行っている。一旦ボタンが押された場合は500ms割り込みを無視するように設定されている。データの送信を行うと、車体側から6系統のAD変換の結果が返ってくるので、そのデータの読み込みを行いA0の結果に応じて赤LEDを点灯する。また3秒以上データがかえってこない場合は通信エラーが発生しているため、赤LEDが点滅する。

*実験


#ref(f84f95255f613330e9cfcb76045bfe85.jpeg)

#highlight(linenumber,cpp){{
//ソースコード: polar_stick_ver4
#include <MsTimer2.h>
#include <math.h>
#define MAX_X 1012
#define MAX_Y 998
#define MIN_X 0
#define MIN_Y 0
#define NEUTRAL_X 414
#define NEUTRAL_Y 422
#define asobi 20
#define STICK_LED 3
#define DATA0_PIN 5

unsigned long MainTimerCount;
/*TimerCount1: データの送信周期用
TimerCount2: スティックボタンのチャタリング対策用
TimerCount3: errorLed用
TimerCount4: 受信データが途切れていないかの確認用
error_flag: flagが1の場合はエラーなので, error_led関数がenableになる
*/
unsigned long TimerCount1, TimerCount2, TimerCount3, TimerCount4;
int stick_mode, error_flag, error_led_mode;

/*通信が不安定になった場合に実行する*/
void XBee_abort(int second){
  int i;
  make_command(0, 0);
  for(i = 0; i < second; i++){
      make_command(0, 0);
      delay(1000);
      Serial.flush();
  }
}

void timer_interrupt(){
  int i;
  double r, s;
  int left_power, right_power;//-255 ~ 255でモーター出力を表現
  if(MainTimerCount - TimerCount1 >= 10){//データの送信
    stick_read(&r, &s);//極座標で値を返す関数
    if(stick_mode == 0)
      convert_pwm2(r,s,&left_power, &right_power);
    else if(stick_mode == 1)
      convert_pwm(r,s,&left_power, &right_power);
    make_command(left_power, right_power);

    TimerCount1 = MainTimerCount;
  }
  error_led();
  //タイマーの加算
  MainTimerCount++;
}

//errorLedを点灯させる関数
void error_led(){
  if(error_flag == 1){
    if(MainTimerCount - TimerCount3 >= 50){
       if(error_led_mode == 0){
         analogWrite(DATA0_PIN,50);
         error_led_mode = 1;
       }
       else{
         analogWrite(DATA0_PIN, 0);
         error_led_mode = 0;
       }
       TimerCount3 = MainTimerCount;
    }
  }
}

void button_interrupt(){
  if(MainTimerCount - TimerCount2 >= 50){
    if(stick_mode == 0){
      stick_mode = 1;
      analogWrite(STICK_LED, 255);
    }
    else{
      stick_mode = 0;
      analogWrite(STICK_LED, 0);
    }
    TimerCount2 = MainTimerCount;
  }
}

void setup(){
  Serial.begin(9600);
  pinMode(STICK_LED, OUTPUT);
  pinMode(DATA0_PIN, OUTPUT);
  MainTimerCount = 0;
  TimerCount1 = 0;
  TimerCount2 = 0;
  TimerCount3 = 0;
  TimerCount4 = 0;
  error_flag = 0;
  attachInterrupt(0, button_interrupt, FALLING);
  MsTimer2::set(10, timer_interrupt);
  MsTimer2::start();
  attachInterrupt(0, button_interrupt, FALLING);
  stick_mode = 0;
  digitalWrite(STICK_LED, LOW);
  analogWrite(DATA0_PIN, 0);
}

/*
rは中央からの距離, sは角度を表す. rの最大は1である.
sはスティックを前に倒すと0, 左に倒すと-90, 右に倒すと90, 後ろに倒すと180 or -180を返す
*/
void stick_read(double *r, double *s){
    double stick_x, stick_y;

   
  //スティック位置の補正
  stick_x = (double)analogRead(0);
  stick_y = (double)analogRead(1);

  if(stick_x < NEUTRAL_X){
    stick_x = (stick_x - NEUTRAL_X) / (NEUTRAL_X - MIN_X);
  }
  else{
    stick_x = (stick_x - NEUTRAL_X) / (MAX_X - NEUTRAL_X);
  }
  if(stick_y < NEUTRAL_Y){
    stick_y = (stick_y -NEUTRAL_Y) / (NEUTRAL_Y - MIN_Y);
  }
  else{
    stick_y = (stick_y - NEUTRAL_Y)/ (MAX_Y - NEUTRAL_Y);
  }
 /* デバッグ用
  Serial.print(stick_x);
  Serial.print(",");
  Serial.println(stick_y);
  */
  *r = sqrt(pow(stick_x,2) + pow(stick_y, 2));
  if(*r > 1)*r = 1;
  else if(*r < -1)*r = -1;
  *s = atan2(stick_x, stick_y) /3.14 * 180;
}
/*
極座標による入力を二つのモーターの出力に変換する関数. 
超信地展開するバージョン. 
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = -1;
   _rp = (s + 180) / 45 - 1;
  }
 else if(-90 <= s && s < 0){
  _lp = (s + 90) / 45 - 1; 
  _rp = 1;
 }
 else if(0 <= s && s < 90){
   _lp = 1;
   _rp = 1 - s / 45;
  }
 else{//90 <= r <= 180の時
   _lp = 1 - (s - 90) / 45;
   _rp = -1;
 }
 *lp = _lp * r * 255;
 *rp = _rp * r * 255;
 if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0; 
 /*デバグ用
 Serial.print(_lp);
 Serial.print(",");
 Serial.println(_rp);
 */
}
/*
極座標による入力を二つのモーターの出力に変換する関数. 
超信地展開しないバージョン. 
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm2(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = (s + 180)  / 90 - 1;
   _rp = (s + 180) / 45 - 1;
  }
 else if(-90 <= s && s < 0){
  _lp = (s + 90) / 90; 
  _rp = 1;
 }
 else if(0 <= s && s < 90){
   _lp = 1;
   _rp = 1 - s / 90;
  }
 else{//90 <= r <= 180の時
   _lp = 1 - (s - 90) / 45;
   _rp = -1 * (s - 90) / 90;
 }
 *lp = _lp * r * 255;
 *rp = _rp * r * 255;
 if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0; 
 /*デバグ用
 Serial.print(_lp);
 Serial.print(",");
 Serial.println(_rp);
 */
}

void make_command(int left_power, int right_power){
  char c0, c1, c2, c3;
  left_power += 255;
  right_power += 255;
  c0 = (left_power & 0b111111100) >> 2;
  c1 = (left_power & 0b000000011) << 5;
  c1 = c1 | ((right_power & 0b111110000)  >> 4);
  c2 = (right_power & 0b000001111) << 3;
  c3 = 0;
  c0 = c0 | 0b10000000;//先頭バイトの先頭ビットを1とする
  c0 = c0 & 0b111111111;
  Serial.print(c0);
  Serial.print(c1);
  Serial.print(c2);
  Serial.print(c3);
}
  
void loop(){
  int i, b;
  char c[12];
  int data[6];
  if(MainTimerCount - TimerCount4 >= 100){
    error_flag = 1;
    XBee_abort(3);
    error_flag = 0;
    make_command(0, 0);
    delay(50);
    TimerCount4 = MainTimerCount;
  }
  //データの読み込み
    if(Serial.available()){
     c[0] = Serial.read();
     if((c[0] & 0b10000000) != 0){
       delay(30);
       for(i = 1 ; i < 12; i++)
        c[i] =Serial.read();
       for(i = 0; i < 6; i++){
         data[i] = c[i * 2] & 0b01111111;
         data[i] = data[i] << 3;
         data[i] = data[i] | c[i * 2 + 1] & 0b00000111;
       }
       b = data[0] / 4 - 100;
       if(b < 0)b = 0;
       analogWrite(DATA0_PIN, b);
       TimerCount4 = MainTimerCount;//データが受信できているかを監視
     }
    }
}
}}

*スティックコントローラーの機能
-スティックを倒した方向に応じて、2つのキャタピラー出力に変換し無線送信する
-スティックを押す事で超信地旋回するモードとしないモードに切り替わる(超信地旋回するモードの場合には緑LEDが点灯)
-車体から送られたデータを読み込み、値によって赤LEDを点灯する
-一定時間データが送られてこない場合通信エラーなので赤LEDが点滅する

*使用した部品
-[[Arduino Fio>http://arduino.cc/en/Main/ArduinoBoardFio]]
-XBee
-LiPoバッテリー
-[[アナログジョイスティック>http://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-07RM]]
-LED × 2(赤, 緑)
-ブレッドボード

*ハードウェアの説明
Arduinoのアナログ入力0, 1にジョイスティックのX軸とY軸を接続し、AD変換を行っている。また、D2ピンにボタンを接続し、スイッチが押された場合はGNDレベルに落ち、割り込みが発生するようになっている。D2ピンとスイッチに関しては　10KΩのプルアップ抵抗が取り付けられている。またD3とD5ピンにLEDが取り付けられており、PWMにより明るさの調整が可能になっている。

*ソフトウェアの説明
XY座標により入力されるデータを2つのモーター出力に変換している。変換のために極座標に飛ばし、角度θにより左右のモーター出力の係数を出力する関数を記述し、それをベクトルの大きさRと掛け合わせる事でモーター出力を得ている。またスイッチによりモードの切り替えを行い、モード0(超信地旋回あり)とモード1(超信地旋回しない)で別の関数を呼び出している。データは現在100ms毎に送信を行っている。チャタリング対策に関してもソフトウェア的に行っている。一旦ボタンが押された場合は500ms割り込みを無視するように設定されている。データの送信を行うと、車体側から6系統のAD変換の結果が返ってくるので、そのデータの読み込みを行いA0の結果に応じて赤LEDを点灯する。また3秒以上データがかえってこない場合は通信エラーが発生しているため、赤LEDが点滅する。

*実験
Arduino Tank Ver0.1と合わせて動作実験を行い、意図した通りに動作している事を確認した。ただし、通信エラーが発生する事がある。通信エラーはPCの近く等電磁波が発生していると考えられる場所、および走っている途中で発生している。

*考察
現在通信エラーが発生してしまっているので、原因を確かめる必要がある。加えて、通信エラーが発生した際に復帰する機構を車体側も合わせ作成する必要がある。通信エラーの原因の特定するために通信モジュール、および車体側のモーターノイズや電圧降下の程度を単体でテストを行う必要がある。

*今後の課題
-通信モジュールのテスト(どういった場合にエラーが発生するか)



#ref(f84f95255f613330e9cfcb76045bfe85.jpeg)

#highlight(linenumber,cpp){{
//ソースコード: polar_stick_ver4
#include <MsTimer2.h>
#include <math.h>
#define MAX_X 1012
#define MAX_Y 998
#define MIN_X 0
#define MIN_Y 0
#define NEUTRAL_X 414
#define NEUTRAL_Y 422
#define asobi 20
#define STICK_LED 3
#define DATA0_PIN 5

unsigned long MainTimerCount;
/*TimerCount1: データの送信周期用
TimerCount2: スティックボタンのチャタリング対策用
TimerCount3: errorLed用
TimerCount4: 受信データが途切れていないかの確認用
error_flag: flagが1の場合はエラーなので, error_led関数がenableになる
*/
unsigned long TimerCount1, TimerCount2, TimerCount3, TimerCount4;
int stick_mode, error_flag, error_led_mode;

/*通信が不安定になった場合に実行する*/
void XBee_abort(int second){
  int i;
  make_command(0, 0);
  for(i = 0; i < second; i++){
      make_command(0, 0);
      delay(1000);
      Serial.flush();
  }
}

void timer_interrupt(){
  int i;
  double r, s;
  int left_power, right_power;//-255 ~ 255でモーター出力を表現
  if(MainTimerCount - TimerCount1 >= 10){//データの送信
    stick_read(&r, &s);//極座標で値を返す関数
    if(stick_mode == 0)
      convert_pwm2(r,s,&left_power, &right_power);
    else if(stick_mode == 1)
      convert_pwm(r,s,&left_power, &right_power);
    make_command(left_power, right_power);

    TimerCount1 = MainTimerCount;
  }
  error_led();
  //タイマーの加算
  MainTimerCount++;
}

//errorLedを点灯させる関数
void error_led(){
  if(error_flag == 1){
    if(MainTimerCount - TimerCount3 >= 50){
       if(error_led_mode == 0){
         analogWrite(DATA0_PIN,50);
         error_led_mode = 1;
       }
       else{
         analogWrite(DATA0_PIN, 0);
         error_led_mode = 0;
       }
       TimerCount3 = MainTimerCount;
    }
  }
}

void button_interrupt(){
  if(MainTimerCount - TimerCount2 >= 50){
    if(stick_mode == 0){
      stick_mode = 1;
      analogWrite(STICK_LED, 255);
    }
    else{
      stick_mode = 0;
      analogWrite(STICK_LED, 0);
    }
    TimerCount2 = MainTimerCount;
  }
}

void setup(){
  Serial.begin(9600);
  pinMode(STICK_LED, OUTPUT);
  pinMode(DATA0_PIN, OUTPUT);
  MainTimerCount = 0;
  TimerCount1 = 0;
  TimerCount2 = 0;
  TimerCount3 = 0;
  TimerCount4 = 0;
  error_flag = 0;
  attachInterrupt(0, button_interrupt, FALLING);
  MsTimer2::set(10, timer_interrupt);
  MsTimer2::start();
  attachInterrupt(0, button_interrupt, FALLING);
  stick_mode = 0;
  digitalWrite(STICK_LED, LOW);
  analogWrite(DATA0_PIN, 0);
}

/*
rは中央からの距離, sは角度を表す. rの最大は1である.
sはスティックを前に倒すと0, 左に倒すと-90, 右に倒すと90, 後ろに倒すと180 or -180を返す
*/
void stick_read(double *r, double *s){
    double stick_x, stick_y;

   
  //スティック位置の補正
  stick_x = (double)analogRead(0);
  stick_y = (double)analogRead(1);

  if(stick_x < NEUTRAL_X){
    stick_x = (stick_x - NEUTRAL_X) / (NEUTRAL_X - MIN_X);
  }
  else{
    stick_x = (stick_x - NEUTRAL_X) / (MAX_X - NEUTRAL_X);
  }
  if(stick_y < NEUTRAL_Y){
    stick_y = (stick_y -NEUTRAL_Y) / (NEUTRAL_Y - MIN_Y);
  }
  else{
    stick_y = (stick_y - NEUTRAL_Y)/ (MAX_Y - NEUTRAL_Y);
  }
 /* デバッグ用
  Serial.print(stick_x);
  Serial.print(",");
  Serial.println(stick_y);
  */
  *r = sqrt(pow(stick_x,2) + pow(stick_y, 2));
  if(*r > 1)*r = 1;
  else if(*r < -1)*r = -1;
  *s = atan2(stick_x, stick_y) /3.14 * 180;
}
/*
極座標による入力を二つのモーターの出力に変換する関数. 
超信地展開するバージョン. 
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = -1;
   _rp = (s + 180) / 45 - 1;
  }
 else if(-90 <= s && s < 0){
  _lp = (s + 90) / 45 - 1; 
  _rp = 1;
 }
 else if(0 <= s && s < 90){
   _lp = 1;
   _rp = 1 - s / 45;
  }
 else{//90 <= r <= 180の時
   _lp = 1 - (s - 90) / 45;
   _rp = -1;
 }
 *lp = _lp * r * 255;
 *rp = _rp * r * 255;
 if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0; 
 /*デバグ用
 Serial.print(_lp);
 Serial.print(",");
 Serial.println(_rp);
 */
}
/*
極座標による入力を二つのモーターの出力に変換する関数. 
超信地展開しないバージョン. 
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm2(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = (s + 180)  / 90 - 1;
   _rp = (s + 180) / 45 - 1;
  }
 else if(-90 <= s && s < 0){
  _lp = (s + 90) / 90; 
  _rp = 1;
 }
 else if(0 <= s && s < 90){
   _lp = 1;
   _rp = 1 - s / 90;
  }
 else{//90 <= r <= 180の時
   _lp = 1 - (s - 90) / 45;
   _rp = -1 * (s - 90) / 90;
 }
 *lp = _lp * r * 255;
 *rp = _rp * r * 255;
 if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0; 
 /*デバグ用
 Serial.print(_lp);
 Serial.print(",");
 Serial.println(_rp);
 */
}

void make_command(int left_power, int right_power){
  char c0, c1, c2, c3;
  left_power += 255;
  right_power += 255;
  c0 = (left_power & 0b111111100) >> 2;
  c1 = (left_power & 0b000000011) << 5;
  c1 = c1 | ((right_power & 0b111110000)  >> 4);
  c2 = (right_power & 0b000001111) << 3;
  c3 = 0;
  c0 = c0 | 0b10000000;//先頭バイトの先頭ビットを1とする
  c0 = c0 & 0b111111111;
  Serial.print(c0);
  Serial.print(c1);
  Serial.print(c2);
  Serial.print(c3);
}
  
void loop(){
  int i, b;
  char c[12];
  int data[6];
  if(MainTimerCount - TimerCount4 >= 100){
    error_flag = 1;
    XBee_abort(3);
    error_flag = 0;
    make_command(0, 0);
    delay(50);
    TimerCount4 = MainTimerCount;
  }
  //データの読み込み
    if(Serial.available()){
     c[0] = Serial.read();
     if((c[0] & 0b10000000) != 0){
       delay(30);
       for(i = 1 ; i < 12; i++)
        c[i] =Serial.read();
       for(i = 0; i < 6; i++){
         data[i] = c[i * 2] & 0b01111111;
         data[i] = data[i] << 3;
         data[i] = data[i] | c[i * 2 + 1] & 0b00000111;
       }
       b = data[0] / 4 - 100;
       if(b < 0)b = 0;
       analogWrite(DATA0_PIN, b);
       TimerCount4 = MainTimerCount;//データが受信できているかを監視
     }
    }
}
}}