// Human_Radicon_Reseive_DRV8830_2WD_channel_type.ino
// Bチャンネル専用 完全動作 
// 2022.4.1  mtakapii@yahoo.co.jp 以下の如く改造
// ◇キャタピラー走行,or 2WD,
// ◇MONOWIRELESS 送受信
// ** Baud rate ... 38400 bps
// ◇Data format ===> B,mmm,nnnL/F
//          Bはチャンネル名，
//          , はデータセパレータ
//          mmmはADC数値

#include <Wire.h>

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

// 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 0x1A
#define MIN_VSET 0x03

byte DRV_ADR;
byte DRV_ADR_NUM;
boolean LED;

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

void clear_control(byte ADR) {
 Wire.beginTransmission(ADR);
 Wire.write(CTR_ADR); // CONTROL flag
 Wire.write(CLEAR);
 Wire.endTransmission();
}

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

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 setup() {
 delay(100);
 pinMode(LED_BUILTIN, OUTPUT);
 //Serial.begin(115200);
 Serial.begin(38400);
 Wire.begin();
 //Serial.println("Human_Radicon_Reseive_DRV8830_2WD_channel_type");

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

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

int VR0, VR1;
int VSET0, VSET1;
char ch;
#define S_in 32

void loop() {
 char serial_in[S_in];
 int idx;

 //Serial.println("loop()");
 /* Omit seberal Line */
 for ( int i = 0 ; i <= 4 ; i++) {
   while ( ch = Serial.read() != 0x0a) {}; // 0x0a ...L/F
 }
 //Serial.println("Start while(1)");

 while (1) {
   /* space clear input array */
   for ( idx = 0 ; idx <= (S_in - 1);) {
     serial_in[idx++] = 0x20;
   }

   idx = 0; ch = 0;
   //while ( (ch != 0x0a) && (idx < S_in)) {
   while ( ch != 0x0a) {
     if ( (ch = Serial.read()) != -1) {
       serial_in[idx++] = ch;
     }
   }
   /*
     Serial.println(); Serial.print("idx=0x");
     Serial.println(idx, HEX);
     Serial.println();
     Serial.println("Next print serial_in[] ");
   for (int i = 0; i <= (idx) ;) {
     Serial.print(serial_in[i++]);
   } */

   /* 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');
       //Serial.print(serial_in[idx]);
       idx++;
     }
     //Serial.print("VR0="); Serial.println(VR0);

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

     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);
     }
     /*
           Serial.print("VSET0="); Serial.println(VSET0);
           Serial.print("VSET1="); Serial.println(VSET1);
           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 ); //
           //while (1);
     */
   }
 }
}