// DRV8830_test_2WD_V3_Net_Motor.ino
// 2022.3.24 mtakapii
// ・　FE130AV ネットで４個購入
// ・　黄色ギヤドモーター
// ・　ヒューマンアカデミーロボットモーター
// ・　1 モーターギヤボックス（小型）
// プログラミング方針：
//     回転起動させるべく、頻繁にコマンド発行

#include <Wire.h>

// DRV8830 Motor Driver I2Cアドレス
#define DRV_ADR_0 0x64 // DRV8830のI2Cアドレス
#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 // ブレーキ

// **** No fault ****
// FE130AV bought by internet
//#define MAX_VSET 0x20 // ...2.42V
//#define MIN_VSET 0x6  // 0.4V ...
// 黄色ギヤドモーター TWIN
#define MAX_VSET 0x36 // ...4.12V
#define MIN_VSET 0x0A // 0.7V...
// ヒューマンアカデミーロボットモーター
//#define MAX_VSET 0x3f // ... 4.86V 
//#define MIN_VSET 0x06 // 0.7V ...
// 1 モーターギヤボックス（小型）
//#define MAX_VSET 0x3f // ... 4.71V
//#define MIN_VSET 0x06 // 0.7V...

byte DRV_ADR;

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(500);
 pinMode(LED_BUILTIN, OUTPUT);
 Serial.begin(115200);
 Wire.begin();

 clear_fault(DRV_ADR_0);
 clear_control(DRV_ADR_0);

 Serial.println();
 Serial.println("DRV8830 test 2022.3.9");

 //********** I2C Scan ***********
 byte error, address;
 int nDevices;

 Serial.println("I2C Scanning...");
 nDevices = 0;
 for (address = 1; address < 127; address++ )
 {
  Wire.beginTransmission(address);
  error = Wire.endTransmission();
  if (error == 0)
  {
   Serial.print("I2C device found at address 0x");
   if (address < 16)
    Serial.print("0");
   Serial.print(address, HEX);
   Serial.println(" !");
   nDevices++;
  }
  else if (error == 4)
  {
   Serial.print("Unknown error at address 0x");
   if (address < 16)
    Serial.print("0");
   Serial.println(address, HEX);
  }
 }
 if (nDevices == 0)
  Serial.println("No I2C devices found\n");
 else
  Serial.println("done");
 //********** End I2C Scan ***********
}

void loop() {
 const int pitch = 150;
 const int keep = 10;
 int i;

 Serial.println("DRV8830 Motor TEST");
 digitalWrite(LED_BUILTIN, HIGH);

 Serial.println("NORMAL up");
 for (i = MIN_VSET ; i <= MAX_VSET ; i += 1) {
  write_vset(i, M_NORMAL, DRV_ADR_0 );
  delay(pitch);
 }
 for (i = 0 ; i <= keep ; i++) {
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_0 );
  delay(pitch);
 }
 Serial.println("NORMAL down");
 for (i = MAX_VSET ; i >= MIN_VSET ; i -= 1) {
  write_vset(i, M_NORMAL, DRV_ADR_0 );
  delay(pitch);
 }
 delay(100);
 /*****************************/

 digitalWrite(LED_BUILTIN, LOW);
 Serial.println("REVERSE up");
 for (i = MIN_VSET ; i <= MAX_VSET ; i += 1) {
  write_vset(i, M_REVERSE, DRV_ADR_0 );
  delay(pitch);
 }
 for (i = 0 ; i <= keep ; i++) {
  write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 );
  delay(pitch);
 }
 Serial.println("REVERSE down");
 for (i = MAX_VSET ; i >= MIN_VSET ; i -= 1) {
  write_vset(i, M_REVERSE, DRV_ADR_0 );
  delay(pitch);
 }
 delay(100);

 Serial.println("End test DRV8830");
 Serial.println();
}