KYOSYO iReceiver

工作

しばらく前に KYOSYO iReceiverとカメラを手に入れ、スマホでコントロールできる小型クローラを作ってみた。



使った部材はこんな感じ。


Arduinoのソースはこんな感じ。

// google : arduino rc receiver
// https://www.sparkfun.com/tutorials/348


// モーター(左)
#define PIN_ML_PWM      3 
#define PIN_ML_IN1      4
#define PIN_ML_IN2      5

// モーター(右)
#define PIN_MR_PWM      6
#define PIN_MR_IN1      8
#define PIN_MR_IN2      7

//CH2
#define PIN_IN_LEFT  9
//CH3
#define PIN_IN_RIGHT 10 


#include <Adafruit_NeoPixel.h>
#define LEDTAPE_NUM  12
#define LEDTAPE_PIN  12
Adafruit_NeoPixel strip = Adafruit_NeoPixel(LEDTAPE_NUM, LEDTAPE_PIN, NEO_GRB + NEO_KHZ800);

class Motor
{
  //private:
public:
  byte pin_pulse; //受信機からのパルスを入力するピン
  byte pin_pwm;   //モータードライバのVrefピン
  byte pin_in1;   //モータードライバのIN1ピン
  byte pin_in2;   //モータードライバのIN2ピン
  int pulse;      //パルス幅
  int speed;      //モータースピード符号あり
  int val;        //モータースピード。上限に対する%
  int limit;      //モータースピード絶対値での上限

  //public:
  /**
   * モーター 初期化
   * 回転が逆の場合、IN1/IN2の指定を入れ替える。
   *
   * @param byte pin_pulse 受信機からのパルスを入力するピン
   * @param byte pin_pwm   モータードライバのVrefピン
   * @param byte pin_in1   モータードライバのIN1ピン
   * @param byte pin_in2   モータードライバのIN2ピン
   * @param int limit      モータースピード絶対値での上限
   */
  Motor(byte pin_pulse, byte pin_pwm, byte pin_in1, byte pin_in2, int limit)
  {
    //クラス変数初期化
    this->pin_pulse = pin_pulse;
    this->pin_pwm = pin_pwm;
    this->pin_in1 = pin_in1;
    this->pin_in2 = pin_in2;
    this->limit = 100;

    pinMode(this->pin_in1, OUTPUT);   //TA7291P IN1 or IN2
    pinMode(this->pin_in2, OUTPUT);   //TA7291P IN1 or IN2
    this->stop();
  }

  void status()
  {
    Serial.print("PIN[");
    Serial.print(this->pin_pulse);
    Serial.print(" ");
    Serial.print(this->pin_pwm);
    Serial.print(" ");
    Serial.print(this->pin_in1);
    Serial.print(" ");
    Serial.print(this->pin_in2);
    Serial.print("] pulse[");
    if (this->pulse >= 0) Serial.print(" ");
    if (abs(this->pulse) < 100) Serial.print(" ");
    if (abs(this->pulse) < 10) Serial.print(" ");
    Serial.print(this->pulse);
    Serial.print("] speed[");
    if (abs(this->speed) < 100) Serial.print(" ");
    if (abs(this->speed) < 10) Serial.print(" ");
    Serial.print(this->speed);
    Serial.print("] val[");
    if (abs(this->val) < 100) Serial.print(" ");
    if (abs(this->val) < 10) Serial.print(" ");
    Serial.print(this->val);
    Serial.print("] ");
    Serial.print(this->limit);
    Serial.print(" ");

  }

  /**
   * モーター 停止
   *
   */
  void stop()
  {
    analogWrite(this->pin_pwm, 0);    //TA7291P Vref
    digitalWrite(this->pin_in1, LOW);
    digitalWrite(this->pin_in2, LOW);
    this->val = 0;
    this->speed = 0;
  }

