// DRV8830_URM37_MONO_V9_steering
//         ソースの簡略化，内輪差のパワー補償追加(両輪ミキシング化)
//         RCモードで自動停止，ブザー，アトラクションサーボ，バックランプ
//
// 2023.12.24 3種のプログラムモードの切替え機構追加
//         ・切り替えは演奏後，LED(音)が点滅する以下の数の時，タクトSWを押す
//          1..."RC"            ラジコンモードで個別製作の送信機で操縦
//          2..."自動回避走行"  障害物を避けて自動的に走行する
//          3..."ついてく"      前方の物体と常に一定の距離を保つ
//          放置..."RC"
//
// 2023.11.29 MONO Wireless を追加してラジコン化
//         ・バッテリー⇨LiPo 2セル,7.4V,1400mAhに換装
// 2021.9 ヒューマンアカデミーロボット教室向け見たいな！
// for "Human Academy Robot Micon" Original Block
// Use Serial servo (PIC12F1822)
// ******************************************************************
// 送信側のデータフォーマット
//       http://mtaka.html.xdomain.jp/4WD/Transmitter_ch.txt
// 本ステアリングロボットは，これを解析してパワーコマンドを発行する
// 本ロボットは，"B"チャンネルを使用する(ちなみに"A"は4輪駆動ロボット)
//     チャンネル"B"指定は送信機のFunction SWを押しながら電源投入
// 本ロボットは，MONOWIRELESSを用いたキャタピラー駆動を参考とした
//       http://mtaka.html.xdomain.jp/Caterpillar/Wireless_operation.pdf
// 
// ・タクトSWはA1  押す...ON,   解放...OFF
// *********      送信機からの信号形式            ************
// ・送信機の可変抵抗とマイコンのピン対応は，
//        A1---A1(舵，右指左右), A2---A2(パワー，左上下)
// ◇Data Sequence ===> B,mmm,nnnL/F
//          Bはチャンネル名，
//          "," カマはデータセパレータ
//          mmm...A1,   nnn...A2      ADC変換後数値
// *********    ステアリング用シリアルサーボ     ************
// ◇Data Sequence ===> ammmL/F
//          a   はシリアルサーボ名，
//          mmm はサーボ角度（0～180）左回転
//    そのため Serial.print の発行は極力抑える
// *********　　URM37 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)
/* ******************************************************/

volatile int URPWM = 3; // PWM Output 0－25000US，Every 50US represent 1cm
volatile int URTRIG= 5; // PWM trigger pin
volatile 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 CLEAR 0x80
// ブリッジ制御
#define M_STANBY B00    // スタンバイ
#define M_REVERSE B01   // 逆転
#define M_NORMAL B10    // 正転
#define M_BRAKE B11     // ブレーキ
// Voltage Definition
//#define MIN_VSET 0x09 // 0.72V
//#define MAX_VSET 0x15 // 1.69V
//#define MAX_VSET 0x50
//#define MAX_VSET 0x3F
//#define MAX_VSET 0x30

#define MAX_VSET 0x34
#define MIN_VSET 0x03
#define HOLD_TIME 40   // [ms]
#define STEP_TIME  4   // [ms]
#define STEP_demo 15   // [ms]
#define DISTANCE_LIMIT 20  // [cm]
#define DISTANCE_HOLD 10   // [cm]
volatile byte DRV_ADR;

// 制御コマンド送信
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();
}


volatile int bzz  = 7;  // パッシブブザー D7 (ATMEGA328 の13番ピン)
volatile int back = 8;  // バックランプ   D8 (ATMEGA328 の14番ピン)

const char B_chan = 'B';  // Channel name ==> Sterring Robot is "B"
const char ATTR = 'Z';    // Attraction Servo name "Z"
const char STER = 'a';    // Steering Servo name "a"

volatile int ang ;        // angle of Steering Servo Motor
volatile int SELECT ;
volatile int P_mode ;     // Program mode  1(avoid), 2(fixed_d), 3(R/C)

#define LED_PIN_BLUE 9
boolean LED;
void LED_tgl() {
  if (LED) {
    digitalWrite(LED_PIN_BLUE, HIGH);
  }
  else {
    digitalWrite(LED_PIN_BLUE, LOW);
  }
  LED = !LED;
}

#define ang_MAX 130            // Maximum of Servo angle (deg)
#define ang_L ang_MAX          // Maximum of Servo angle Left

