// Human_Radicon_Reseive_DRV8830_2WD.ino
// 2021.12.29  mtakapii4@gmail.com
// DRV8830 & キャタピラー走行向け,or 2WD
//       MONOWIRELESS 受信　＆　モーター制御
// ** Baud rate := 38400 bps
// Data Sequence ===> ***.***L/F
//                   Right.Left
// DVR8830 は頻繁にfaultするので，clear_fault()を発行
// 電流センス抵抗は0.05Ωなどと掟を破って見たりしています
// 4モーター制御を流用したので，それらのソースが残っています
// loop()は，できる限り早く応答するようにSerialprintなどを
// 無効にしています。

#include <Wire.h>


int ledPin = 13;

// DRV8830 Motor D river 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 MAX_VSET 0x14

#define MIN_VSET 0x02

#define HOLD_TIME 40    // [ms]

#define STEP_TIME 4     // [ms]

#define CENTER 500

byte DRV_ADR;

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 ;

boolean LED ;



void setup() {

 Serial.begin(38400);

 //Serial.begin(115200);

 Wire.begin();

 Serial.println("2WD Receive & Drive 2021.12.27");

 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



 delay(200);

 LED = true;

 // 超信地旋回

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

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



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

 LED_tgl();

 delay(300);

 LED_tgl();

 delay(300);

}



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



       //Serial.print(VSET0);

       //Serial.print(". ");

       //Serial.println(VSET1);



       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;

}