S88 8ch Detector ToF(VL6180X)

//S88 control board
// VL6180 VL6180X
// reference : RB_S88
// Software by Ruud Boer, November 2014.
// https://rudysmodelrailway.wordpress.com/software/

// 20190817 180316 8ch ver for Arduino nanoをベースに作成

// V6180X からの数値対距離の表
// https://www.denshi.club/cookbook/sensor/distance/1-4.html

#include <Wire.h>
#include <VL6180X.h>

//#define DEBUG

VL6180X sensor;
#define MinDistance 10    // SensorMin 9
#define MaxDistance 100   // sensorMax 200

#define CI_A 14           // 74HC4051 11:S0
#define CI_B 15           // 74HC4051 10:S1
#define CI_C 16           // 74HC4051  9:S2

int clk_counter=0;
const byte dataIn=0;  //data input from next Arduino in S88 chain
const byte dataOut=1; //data output pin=6

//Sensor control
#define RUN_LED_PIN 13 // RUN_LED(status LED) on/off pin
#define SENSOR_INTERVAL 50 //sensor interval[ms]
#define ANALOG_PORT_NUM 8 //analog port number (arduino uno'number is 6)
#define CONT_PORT_NUM 16 //port_num to see from DCC Controller

unsigned long gPreviousL5 = 0; 
boolean ir_led_onoff =false;
int reflector_state = 0;
int light_on_value[ANALOG_PORT_NUM];
int light_off_value[ANALOG_PORT_NUM];
int diff_value[ANALOG_PORT_NUM];
int transfer_value[CONT_PORT_NUM];
int sensor_value[CONT_PORT_NUM]; 

int diff_value_threshold = 50;

char TofSensorTable[10];

void setup() {
  //S88 Control
  pinMode(2, INPUT_PULLUP);
  attachInterrupt(0,clk,RISING); //pin 2 = clock interrupt
  pinMode(3, INPUT_PULLUP);
  attachInterrupt(1,PS,RISING);    //pin 3 = PS interrupt
  pinMode(dataIn,INPUT_PULLUP); // data in from next Arduino S88 in chain
  pinMode(dataOut, OUTPUT); // data out to commnad station or to previous Arduino in S88 chain
  digitalWrite(dataOut, LOW);   

  //Sensor Control
  pinMode(CI_A, OUTPUT); 
  pinMode(CI_B, OUTPUT); 
  pinMode(CI_C, OUTPUT); 

  //Status LED Control
  pinMode(RUN_LED_PIN, OUTPUT);
  gPreviousL5 = millis();
  reflector_state = 0;

  #ifdef DEBUG
  Serial.begin(115200);
  Serial.println("");
  Serial.println("S88 8ch ToF");
  #endif

  Wire.begin();                       // I2C 初期化

  for(char ch = 0 ; ch <= 7 ; ch++ ){ // VL6180X初期化
    bitSelector(ch);
    sensor.init();
    sensor.configureDefault();
    sensor.setTimeout(500);        
  }

    //init sensor_value,transfer_value
  for(int i = 0; i < CONT_PORT_NUM;i++)
  {
    transfer_value[i] = 0;
    sensor_value[i] = 0;
  }

  #ifdef DEBUG
  Serial.println("Ready");
  #endif
}

void loop()
{
  if(millis() - gPreviousL5 > SENSOR_INTERVAL)
  {
    gPreviousL5 = millis();
    read_sensor();
  }
}

//--------------------------------------------------------------------------------
// 8ch分読みだすのにどのくらいかかるのかな？
// 4ch分で 460〜500mS
//--------------------------------------------------------------------------------
char read_sensor()
{
  enum{
      ST_INIT = 0,
      ST_CH1,
      ST_CH2,
      ST_CH3,
      ST_CH4,
      ST_CH5,
      ST_CH6,
      ST_CH7,
      ST_CH8,
      ST_RE,
  };

  static char state = 0;
  unsigned char TofVal;

  switch(state){
    case ST_INIT:
                  state = ST_CH1;
                  break;
    case ST_CH1:
    case ST_CH2:
    case ST_CH3:
    case ST_CH4:
    case ST_CH5:
    case ST_CH6:
    case ST_CH7:
    case ST_CH8:
                  bitSelector(state-1);  // state を ch に使うなんて、わかりづら。
                  TofVal=sensor.readRangeSingleMillimeters();

                  #ifdef DEBUG
                  Serial.print(state,DEC);
                  Serial.print(":");
                  Serial.print(TofVal,DEC);
                  Serial.print(",");
                  #endif                                    

                  if( TofVal >= MinDistance && TofVal <= MaxDistance) // 閾値を確認して範囲内だったらON
                    sensor_value[state-1] = 1;  // ON                     

                  #ifdef DEBUG                  
                  if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
                  #endif

                  state++; 
                  break;      
    case ST_RE:
                  state = ST_CH1;

                  #ifdef DEBUG
                  Serial.println();
                  for(char i=0;i<=7;i++){
                    Serial.print(sensor_value[i]);
                  }
                  Serial.println();
                  #endif

                  break;
    default:
                  break; 

  }
}