#define ang_MIN 40             // Minimum of Servo angle Left
#define ang_R ang_MIN          // Minimum of Servo angle Right
//#define ang_center 86          // Center of Servo angle (deg)
#define ang_center 81          // Center of Servo angle (deg)


void pika1(){
  digitalWrite( LED_PIN_BLUE, HIGH );    //  ヘッドランプ点灯
  digitalWrite(back, HIGH);              //  バックランプ点灯
  tone(bzz, 1300, 150);
  delay(150);
  noTone(bzz);             // toneを終了
  digitalWrite( LED_PIN_BLUE, LOW );    //  ヘッドランプ消灯
  digitalWrite(back, LOW);              //  バックランプ消灯
  delay(100);
}
void pika2(){
  for(int cnt=0 ; cnt<2 ; cnt++ ){
    pika1();
  }
}
void pika3(){
  for(int cnt=0 ; cnt<3 ; cnt++){
    pika1();
  } 
}

#define C  262
#define D  294
#define E  330
#define F  349
#define G  392
#define A  440
#define B  494
#define c  523
#define d  587
#define e  659
#define f  698
#define g  784
#define R  1    // Rest

const PROGMEM int16_t notes[]={  // 楽譜データ
  R,D,D,B,A,G,D,R, R,D,D,B,A,G,E,R, R,E,E,c,B,A,F,R, R,d,d,d,c,A,B,R,
  R,D,D,B,A,G,D,R, R,D,D,B,A,G,E,R, R,E,E,c,B,A,d,d, d,d,e,d,c,A,G,R, d,R,0
    //B,B,B,R, B,B,B,R, B,d,G,A,B,R,R,R, c,c,c,c,c,B,B,B, B,A,A,B,A,R, d,R,
  //B,B,B,R, B,B,B,R, B,d,G,A,B,R,R,R, c,c,c,c,c,B,B,B, d,d,c,A,G,R, g,R,0
}; 

const PROGMEM int16_t notes2[]={      // 短時間楽譜データ
  //R,D,D,B,A,G,D,R, R,D,D,B,A,G,E,R, R,E,E,c,B,A,F,R, R,d,d,d,c,A,B,R,0
  R,D,D,B,A,G,D,R, R,D,D,B,A,G,E,R,0
};

volatile int Tempo = 130;             // 変数Tempoに300を代入する

void Jinglebell(){
  for(int i=0;;i++){       // 楽譜データを順に演奏
    int note = pgm_read_word(&notes[i]) ;     // 変数noteにひとつ読み
    //if(note == 0)  break;                     // noteが0なら1曲演奏終了
    if(note == 0)  return;                     // noteが0ならreturn
    else if(note!=R)  tone(bzz, note, Tempo); // noteがR(休符)でなければ
    // bzzピンにnote[Hz]の周波数の音をTempo[ms]出力する
    delay(Tempo*1.1);      // Tempoだけ待つ
  }
  delay(Tempo*8);          // しばらくしてから演奏を繰り返す
}

void Jinglebell2(){
  for(int i=0;;i++){       // 楽譜データを順に演奏
    int note = pgm_read_word(&notes2[i]) ;     // 変数noteにひとつ読み
    //if(note == 0)  break;                    // noteが0なら1曲演奏終了
    if(note == 0)  return;                     // noteが0ならreturn
    else if(note!=R)  tone(bzz, note, Tempo);  // noteがR(休符)でなければ
    // bzzピンにnote[Hz]の周波数の音をTempo[ms]出力する
    delay(Tempo*1.1);      // Tempoだけ待つ
  }
  delay(Tempo*8);          // しばらくしてから演奏を繰り返す
}


