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

ジェットビートル 回路図とスケッチを置いときます。

///

/// 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'を送る

    }

  }

}