Dynamixel AX12(1);
int L,R,CR=2800,CL=2800;
void setup(){
  InitAX12();
  InitIR();
}

void loop(){
  L=ADC(1);R=ADC(4);
  if(L>CL&&R>CR){
    fd(800);
  }
  else if(L<CL&&R>CR){
    sl(800);
  }
  else if(L>CL&&R<CR){
    sr(800);
  }
  else if(L<CL&&R<CR){
    fd(800);
  }

}

void InitAX12(){
// Dynamixel 2.0 Protocol -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps 
  AX12.begin(3);
// Disable CCW Angle Limit(L) to use wheel mode 
  AX12.writeWord(1,8,0);
  AX12.writeWord(2,8,0);
  AX12.writeWord(3,8,0);
  AX12.writeWord(4,8,0); 
  AX12.jointMode(5); 
}
void fd(int speed){
  AX12.writeWord(1,32,speed);
  AX12.writeWord(2,32,speed);
  AX12.writeWord(3,32,speed|0x400);
  AX12.writeWord(4,32,speed|0x400);
}
void bk(int speed){
  AX12.writeWord(1,32,speed|0x400);
  AX12.writeWord(2,32,speed|0x400);
  AX12.writeWord(3,32,speed);
  AX12.writeWord(4,32,speed);
}
void sl(int speed){
  AX12.writeWord(1,32,speed);
  AX12.writeWord(2,32,speed);
  AX12.writeWord(3,32,speed);
  AX12.writeWord(4,32,speed);
}
void sr(int speed){
  AX12.writeWord(1,32,speed|0x400);
  AX12.writeWord(2,32,speed|0x400);
  AX12.writeWord(3,32,speed|0x400);
  AX12.writeWord(4,32,speed|0x400);
}

int ADC(unsigned char CH){
  int A;
  if(CH==1){
    digitalWrite(7,1);delayMicroseconds(15);
    A=analogRead(2);
    digitalWrite(7,0);
    return A;
  }
  else{
    digitalWrite(13,1);delayMicroseconds(15);
    A=analogRead(1);
    digitalWrite(13,0);
    return A;
  }
}

void InitIR(){
  pinMode(2, INPUT_ANALOG); // set pin 0 as analog input
  pinMode(1, INPUT_ANALOG); // set pin 0 as analog input
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
  digitalWrite(12,0);
  digitalWrite(13,0);
  digitalWrite(6,0);
  digitalWrite(7,0);
}