#include<SoftwareSerial.h>  //Bluetoothの無線通信に使用
#include <VarSpeedServo.h>  //サーボモータの制御に使用
VarSpeedServo myservo;    // create servo object to control a servo
VarSpeedServo myesc;    // create servo object to control a esc


/* ステアリングの設定 */
int mov_speed_ST = 30; //ステア移動速度
int center_pos = 90; //ステア中心位置 [サーボモータの中心位置 (90°)]
int left_DR = 20; //左位置 [中心位置より反時計回りに20°回転した位置]
int right_DR = 25; //右位置 [中心位置より時計回りに25°回転した位置]
int left_max = center_pos - left_DR; //左の最大切れ角
int right_max = center_pos + right_DR; //右の最大切れ角

/* スロットルの設定 */
int mov_speed_TH = 5; //スロットル移動速度
int brake_speed = 100; //ブレーキ速度
int neutral_pos = 91; //中立位置 [スロットルの中立位置 (90) ※ESCの設定によってずれがあるので、前後に走行しないよう値を調整する。ESC側を90で中立になるよう設定してもよい。]
int forward_DR = 20; //前進位置
int backward_DR = 20; //バック位置
int forward_max = neutral_pos + forward_DR; //前進の最大位置
int backward_max = neutral_pos - backward_DR; //バックの最大位置

//速度(mov_speed_ST,mov_speed_TH)は 1~255 の範囲で与える。（0にすると最大速度で移動）
//スロットル、サーボモータの値(pos)の範囲は、 0≦ pos ≦180 で与える。
//myservo.write 関数には回転角を絶対的な位置で与える。例) 90°から 45°反時計回りに動いてほしいときは、-45ではなく、45を関数に入力する。


SoftwareSerial Bluetooth(10, 11); // Arduinoから見て10ピンがRX（受信）, 11ピンがTX（送信)になる。bluetoothモジュールのTXにはArduinoのRX(10)が、bluetoothモジュールのRXにはArduinoのTX(11)が接続されていれば良い。

void setup() {
  Bluetooth.begin(9600); //シリアルポートを開いて、伝送速度を9600[bps] に設定
  Serial.begin(9600); //シリアルモニタで確認用
  myservo.attach(9); //サーボモータのPWM端子とArduinoの9番ピンを接続
  myesc.attach(6); //ESCのPWM端子とArduinoの6番ピンを接続
}

void loop() {
  if (Bluetooth.available()) {
    char input = Bluetooth.read(); //受信したテキストを変数inputに保存
    Serial.println(input); //受信したテキストをシリアルモニタに表示
    if (input == 'L') {
      myservo.write(left_max, mov_speed_ST, true); // ステアを左(Left)に切る
    } else if (input == 'C') {
      myservo.write(center_pos, mov_speed_ST, true); // ステアを中心(Center)に
    } else if (input == 'R') {
      myservo.write(right_max, mov_speed_ST, true);  //ステアを右(Right)に切る
    } else if (input == 'F') {
      myesc.write(forward_max, mov_speed_TH, true);  //前進(Forward)
    } else if (input == 'N') {
      myesc.write(neutral_pos, brake_speed, true);  //中立(Neutral)
    } else if (input == 'B') {
      myesc.write(backward_max, mov_speed_TH, true);  //後退(Backward)
    }
    else {
    }
  }
}