//////////////////////////////////////////////////////////////
void setup() {
  Serial.begin(38400);
  pinMode( LED_PIN_BLUE, OUTPUT );
  pinMode(bzz, OUTPUT);     // ブザー
  pinMode(back, OUTPUT);    // バックランプ
  
  Wire.begin();
  write_vset(00, M_NORMAL, DRV_ADR_0 ); // Stop
  write_vset(00, M_NORMAL, DRV_ADR_1 ); // Stop
  delay(100);

  /*
  tone(bzz, 784, 200);     // bzzピンに784Hzの音を200ms出力する(ポッ)
   delay(300);              // 600ms待つ
   tone(bzz, 784, 200);     // bzzピンに784Hzの音を200ms出力する(ポッ)
   delay(300);              // 600ms待つ
   tone(bzz, 784, 200);     // bzzピンに784Hzの音を200ms出力する(ポッ)
   delay(300);              // 600ms待つ
   tone(bzz, 1046, 1200);   // bzzピンに1046Hzの音を1200ms出力する(ポーン)
   delay(300);             //
   noTone(bzz);            // toneを終了
   delay(500);
   */
  Jinglebell();

  // Serect P_mode ....起動時，3秒以内にタクトスイッチ操作
  // 放置の時は，P_mode = 1;
  P_mode = 0;
  for( int i = 0 ; i<3 ; i++ ){
    pika1();
    for(int cnt = 0 ; cnt < 100 ; cnt++){
      if(  analogRead(A1) < 500 ){
        P_mode = 1;
      }
      delay(10);
    }
  } 
  if( P_mode == 0 ){
    for( int i=0 ; i<3 ; i++ ){
      pika2();

      for(int cnt = 0 ; cnt < 100 ; cnt++){
        if( analogRead(A1) < 500 ) {
          P_mode = 2;
        }
        delay(10);
      }
    }
  }
  if( P_mode == 0 ){
    for( int i=0 ; i<3 ; i++ ){
      pika3();
      for(int cnt = 0 ; cnt < 100 ; cnt++){
        if( analogRead(A1) < 500 ) {
          P_mode = 3;
        }
        delay(10);
      }
    }
  }
  if( P_mode == 0 ){
    P_mode = 1;
  }

  // P_mode 設定終了のサイン

  digitalWrite( LED_PIN_BLUE, HIGH );
  tone(bzz, 2000, 1000);
  delay(1200);
  noTone(bzz);             // toneを終了
  digitalWrite( LED_PIN_BLUE, LOW );

  // ステアリング機構のウオームアップ
  Serial.print(STER);
  Serial.println(ang_center);
  delay(300);
  Serial.print(STER);
  Serial.println(ang_MIN);
  delay(300);
  Serial.print(STER);
  Serial.println(ang_MAX);
  delay(500);

  for( ang = ang_MIN ; ang < ang_MAX ; ){
    Serial.print(STER);
    Serial.println(ang);
    ang += 1;
    delay(STEP_demo);
  }
  for( ang = ang_MAX ; ang > ang_MIN ; ){
    Serial.print(STER);
    Serial.println(ang);
    ang -= 1;
    delay(STEP_demo);
  }
  for( ang = ang_MIN ; ang > ang_center ; ){
    Serial.print(STER);
    Serial.println(ang);
    ang += 1;
    delay(STEP_demo);
  }

  Serial.print(STER);
  Serial.println(ang_center);
  delay(100);

  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
  {        // every 50us low level stands for 1cm
    Distance = DistanceMeasured/50; 
  }
}

volatile int sensorValue;
volatile int Right_D;
volatile int Left_D ;

void loop() { //
  write_vset(00, M_NORMAL, DRV_ADR_0 );
  write_vset(00, M_NORMAL, DRV_ADR_1 );
  delay(300);

  if ( P_mode == 1 ){
    RC();
  }
  if ( P_mode == 2 ){
    avoid();
  }
  if ( P_mode == 3 ){
    fixed_d();
  }
  //exit(0);
}

void Z_servo(){
  pika2();
  Serial.print(ATTR);
  Serial.println("90");
  delay(300);
  Serial.print(ATTR);
  Serial.println("90");
  delay(300);
  Serial.print(ATTR);
  Serial.println("180");
  delay(500);
  Serial.print(ATTR);
  Serial.println("0");
  delay(500);
  Serial.print(ATTR);
  Serial.println("90");
  delay(300);
}


// ***********   R/C mode  *********
// Human_Radicon_Reseive_DRV8830_command_4WD.ino
// 以上を参考としている
// ----.----.----.----.----.----.----.----.----.----.----.----.
// DRV8830 & uxcell ギヤドモーター2WD
//        MONOWIRELESS Recseive　&　Control
//        command type ,    "B"チャンネル使用
//
// 送信側から、レバーの位置に対応して、チャンネル当たり0から
//         B,mmmA1(右レバー),nnnA2(左レバー)L/F
// 1000までの数が送られてくるので，500を中心として，
// CH1(A1) は舵取りチャンネルとし，サーボ信号発行。右レバー左右
//         左...37   右...890
//         センター　... 462
#define A1_MIN 30.
#define A1_MAX 900.
#define A1_CENTER 462.
//
// CH2(A2) は正転・逆転を判断しパワー制御する。左レバー上下
//         上...951  下...87
//         センター　... 512
#define A2_MIN 80.
#define A2_MAX 970.
#define A2_CENTER 512.
#define S_in 32

