#define CYC_CNT 100 
 
const int PWMchL = 5;     // enable for L motor  (PWM 500Hz)
const int DOutIN1 =  2;      // LIN1 for L motor 
const int DOutIN2 =  3;      // LIN2 for L motor

const int PWMchR = 6;      // enable for R motor (PWM 500Hz)
const int DOutIN3 =  4;      // RIN1 for R motor
const int DOutIN4 =  7;      // RIN2 for R motor

const int PWMG1 = 9;      // enable for R motor (PWM 500Hz)
const int DOutIN5 =  8;      // RIN1 for R motor
const int DOutIN6 =  11;      // RIN2 for R motor

const int PWMG2 = 10;      // enable for R motor (PWM 500Hz)
//const int DOutA =  4;      // RIN1 for R motor
//const int DOutB =  7;      // RIN2 for R motor


const int DIn1 =  12;      // the number of the LED pin
const int DIn2 =  13;      // the number of the LED pin
const int DIn3 =  A2;      // the number of the LED pin
const int DIn4 =  A3;      // the number of the LED pin
const int DIn5 =  A4;      // the number of the LED pin
const int DIn6 =  A5;      // the number of the LED pin

const int AInMTRL =  A0;      // PWML for adj
const int AInMTRR =  A1;      // PWMR for adj

// io assignment
byte counter=0;
byte IN_sensor=0b00000000;

int outputValueL;
int outputValueR;

int sensorValueL;
int sensorValueR;


void setup() 
{
  pinMode(DIn1, INPUT);
  pinMode(DIn2, INPUT);
  pinMode(DIn3, INPUT);
  pinMode(DIn4, INPUT);
  pinMode(DIn5, INPUT);
  pinMode(DIn6, INPUT);
 
  pinMode(DOutIN1, OUTPUT);
  pinMode(DOutIN2, OUTPUT);
  pinMode(DOutIN3, OUTPUT);
  pinMode(DOutIN4, OUTPUT);
  pinMode(DOutIN5, OUTPUT);
  pinMode(DOutIN6, OUTPUT);
  analogWrite(PWMchL, 200);
  analogWrite(PWMchR, 200);
   Serial.begin(9600);  
}

void loop()
{
  // Experiment 1.3 OUT1=S1 AND S3
  if(Din1() && Din3()) Out1(1);
  else Out1(0);

  // Experiment 1.4 OUT3=S1 OR S3
  if(Din1() || Din3()) Out3(1);
  else Out3(0);
  
  // Experiment 2.1 OUT2=(S2 AND S3) AND S4
  if((Din2() && Din3()) && Din4()) Out2(1);
  else Out2(0);
  
  // Experiment 2.2 OUT4=(S2 AND S3) OR S4
  if((Din2() && Din3()) || Din4()) Out4(1);
  else Out4(0);

}

unsigned long GetTick()
{
  return(millis());
}

void GetPWML(byte cc)
{
  if (cc == 3)
  {
    sensorValueL = analogRead(AInMTRL);            
  // map it to the range of the analog out:
   outputValueL = map(sensorValueL, 0, 1023, 80, 254);  
    Serial.print("sensorL = " );                       
    Serial.print(sensorValueL);
    Serial.print("sensorR = " );                       
    Serial.print(sensorValueR);
    Serial.print(" timer = " );                          
    Serial.println(GetTick());   

  // change the analog out value:
   analogWrite(PWMchL, outputValueL);           
         

  }
}

void GetPWMR(byte cc)
{
  if (cc == 1)
  {
    sensorValueR = analogRead(AInMTRR);            
  // map it to the range of the analog out:
    outputValueR = map(sensorValueR, 0, 1023, 80, 254);  
  // change the analog out value:
  analogWrite(PWMchR, outputValueR);
  }    
}

//------------------------------
void GetInput()
{
  IN_sensor=0b00000000;
  if (Din1()==1)
    IN_sensor |= (0x01<<0);
  if (Din2()==1)
    IN_sensor |= (0x01<<1);
  if (Din3()==1)
    IN_sensor |= (0x01<<2);
  if (Din4()==1)
    IN_sensor |= (0x01<<3);
  if (Din5()==1)
    IN_sensor |= (0x01<<4);
  if (Din6()==1)
    IN_sensor |= (0x01<<5);
}

int Din1()
{
   return(digitalRead(DIn1));
}

int Din2()
{
   return(digitalRead(DIn2));
}
int Din3()
{
   return(digitalRead(DIn3));
}
int Din4()
{
   return(digitalRead(DIn4));
}
int Din5()
{
   return(digitalRead(DIn5));
}
int Din6()
{
   return(digitalRead(DIn6));
}



int Out1(int state)
{
    if (state==1)
      digitalWrite(DOutIN1,HIGH);
    else
      digitalWrite(DOutIN1,LOW);  
}

void Out2(int state)
{
    if (state==1)
      digitalWrite(DOutIN2,HIGH);
    else
      digitalWrite(DOutIN2,LOW);  
}

void Out3(int state)
{
    if (state==1)
      digitalWrite(DOutIN3,HIGH);
    else
      digitalWrite(DOutIN3,LOW);  
}

void Out4(int state)
{
    if (state==1)
      digitalWrite(DOutIN4,HIGH);
    else
      digitalWrite(DOutIN4,LOW);  
}

void Out5(int state)
{
    if (state==1)
      digitalWrite(DOutIN5,HIGH);
    else
      digitalWrite(DOutIN5,LOW);  
}

void Out6(int state)
{
    if (state==1)
      digitalWrite(DOutIN6,HIGH);
    else
      digitalWrite(DOutIN6,LOW);  
}