// DRV8830_2_Human_Robot_URM37_V4_steering.ino
// 2021.1.２９   mtakapii4@gmail.com
// for "Human Academy Robot Micon" Original Block
// use Serial servo (PIC12F1822)
/* ******************************************************/
// # Connection:
// #       Pin 1 VCC (URM V3.2)       -> VCC (Arduino)
// #       Pin 2 GND (URM V3.2)       -> GND (Arduino)
// #       Pin 4 PWM (URM V3.2)       -> Pin 3 (Arduino)
// #       Pin 6 COMP/TRIG (URM V3.2) -> Pin 5 (Arduino)
/* ******************************************************/
int URPWM = 3; // PWM Output 0－25000US，Every 50US represent 1cm
int URTRIG= 5; // PWM trigger pin
unsigned int Distance=  0;
uint8_t EnPwmCmd[4]={
  0x44,0x02,0xbb,0x01};    // distance measure command
/* ******************************************************/

#include <Wire.h>

// DRV8830 Motor Driver I2Cアドレス
#define DRV_ADR_0 0x64  // DRV8830のI2Cアドレス #0
//#define DRV_ADR_1 0x67  // DRV8830のI2Cアドレス #1
#define DRV_ADR_1 0x61  // DRV8830のI2Cアドレス #1
#define CTR_ADR   0x00  // CONTROLレジスタのサブアドレス
#define FLT_ADR   0x01  // FAULTレジスタのアドレス
// ブリッジ制御
#define M_STANBY  B00   // スタンバイ
#define M_REVERSE B01   // 逆転
#define M_NORMAL  B10   // 正転
#define M_BRAKE   B11   // ブレーキ

// 電圧定義
//#define MAX_VSET 0x15   // 1.69V
//#define MIN_VSET 0x09   // 0.72V
//#define MAX_VSET 0x50
#define MAX_VSET 0x3F
#define MIN_VSET 0x03
#define HOLD_TIME 40     // [ms]
#define STEP_TIME 4      // [ms]
#define DISTANCE_LIMIT 15   // [cm]
#define DISTANCE_HOLD  10   // [cm]
byte DRV_ADR;

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

int ang ;
int SELECT ;

#define LED_PIN_BLUE 9

#define ang_MAX    130
#define ang_L      ang_MAX
#define ang_MIN    39
#define ang_R      ang_MIN
#define ang_center 91

void setup() {
  Serial.begin(38400);
  pinMode( LED_PIN_BLUE, OUTPUT );
  Wire.begin();

  write_vset(00, M_NORMAL, DRV_ADR_0 );          // Stop
  write_vset(00, M_NORMAL, DRV_ADR_1 );

  delay(500);
  SELECT = analogRead(A1);
  Serial.print("a");
  Serial.println(ang_center);
  delay(500);

  Serial.print("a");
  Serial.println(ang_MIN);
  delay(500);
  Serial.print("a");
  Serial.println(ang_MAX);
  delay(500);

  for( ang = ang_MIN ; ang < ang_MAX ; ){
    Serial.print("a");    
    Serial.println(ang);
    ang += 1;
    delay(40);
  }
  for( ang = ang_MAX ; ang > ang_MIN ; ){
    Serial.print("a");
    Serial.println(ang);
    ang -= 1;
    delay(40);
  }  
  Serial.print("a");
  Serial.println(ang_center);
  delay(00);

  PWM_Mode_Setup();  // URM37 V3.2
}

void PWM_Mode_Setup()
{
  pinMode(URTRIG,OUTPUT);             // A low pull on pin COMP/TRIG
  digitalWrite(URTRIG,HIGH);          // Set to HIGH
  pinMode(URPWM, INPUT);              // Sending Enable PWM mode command
  for(int i=0;i<4;i++)
  {
    Serial.write(EnPwmCmd[i]);       // URM37 V3.2
  }
}
void PWM_Mode()
{                     // a low pull on pin COMP/TRIG  triggering a sensor reading
  digitalWrite(URTRIG, LOW);
  digitalWrite(URTRIG, HIGH);               // reading Pin PWM will output pulses
  unsigned long DistanceMeasured=pulseIn(URPWM,LOW);
  if(DistanceMeasured>=10200)
  {              // the reading is invalid.
    Distance = 150;
  }
  else
  {
    Distance=DistanceMeasured/50;      // every 50us low level stands for 1cm
  }
}