  /**
   * モーター 状態の後進
   * 受信機からのパルスを読み取り、スピードを更新する。
   *
   */
  void update()
  {
    int stick_dir;  //スティック向き
    int stick_val;  //スティック値(絶対値)  0〜100
    int stick_threshold = 40; //スティックの中立判定のしきい値

    //スティック値の取得
    // ↑ 1900　
    // ↓ 1000
    this->pulse  = pulseIn(this->pin_pulse, HIGH, 25000);
    if (this->pulse < 800 || this->pulse > 2200) {
      //イレギュラーなパルス幅の際は更新なし
      return;
    }
    this->pulse -= 1500;    //-500 〜 500を期待
    if (this->pulse >= stick_threshold) {
      //スティックは前進
      stick_val = map(this->pulse, stick_threshold, 450, 1, 100);
      stick_dir = 1;
    } 
    else if (this->pulse <= - stick_threshold) {
      //スティックは後進
      stick_val = map(- this->pulse, stick_threshold, 450, 1, 100);
      stick_dir = -1;
    } 
    else {
      //スティックは中立
      stick_val = 0;
      stick_dir = 0;
    }

    //現在速度の更新
    int stickspeed = stick_val * stick_dir; //符号ありスピード
    if (this->speed < stickspeed && this->speed < 100) {
      //先進方向に増速 or 後進方向で減速
      this->speed += 4;

    } 
    else if (this->speed > stickspeed && this->speed > -100) {
      //前進方向に減速 or 後進方向に増速
      this->speed -= 4;

    } 
    else {
      //変化なし
    }    

    //モータドライバ制御
    if (this->speed > 0) {
      //前進
      this->val = map(this->speed, 1, 100, 35, this->limit);
      digitalWrite(this->pin_in1, HIGH);
      digitalWrite(this->pin_in2, LOW);
    } 
    else if (this->speed < 0) {
      //後進
      this->val = map(- this->speed, 1, 100, 35, this->limit);
      digitalWrite(this->pin_in1, LOW);
      digitalWrite(this->pin_in2, HIGH);
    } 
    else {
      //free
      this->val = 0; 
      digitalWrite(this->pin_in1, LOW);
      digitalWrite(this->pin_in2, LOW);
    }
    analogWrite(pin_pwm, this->val);    //TA7291P Vref
  }
};

//(byte pin_pulse, byte pin_pwm, byte pin_in1, byte pin_in2, int limit)

Motor ML = Motor(PIN_IN_LEFT,  PIN_ML_PWM, PIN_ML_IN1, PIN_ML_IN2, 100);
Motor MR = Motor(PIN_IN_RIGHT, PIN_MR_PWM, PIN_MR_IN1, PIN_MR_IN2, 100);


void setup()
{ 
  Serial.begin(9600);

  ML.stop();
  MR.stop();

  //LED TAPE の初期化
  strip.begin();
  strip.show();
}

void loop() {
  int i;
  int num = LEDTAPE_NUM / 2;
  int light = 16;
  
  //モーター状態更新
  ML.update();
  //モーターステータス表示
  ML.status();
  //LEDテープ
  int sl = map(abs(ML.speed), 0, 100, 0, num);
  for (i = 0; i < num; i++) {
     if (sl >= i) {
       if (ML.speed == 0){
         strip.setPixelColor(LEDTAPE_NUM - i - 1, strip.Color(light, light, 0));
       } else if (ML.speed > 0){
         strip.setPixelColor(LEDTAPE_NUM - i - 1, strip.Color(0, light, 0));
       } else {
         strip.setPixelColor(LEDTAPE_NUM - i - 1, strip.Color(light, 0, 0));
       }
     } else {
       strip.setPixelColor(LEDTAPE_NUM - i - 1, strip.Color(0, 0, 0));
     }
  }

  //モーター状態更新
  MR.update();
  //モーターステータス表示
  MR.status();
  //LEDテープ
  int sr = map(abs(MR.speed), 0, 100, 0, num);
  for (i = 0; i < num; i++) {
     if (sr >= i) {
       if (MR.speed == 0){
         strip.setPixelColor(i, strip.Color(light, light, 0));
       } else if (MR.speed > 0){
         strip.setPixelColor(i, strip.Color(0, light, 0));
       } else {
         strip.setPixelColor(i, strip.Color(light, 0, 0));
       }
     } else {
       strip.setPixelColor(i, strip.Color(0, 0, 0));
     }
  }
  
  strip.show(); 
  Serial.println("");
  //delay(10);
}