//--------------------------------------------------------------------------------
// 3bit セレクタ
// I2CのSCLを0-8に切り替える
// 74HC4051 8-Channel Analog Multiplexer / Demultiplexer 切替用
//--------------------------------------------------------------------------------
void bitSelector(char ch)
{
  digitalWrite(CI_A, ch & 0x01 ? HIGH:LOW); 
  digitalWrite(CI_B, ch & 0x02 ? HIGH:LOW);
  digitalWrite(CI_C, ch & 0x04 ? HIGH:LOW);
#if 0 
  Serial.println();
  Serial.print(ch,DEC);
  Serial.print(",");
  Serial.print(digitalRead(CI_A));
  Serial.print(digitalRead(CI_B));
  Serial.println(digitalRead(CI_C));
#endif
}


#if 0
void bitSelector(char ch)
{
  digitalWrite(CI_A, LOW); 
  digitalWrite(CI_B, LOW);
  digitalWrite(CI_C, LOW);

  if(ch & 0x01) digitalWrite(CI_A, HIGH); 
  if(ch & 0x02) digitalWrite(CI_B, HIGH); 
  if(ch & 0x04) digitalWrite(CI_C, HIGH);
  Serial.println();
  Serial.print(ch,DEC);
  Serial.print(",");
  Serial.print(digitalRead(CI_A));
  Serial.print(digitalRead(CI_B));
  Serial.println(digitalRead(CI_C));
}
#endif

#if 0
void bitSelector(char ch)
{
  switch(ch){
    case 0:
          digitalWrite(CI_A, LOW); 
          digitalWrite(CI_B, LOW);
          digitalWrite(CI_C, LOW);
          break;
    case 1:
          digitalWrite(CI_A, HIGH); 
          digitalWrite(CI_B, LOW);
          digitalWrite(CI_C, LOW);
          break;
    case 2:
          digitalWrite(CI_A, LOW); 
          digitalWrite(CI_B, HIGH);
          digitalWrite(CI_C, LOW);
          break;                    
    case 3:
          digitalWrite(CI_A, HIGH); 
          digitalWrite(CI_B, HIGH);
          digitalWrite(CI_C, LOW);
          break;                    
    case 4:
          digitalWrite(CI_A, LOW); 
          digitalWrite(CI_B, LOW);
          digitalWrite(CI_C, HIGH);
          break;                    
    case 5:
          digitalWrite(CI_A, HIGH); 
          digitalWrite(CI_B, LOW);
          digitalWrite(CI_C, HIGH);
          break;                    
    case 6:
          digitalWrite(CI_A, LOW); 
          digitalWrite(CI_B, HIGH);
          digitalWrite(CI_C, HIGH);
          break;   
    case 7:
          digitalWrite(CI_A, HIGH); 
          digitalWrite(CI_B, HIGH);
          digitalWrite(CI_C, HIGH);
          break;
    default:
          break;        
  }
  Serial.print(ch,DEC);
  Serial.print(",");
  Serial.print(digitalRead(CI_A));
  Serial.print(digitalRead(CI_B));
  Serial.println(digitalRead(CI_C));
}
#endif

//--------------------------------------------------------------------------------
// ToFセンサが接続されているか確認　この関数必要？→いらないみたい
//--------------------------------------------------------------------------------
void InquirySensor()
{
  char ch = 0;

  for(ch = 0 ; ch <= 7 ; ch++){
    bitSelector(ch);
    TofSensorTable[ch]=read_sensor();
  }
}


//--------------------------------------------------------------------------------
// LOADインタラクティブ処理?
//--------------------------------------------------------------------------------
void PS()
{
  //Set transfer value
  clk_counter=0;
  for(int i = 0 ;i < CONT_PORT_NUM ; i++)
  {
    transfer_value[i] = sensor_value[i];
    sensor_value[i] = 0;
  }
}

//--------------------------------------------------------------------------------
// clk インタラクティブ処理?
//--------------------------------------------------------------------------------
void clk()
{
  //transfer 
  //Send transfer_value
  digitalWrite(dataOut,transfer_value[clk_counter]);
  //Delay makes reading output signal from next Arduino in chain more reliable.
  delayMicroseconds(16); 
  //Store dataIn value
  transfer_value[clk_counter] = digitalRead(dataIn);
  clk_counter =(clk_counter +1) % CONT_PORT_NUM;
}


//-----------------------------------------------------
// LED を使って、メッセージを表示させる。
//上位ビットから吐き出す
// ex 0xa0,4 -> 1010 -> ー・ー・
//-----------------------------------------------------
void LightMes( char sig ,char set)
{
  char cnt;
  for( cnt = 0 ; cnt<set ; cnt++ ){
    if( sig & 0x80){
      digitalWrite(RUN_LED_PIN, HIGH); // 長光
      delay(1000);
      digitalWrite(RUN_LED_PIN, LOW);
      delay(200);
    } else {
      digitalWrite(RUN_LED_PIN, HIGH); // 短光
      delay(200);
      digitalWrite(RUN_LED_PIN, LOW);            
      delay(200);
    }
    sig = sig << 1;
  }
      delay(400);
}

void lchika()
{
  digitalWrite(RUN_LED_PIN, !digitalRead(RUN_LED_PIN));
}