void loop() { //
  write_vset(00, M_NORMAL, DRV_ADR_0 );
  write_vset(00, M_NORMAL, DRV_ADR_1 );
  delay(300);
  // 順方向 徐々にスピードを上げて、下げる
  for (byte i = MIN_VSET; i <= MAX_VSET; i++) {
    write_vset(i, M_NORMAL, DRV_ADR_0 );
    write_vset(i, M_NORMAL, DRV_ADR_1 );  
    delay(STEP_TIME);
  }
  delay(HOLD_TIME);
  for (byte i = MAX_VSET; i >= MIN_VSET; i--) {
    write_vset(i, M_NORMAL, DRV_ADR_0 );
    write_vset(i, M_NORMAL, DRV_ADR_1 );  
    delay(STEP_TIME);
  }
  //digitalWrite( LED_PIN_BLUE, LOW );
  // 逆方向 徐々にスピードを上げて、下げる
  for (byte i = MIN_VSET; i <= MAX_VSET; i++) {
    write_vset(i, M_REVERSE, DRV_ADR_0 );
    write_vset(i, M_REVERSE, DRV_ADR_1 );  
    delay(STEP_TIME);
  }
  delay(HOLD_TIME);
  for (byte i = MAX_VSET; i >= MIN_VSET; i--) {
    write_vset(i, M_REVERSE, DRV_ADR_0 );
    write_vset(i, M_REVERSE, DRV_ADR_1 );  
    delay(STEP_TIME);
  }

  // MAX_VSET 左右連続旋回
  Serial.print("a");
  Serial.println(ang_L);
  write_vset(MAX_VSET/5, M_NORMAL,  DRV_ADR_0 );
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_1 );
  delay(500);
  write_vset(MAX_VSET/5, M_REVERSE, DRV_ADR_0 );
  write_vset(MAX_VSET, M_REVERSE,  DRV_ADR_1 );   
  delay(500);

  Serial.print("a");
  Serial.println(ang_R);
  write_vset(MAX_VSET, M_NORMAL, DRV_ADR_0 );
  write_vset(MAX_VSET/5, M_NORMAL,  DRV_ADR_1 );   
  delay(500);
  write_vset(MAX_VSET, M_REVERSE,  DRV_ADR_0 );
  write_vset(MAX_VSET/5, M_REVERSE, DRV_ADR_1 );  
  delay(500);

  write_vset(00, M_NORMAL, DRV_ADR_0 );
  write_vset(00, M_NORMAL, DRV_ADR_1 );

  Serial.print("a");
  Serial.println(ang_center);

  int sensorValue;
  int Right_D;
  int Left_D ;

  if (  SELECT > 500 ){
    while(1){
      digitalWrite( LED_PIN_BLUE, HIGH );
      write_vset(MAX_VSET/2, M_NORMAL, DRV_ADR_0 );
      write_vset(MAX_VSET/2, M_NORMAL, DRV_ADR_1 );

      PWM_Mode();
      if(  Distance < DISTANCE_LIMIT ){
        digitalWrite( LED_PIN_BLUE, LOW );
        write_vset(00, M_BRAKE, DRV_ADR_0 );
        write_vset(00, M_BRAKE, DRV_ADR_1 );
        delay(100);
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 );  // Back
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_1 );
        delay(400);
        write_vset(00, M_BRAKE, DRV_ADR_0 );          // Stop
        write_vset(00, M_BRAKE, DRV_ADR_1 );
        delay(200);

        Serial.print("a");
        Serial.println(ang_R);
        delay (400);
        PWM_Mode();
        Right_D = Distance;
        delay(100);

        Serial.print("a");
        Serial.println(ang_L);    //  set MAX angle
        delay (400);
        PWM_Mode();
        Left_D = Distance;
        delay(100);

        if( Right_D < Left_D ){        // Turn Left
          Serial.print("a");
          Serial.println(ang_R);
          delay (200);
          write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 );  // Back
          write_vset(MAX_VSET/2, M_REVERSE, DRV_ADR_1 );
          delay(400);
          Serial.print("a");
          Serial.println(ang_center);
          delay (200);
          write_vset(00, M_NORMAL, DRV_ADR_0 );          // Stop
          write_vset(00, M_NORMAL, DRV_ADR_1 );
          delay(100);
        }
        else{

          Serial.print("a");
          Serial.println(ang_L);
          delay (200);
          write_vset(MAX_VSET/2, M_REVERSE, DRV_ADR_0 );  // Back
          write_vset(MAX_VSET, M_REVERSE, DRV_ADR_1 );
          delay(400);
          Serial.print("a");
          Serial.println(ang_center);
          delay (200);
          write_vset(00, M_NORMAL, DRV_ADR_0 );          // Stop
          write_vset(00, M_NORMAL, DRV_ADR_1 );
          delay(100);
        }
      }
      Serial.print("a");
      Serial.println(ang_center);
      delay(200);
    }
  }
  else{
    while(1){
      int VSET;

      PWM_Mode();      
      if( (DISTANCE_HOLD-1) < Distance && Distance < (DISTANCE_HOLD +1)){
        write_vset(00, M_NORMAL, DRV_ADR_0 );
        write_vset(00, M_NORMAL, DRV_ADR_1 );
      }
      if( Distance < (DISTANCE_HOLD-1)){
        VSET = (DISTANCE_HOLD - Distance)*20;
        if(VSET > MAX_VSET){
          VSET = MAX_VSET;
        }
        write_vset(VSET, M_REVERSE, DRV_ADR_0 );
        write_vset(VSET, M_REVERSE, DRV_ADR_1 );
      }
      if( DISTANCE_HOLD < Distance){
        VSET = ( Distance - DISTANCE_HOLD)*20 ;          
        if(VSET > MAX_VSET){
          VSET = MAX_VSET;
        }         
        write_vset(VSET, M_NORMAL, DRV_ADR_0 );
        write_vset(VSET, M_NORMAL, DRV_ADR_1 );
      }
      delay(20);        // delay in between reads for stability
    }  
  }
}