// Human_Radicon_Reseive_channnel_type_4WD_V2.ino
// 完全動作 Aチャンネル専用 自動センター
// 2022.4.6 mtakapii4@gmail.com
// ◇by DRV8830 for 4WD Treedix 4組 4車輪
// 電動モーター タイヤ プラスチックギア DC 3-6V
// ◇4WD 4モーター（モータードライブ）専用
// ◇MONOWIRELESS 送受信
// ** Baud rate ... 115200 bps
// ◇Data Sequence ===> A,mmm,nnnL/F
//          Aはチャンネル名，
//          , はデータセパレータ
//          mmm, nnn は送信機のADC数値

//#define TEST // if TEST comment out
#include <Wire.h>

const int ledPin = 13;

// チャンネル
#define chan 'A'

// 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]

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();
}

void setup() {
 //Serial.begin(38400);
 Serial.begin(115200);
#ifdef TEST
 Serial.println();
 Serial.println("Start SETUP()");
#endif
 Wire.begin();
#ifdef TEST
 Serial.println("Human_Radicon_Reseive_channnel_type_4WD_V2");
#endif
 delay(500);
 write_vset(00, M_NORMAL, DRV_ADR_0 ); // to Zero
 write_vset(00, M_NORMAL, DRV_ADR_1 ); //
 write_vset(00, M_NORMAL, DRV_ADR_2 ); //
 write_vset(00, M_NORMAL, DRV_ADR_3 ); //
 delay(50);

 // 超信地旋回
 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(100);

 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(200);

 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(100);

 LED_tgl();
 write_vset(00, M_NORMAL, DRV_ADR_0 ); // to Zero
 write_vset(00, M_NORMAL, DRV_ADR_1 ); //
 write_vset(00, M_NORMAL, DRV_ADR_2 ); //
 write_vset(00, M_NORMAL, DRV_ADR_3 ); //
 delay(50);

 for (int i = 0 ; i >= 20 ; i++) {
   LED_tgl();
   delay(50);
 }
 LED = true;
#ifdef TEST
 Serial.println("End SETUP()");
#endif
}

/*********  data process *********/
// first, second ===> VR0, VR1

#define S_in 32
int VR0, VR1;
char serial_in[S_in];

void data_process() {
 int idx;
 char ch;

 /* input array space clear */
 for ( idx = 0 ; idx <= (S_in - 1);) {
   serial_in[idx++] = 0x20;
 }
 /*  Read 1 Line  */
 idx = 0; ch = 0;
 while ( ch != 0x0a) {
   if ( (ch = Serial.read()) != -1) {
     serial_in[idx++] = ch;
   }
 }
#ifdef TEST
 Serial.println();
 Serial.println("**** data Process ****");
 Serial.print("idx=0x");
 Serial.println(idx, HEX);
 Serial.print("serial_in[] := ");
 for (int i = 0; i <= (idx - 2) ;) {
   Serial.print(serial_in[i++]);
 }
 Serial.println();
#endif

 /* Start input stream Analysis */
 /* first data */

 idx = 0; VR0 = 0;
 if ( serial_in[idx] == chan) {
   idx += 2;  // first data point
   while ( serial_in[idx] != ',') {  // separator ","
     VR0 *= 10;
     VR0 = VR0 + ( serial_in[idx] - '0');
     idx++;
   }
#ifdef TEST
   Serial.print("VR0 = "); Serial.print(VR0);
#endif
 }

 /*  second data */
 idx++ ;       // => second data point
 VR1 = 0;
 while ( serial_in[idx + 1] != 0x0a) { // End Of Line
   VR1 *= 10;
   VR1 = VR1 + ( serial_in[idx] - '0');
   idx++;
 }
#ifdef TEST
 Serial.print("   ");
 Serial.print("VR1 = "); Serial.println(VR1);
#endif
}
/********* end data process *********/

void loop() {
 int i, VR0_C, VR1_C;
 int VSET0, VSET1;
 char ch;

#ifdef TEST
 Serial.println("loop()");
 Serial.println("Omit seberal Lines");
#endif
 for ( i = 0 ; i <= 3 ; i++) {
   data_process();
 }
 VR0_C = VR0;
 VR1_C = VR1;

 while (1) {
   data_process();
   LED_tgl();
   VR0 = VR0 - VR0_C;
   VR1 = VR1 - VR1_C;
   VSET0 = map(VR0, -500, 500, -MAX_VSET, MAX_VSET);
   VSET1 = map(VR1, -500, 500, -MAX_VSET, MAX_VSET);

#ifdef TEST
   Serial.print("VR0_C = ");
   Serial.print(VR0_C);
   Serial.print(" VR1_C = ");
   Serial.println(VR1_C);

   Serial.print("VSET0 = ");
   Serial.print(VSET0);
   Serial.print(" VSET1 = ");
   Serial.println(VSET1);
#endif

   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);
   }
 }
}

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