void RC(){

  float ST_A1, PW_A2;
  float ST_VSETA1, PW_VSETA2, VSET_WK;
  const float Trim = -5;
  char ch;
  //const float ang_MIN_st =  25;    // [deg.]
  const float ang_MIN_st =  35;    // [deg.]
  //const float ang_MAX_st = 145;    // [deg.]
  const float ang_MAX_st = 135;    // [deg.]
  const float ST_dead_zone = 5;    // [deg.]

  char serial_in[S_in];            // 無線入力バッファ
  int  idx;
  //
  while(1){                       // 動作ループ開始
    /*  Omit seberal Line  */
    for ( int i = 0 ; i <= 4 ; i++) {
      while ( ch = Serial.read() != 0x0a) {
      }   // 0x0a ...L/F
    }
    //
    while (1) {                        // 入力エリアのスペースクリア
      /*  space clear input array */
      for ( idx = 0 ; idx <= (S_in - 1);) {
        serial_in[idx++] = 0x20;
      }
      idx = 0; 
      ch  = 0;
      while ( ch != 0x0a) {
        if ( (ch = Serial.read()) != -1) {
          serial_in[idx++] = ch;
        }
      }
      //
      /*  Start input stream Analysis */
      idx = 0; 
      ST_A1 = 0;

      if ( serial_in[idx] == ATTR ) {
        Z_servo();
        Jinglebell2();
      }
      else{
        /*  first data A1..操舵 右指左右 */
        if ( serial_in[idx] == B_chan ) {
          idx += 2;                              // first data point
          while ( serial_in[idx] != ',') {       // data separator ","
            ST_A1 *= 10;
            ST_A1 = ST_A1 + ( serial_in[idx] - '0');
            idx++;
          }
          /*   second data A2..パワー 左指上下　*/
          idx++;  
          PW_A2 = 0;
          while ( serial_in[idx + 1] != 0x0a) {     // End Of Line
            PW_A2 *= 10;
            PW_A2 = PW_A2 + ( serial_in[idx] - '0');
            idx++;
          }
          // ST_A1、PW_A2　は生データ  (A1_MIN ～　A1_MAX)
          ST_VSETA1 = map( ST_A1,
          A1_MAX, A1_MIN, ang_MIN_st, ang_MAX_st );       
          ST_VSETA1 = ST_VSETA1 + Trim;      // サーボ角度のトリム調整

          if( (PW_A2 - A2_CENTER) > 0 ){             // 前進
            PW_VSETA2 = map( PW_A2, 
            A2_CENTER, A2_MAX, MIN_VSET, MAX_VSET );
            //
            digitalWrite(LED_PIN_BLUE, HIGH);     //  ヘッドライト点灯
            digitalWrite(back, LOW);              //  バックランプ消灯
            PWM_Mode();                              // 測距
            if( Distance < DISTANCE_LIMIT ){         // 前方に障害物あり！
              write_vset(00, M_NORMAL, DRV_ADR_0);   // 自動停止
              write_vset(00, M_NORMAL, DRV_ADR_1);   // 自動停止
              //
              noTone(bzz);             // toneを終了
              digitalWrite(LED_PIN_BLUE, LOW);
              delay(100);
              digitalWrite(LED_PIN_BLUE, HIGH);
              tone(bzz, 1900, 150);
              delay(150);
            }
            // 障害物無し
            if( abs(ST_A1 - A1_CENTER ) < ST_dead_zone ){   // ステアが小さい             
              write_vset(PW_VSETA2, M_NORMAL, DRV_ADR_0);
              write_vset(PW_VSETA2, M_NORMAL, DRV_ADR_1);
            }
            else{                                 // 前進内輪差ミキシング開始
              if( (ST_A1 - A1_CENTER) > ST_dead_zone){    // 前進右ハンドリング
                // パワーアップミキシング
                VSET_WK = PW_VSETA2 * ( 1 + (ST_A1 - A1_CENTER)/(A1_MAX)/2. );                        
                write_vset(VSET_WK, M_NORMAL, DRV_ADR_0);
                // パワーダウンミキシング
                PW_VSETA2 = PW_VSETA2 * ( 1 - (ST_A1 - A1_CENTER)/(A1_MAX) );
                write_vset(PW_VSETA2, M_NORMAL, DRV_ADR_1);
              }
              else{                                       // 前進左ハンドリング
                // パワーアップミキシング              
                VSET_WK = PW_VSETA2 * ( 1 - (ST_A1 - A1_CENTER)/(A1_MAX)/2. ); 
                write_vset(VSET_WK, M_NORMAL, DRV_ADR_1);
                // パワーダウンミキシング
                PW_VSETA2 = PW_VSETA2 * ( 1 +  (ST_A1 - A1_CENTER)/(A1_CENTER));
                write_vset(PW_VSETA2, M_NORMAL, DRV_ADR_0);
              }                                    // 前進内輪差ミキシングの終わり
            }                                      // 前進障害物無し終わり
          }                                        // 前進の終わり
          //
          if ( (PW_A2 - A2_CENTER) < 0) {                    // 後進の開始
            digitalWrite(LED_PIN_BLUE, LOW);       //  ヘッドライト消灯
            digitalWrite(back, HIGH);              //  バックランプ点灯
            PW_VSETA2 = map( PW_A2, 
            A2_CENTER, A2_MIN, MIN_VSET, MAX_VSET );          
            if( (ST_A1 - A1_CENTER) > ST_dead_zone){  // 後進右ハンドリング
              // パワーアップミキシング
              VSET_WK = PW_VSETA2 * ( 1 + (ST_A1 - A1_CENTER)/(A1_MAX)/2. );
              write_vset(VSET_WK, M_REVERSE, DRV_ADR_0);
              // パワーダウンミキシング
              PW_VSETA2 = PW_VSETA2 * ( 1 - (ST_A1 - A1_CENTER)/(A1_MAX) );
              write_vset(PW_VSETA2, M_REVERSE, DRV_ADR_1);
            }
            else{                                       // 後進左ハンドリング
              // パワーアップミキシング
              VSET_WK = PW_VSETA2 * ( 1 - (ST_A1 - A1_CENTER)/(A1_MAX)/2. );
              write_vset(VSET_WK, M_REVERSE, DRV_ADR_1);
              // パワーダウンミキシング
              PW_VSETA2 = PW_VSETA2 * ( 1 +  (ST_A1 - A1_CENTER)/(A1_CENTER));
              write_vset(PW_VSETA2, M_REVERSE, DRV_ADR_0);
            }                                    // 後進内輪差ミキシングの終わり
          }                                      // 後進障害物無し終わり
          Serial.print(STER);
          Serial.println(int(ST_VSETA1));    // ステアコマンド発行
        }                                    // 解析の終わり
      }                                      // 入力エリアクリアの終わり
    }                                        // 動作ループの終わり
  }                                          // Attraction "Z" 処理の終わり
}                                            // RC()本体の終わり

// **********************************************

void avoid(){      //  ... 障害物回避モード
  while(1){
    digitalWrite( LED_PIN_BLUE, HIGH );
    write_vset(MAX_VSET, M_NORMAL, DRV_ADR_0 );
    write_vset(MAX_VSET, 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(600);
      write_vset(00, M_BRAKE, DRV_ADR_0 ); // Stop
      write_vset(00, M_BRAKE, DRV_ADR_1 );
      delay(200);
      Serial.print(STER);
      Serial.println(ang_R);
      delay (400);
      PWM_Mode();
      Right_D = Distance;
      delay(100);
      Serial.print(STER);
      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(STER);
        Serial.println(ang_R);
        delay (200);
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 ); // Back
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_1 );
        delay(400);
        Serial.print(STER);
        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(STER);
        Serial.println(ang_L);
        delay (200);
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_0 ); // Back
        write_vset(MAX_VSET, M_REVERSE, DRV_ADR_1 );
        delay(400);
        Serial.print(STER);
        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(STER);
    Serial.println(ang_center);
    delay(200);
  }
}

void fixed_d (){    //  ...一定距離維持(後をついてくる見たいな)
  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
  }
}