2018-09-16

ジェットビートル 回路図とスケッチ

ジェットビートル 回路図とスケッチを置いときます。
///
/// beatle
/// 6ch HC-05 BT serial receiver
///

#include
int ND0 = 0;      //serial data 0 L_UD
int ND1 = 0;      //serial data 1 L_LR
int ND2 = 0;      //serial data 2 R_LR
int ND3 = 0;      //serial data 3 R_UD
int ND4 = 0;      //serial data 4 LSW
int ND5 = 0;      //serial data 5 RSW
int PWML = 0;     //左速度
float SPD = 1;    //走行スピード係数
int CENTER = 90;  //ステアリングトリム
int ROT;          //ステアリングサーボ
float k = 0.27;   //ステアリング角度範囲計数
Servo servo09;   //サーボの名称を設定

void setup() {
  pinMode(3, OUTPUT);     //L正転(490Hz)
  pinMode(5, OUTPUT);     //L反転(977Hz)
  servo09.attach( 9);     //サーボ割り当て
  servo09.write(CENTER);  //センター初期位置
  Serial.begin(115200);  // HC05 serial speed
  Serial.write(65);      //'A'
}

void loop() {
  //バッファ領域に7個以上のデータがあるとき
  if (Serial.available() > 6 ) {
    if (Serial.read() == 63 ) { //'?'
      ND0 = Serial.read(); //L_UD //L_UD
      ND1 = Serial.read(); //R_UD //L_LR
      ND2 = Serial.read(); //L_LR //R_LR
      ND3 = Serial.read(); //R_LR //R_UD
      ND4 = Serial.read(); //LSW
      ND5 = Serial.read(); //RSW

      // ND0をPWML, ND1をPWMRに割り当て
      // NDxのセンターを90度としてPWM出力
      // 112〜144を不感帯とする。

      // 5:L正転、3:L反転
      if (ND0 > 144 ) {
        PWML = (ND0 - 128) * SPD ; //128以上はマイナス128してSPD倍に
        analogWrite(3, PWML)     ; //L正転
        analogWrite(5, 0   )     ;
      }
      else if  (ND0 < 112 ) {
        PWML = (127 - ND0) * SPD ; //128未満は反転してSPD倍に
        analogWrite(3, 0   )     ;
        analogWrite(5, PWML)     ; //L反転
      }
      else {
        PWML = 0;
        analogWrite(3, 0)        ; //L stop
        analogWrite(5, 0)        ; //L stop
      }

      // ステアリングサーボ
      //if (ND2 > 128 ) {
      //  ROT = CENTER + (ND2 - 128) * k ;
      //}
      //else if (ND2 < 128 ) {
      //  ROT = CENTER + (ND2 - 128) * k ;
      //}
      ROT = CENTER + (127 - ND2) * k ;
      servo09.write(ROT);

      // 2.50V(128 7Fh)Aボタン判定
      if (ND5 > 108 && ND5 < 148 ) {
        SPD = 1 ; // normal speed
        //} else {
      }
      // 3.33V(170 AAh)Bボタン判定
      if (ND5 > 150 && ND5 < 190 ) {
        SPD = 2 ; // double speed
        //} else {
      }

      Serial.write(65);//合図用のデータ'A'を送る
    }

  }
}

0 件のコメント: