#include <Servo.h>
#include <Cytron_PS2Shield.h>
#include <SoftwareSerial.h>
Cytron_PS2Shield ps2(10,11);
int tcnt,aaw=200,wat,cnt=0,za;
Servo myservo;
int pos = 0;


/* 初期設定 */
void setup() {
  myservo.attach(12);
  ps2.begin(9600);
  Serial.begin(9600);
  
  pinMode(2, 1);//output
  pinMode(3, 1);//output
  pinMode(4, 1);//output
  pinMode(5, 1);//output
  pinMode(6, 1);//output
  pinMode(7, 1);//output
  pinMode(8, 1);//output
   for (int i = 22; i <= 53; i++) {
    pinMode(i, 1);//output
  }
}


/* メイン */
void loop() {
 
  func1();func2();func3();func4();
  
  //check LEFT_X_joystick
  Serial.print("X Axis val:");
  Serial.print(ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS));
  Serial.print(" ");
  Serial.print("Y Axis val:");
  Serial.println(ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS));
  
  
  
   
}

//***  FUNCTION  ******************
void func1(){
  if(ps2.readButton(PS2_JOYSTICK_LEFT)==0){cnt++;delay(100);}
  if(ps2.readButton(PS2_JOYSTICK_RIGHT)==0){cnt--;delay(100);}
  
    if(cnt>=5){cnt=5;}
    if(cnt<=0){cnt=0;}
     if(cnt==0){aaw=200;}
     if(cnt==1){aaw=150;}
     if(cnt==2){aaw=100;}
     if(cnt==3){aaw=90;}
     if(cnt==4){aaw=80;}
     if(cnt==5){aaw=70;}
}
void func2(){
  if(ps2.readButton(PS2_UP)==0)  {digitalWrite(27, 1);analogWrite(6,255);}
  else if
    (ps2.readButton(PS2_DOWN)==0){digitalWrite(27, 0);analogWrite(6,255);}
  else
  {analogWrite(6,0);}
  
  if(ps2.readButton(PS2_LEFT_1)==0){za=0;}
  if(ps2.readButton(PS2_RIGHT_1)==0){za=90;}
  servo3(za);
  if(ps2.readButton(PS2_TRIANGLE)==0){digitalWrite(36, 1);}else{digitalWrite(36, 0);}
  if(ps2.readButton(PS2_CROSS)==0){digitalWrite(37, 1);}else{digitalWrite(37, 0);}
  if(ps2.readButton(PS2_SQUARE)==0){digitalWrite(35, 1);}else{digitalWrite(35, 0);}
  if(ps2.readButton(PS2_CIRCLE)==0){digitalWrite(34, 1);}else{digitalWrite(34, 0);}
  if(ps2.readButton(PS2_LEFT)==0){digitalWrite(32, 1);}else{digitalWrite(32, 0);}
  if(ps2.readButton(PS2_RIGHT)==0){digitalWrite(33, 1);}else{digitalWrite(33, 0);}
}
void func3(){
 if  (((ps2.readButton(PS2_JOYSTICK_RIGHT_X_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_RIGHT_X_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_RIGHT_Y_AXIS))>200))
       {digitalWrite(29, 1);analogWrite(8,255);}
   else if
      (((ps2.readButton(PS2_JOYSTICK_RIGHT_X_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_RIGHT_X_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_RIGHT_Y_AXIS))<50))
       {digitalWrite(29, 0);analogWrite(8,255);} 
   else
       {analogWrite(8,0);}
}


void func4(){
 if (((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))<50))
       {left();}
   else if
      (((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))>200))
       {r();}
   else if
      (((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))>200))
       {f();}
   else if
      (((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))>117) && 
       ((ps2.readButton(PS2_JOYSTICK_LEFT_X_AXIS))<137) &&
       ((ps2.readButton(PS2_JOYSTICK_LEFT_Y_AXIS))<50))
       {b();}
   else if
       (ps2.readButton(PS2_LEFT_2)==0){rl();}
   else if
       (ps2.readButton(PS2_RIGHT_2)==0){rr();}    
   else
       {st();} 
  
}
//****** Motor stop forward back right left seigyo **********
void st(){
  digitalWrite(22, 0);digitalWrite(23, 0);digitalWrite(24, 0);digitalWrite(25, 0);
  analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);analogWrite(7,0);
  
}
void f(){
  
  digitalWrite(22, 0);digitalWrite(23, 0);digitalWrite(24, 0);digitalWrite(25, 0);digitalWrite(28, 0);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);analogWrite(7,aaw);
}
void b(){
  digitalWrite(22, 1);digitalWrite(23, 1);digitalWrite(24, 1);digitalWrite(25, 1);digitalWrite(28, 1);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);analogWrite(7,aaw);
}
void r(){
  digitalWrite(22, 1);digitalWrite(23, 0);digitalWrite(24, 0);digitalWrite(25, 1);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
}
void left(){
  digitalWrite(22, 0);digitalWrite(23, 1);digitalWrite(24, 1);digitalWrite(25, 0);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
}
void rr(){
  digitalWrite(22, 1);digitalWrite(23, 0);digitalWrite(24, 1);digitalWrite(25, 0);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
}
void rl(){
  digitalWrite(22, 0);digitalWrite(23, 1);digitalWrite(24, 0);digitalWrite(25, 1);
  analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
}
void servo1(){
  myservo.write(0);delay(15);
  //analogWrite(12, 0);
}
void servo2(){  
  myservo.write(180);delay(15);
  //analogWrite(12, 0);
  
}
void servo3(int za){  
  myservo.write(za);delay(20);
  //analogWrite(12, 0);
  
}