// Human_Radicon_Reseive_DRV8830_4WD.ino
// 2021.12.27   mtakapii4@gmail.com
// ◇by DRV8830 for 4WD Treedix 4組 車輪
//   電動モーター タイヤ プラスチックギア DC 3-6V
// ◇キャタピラー走行,or 2WD
// ◇MONOWIRELESS 送受信
//     ** Baud rate ... 115200 bps
// ◇Data Sequence ===>  ***.***L/F
//                    Right Left

#include <Wire.h>

int ledPin = 13;

// DRV8830 Motor Driver I2Cアドレス
#define DRV_ADR_0 0x64  // DRV8830のI2Cアドレス #0
#define DRV_ADR_1 0x63  // DRV8830のI2Cアドレス #1
#define DRV_ADR_2 0x60  // DRV8830のI2Cアドレス #2
#define DRV_ADR_3 0x61  // DRV8830のI2Cアドレス #3

#define CTR_ADR   0x00  // CONTROLレジスタのサブアドレス
#define FLT_ADR   0x01  // FAULTレジスタのアドレス
#define CLEAR     0x80
// ブリッジ制御
#define M_STANBY  B00   // スタンバイ
#define M_REVERSE B01   // 逆転
#define M_NORMAL  B10   // 正転
#define M_BRAKE   B11   // ブレーキ

// 電圧定義
#define MAX_VSET 0x30
#define MIN_VSET 0x03
#define HOLD_TIME 40     // [ms]
#define STEP_TIME 4      // [ms]
#define CENTER 500
byte DRV_ADR;
boolean LED;
void LED_tgl();

// 制御コマンド送信
int write_vset(byte vs, byte ctr, byte DRV_ADR) {
  clear_fault(DRV_ADR);
  Wire.beginTransmission(DRV_ADR);
  Wire.write(CTR_ADR);
  Wire.write( ctr + (vs << 2) );
  return Wire.endTransmission();
}

void clear_fault(byte ADR) {
  Wire.beginTransmission(ADR);
  Wire.write(FLT_ADR);
  Wire.write(CLEAR); // Clear FALUT flag
  Wire.endTransmission();
}

int VR0 = 0, VR1 = 0 ;
int VSET0,  VSET1;
char ch;
boolean VR1SW ;

void setup() {
  //Serial.begin(38400);
  Serial.begin(115200);
  Wire.begin();
  //Serial.println("Receiver 2021.2.14");
  delay(600);
  write_vset(00, M_NORMAL, DRV_ADR_0 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_1 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_2 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_3 ); // Stop

  //Serial.println("after vset");

  delay(100);
  // 超信地旋回
  LED_tgl();
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_0 );
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_1 );
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_2 );
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_3 );
  delay(400);

  LED_tgl();
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 );
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_1 );
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_2 );
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_3 );
  delay(800);

  LED_tgl();
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_0 );
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_1 );
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_2 );
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_3 );
  delay(400);

  LED_tgl();
  write_vset(00, M_NORMAL, DRV_ADR_0 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_1 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_2 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_3 ); // Stop

  delay(100);
  LED_tgl();
  delay(300);
  LED_tgl();
  delay(300);
  LED = true;
}

void loop() {
  //Serial.println("Start loop()");

  while (Serial.read() != 0x0a);  // 初めのデータは不確定

  VR1SW = false;
  VR0 = 500;
  VR1 = 500;
  while (1) {
    ch = Serial.read();
    if ( ch != -1 ) {
      if (ch == 0x0a) {    // 0x0a := L/F
        LED_tgl();

        VR0 -= 500;
        VR1 -= 500;
        VSET0 = map(VR0, -500, 500, -MAX_VSET, MAX_VSET);
        VSET1 = map(VR1, -500, 500, -MAX_VSET, MAX_VSET);

        if (VSET0 > 0) {
          write_vset(VSET0, M_REVERSE, DRV_ADR_0);
          write_vset(VSET0, M_REVERSE, DRV_ADR_1);
        }
        if (VSET0 < 0) {
          VSET0 = abs(VSET0);
          write_vset(VSET0, M_NORMAL, DRV_ADR_0);
          write_vset(VSET0, M_NORMAL, DRV_ADR_1);
        }
        if (VSET1 > 0) {
          write_vset(VSET1, M_REVERSE, DRV_ADR_2);
          write_vset(VSET1, M_REVERSE, DRV_ADR_3);
        }
        if (VSET1 < 0) {
          VSET1 = abs(VSET1);
          write_vset(VSET1, M_NORMAL, DRV_ADR_2);
          write_vset(VSET1, M_NORMAL, DRV_ADR_3);
        }
        VR0 = 0;
        VR1 = 0;
        VR1SW = false;
      }
      if ( ch > 0x2F && ch < 0x3A) {  // "0"～"9"
        if ( VR1SW == false) {
          VR0 *= 10;
          VR0 = VR0 + (ch - 0x30);
        }
        else {
          VR1 *= 10;
          VR1 = VR1 + (ch - 0x30);
        }
      }
      if (ch == 0x2c) {              // 0x2c :=","
        VR1SW = true;
      }
    }
  }
}

void LED_tgl() {
  if (LED) {
    digitalWrite(ledPin, HIGH);
  }
  else {
    digitalWrite(ledPin, LOW);
  }
  LED = !LED;
}