//++++++++++++++++++++++++//This sketch created by 1016blog
//#include <FlexiTimer2.h>
int seg[] = {0xff, 0x0a, 0x05, 0x00, 0x0f, 0x09, 0x06, 0x01, 0x04, 0x04, 0x08, 0x77, 0x7c, 0x58, 0x5e, 0x79, 0x71};
int R3f=0,L3f=0;
int Flag1=1,Flag2=0,Flag3=0,FlagS1=1,FlagS2=0;
int tcnt,aaw,wat;
int pw=0,i=10,j0=10,k=10,m=10,n=10,jz=10,jy=10,jx=10,j2=10,ja=10,jb=10;


#include  <PS3BT.h>
//*********servo_write*********************
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN 100 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 2500 // this is the 'maximum' pulse length count (out of 4096)

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

USB Usb;
BTD Btd(&Usb);
PS3BT PS3(&Btd);

void setup() {
Serial.begin(1151);
if (Usb.Init() == -1) {
Serial.print(F("\r\nOSC did not start"));
while (1); //halt
}
Serial.print(F("\r\nPS3 Bluetooth Library Started"));

 for (int i = 2; i <= 53; i++) {
    pinMode(i, 1);
  }
  for (int i = 54; i <= 59; i++) {
    pinMode(i, 0);
  }
//***************  FlexiTimer2 ***************  
//  FlexiTimer2::set(1, warikomi);
//  FlexiTimer2::start();
//***************  servo_write***************
pwm.begin();
pwm.setPWMFreq(50); // Analog servos run at ~50 Hz updates
//servo_write(0,90);
//******************************
}

void loop() {
//******************************
Usb.Task();

if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
//**************************************************************************  
//ホームポジション
aaw=100;
if ((PS3.getAnalogHat(LeftHatX) > 117 && PS3.getAnalogHat(LeftHatX) < 137) && (PS3.getAnalogHat(LeftHatY) > 117 && PS3.getAnalogHat(LeftHatY) < 137)){
PORTA=seg[0];analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0); 
}
//前進
else if((PS3.getAnalogHat(LeftHatX) > 107 && PS3.getAnalogHat(LeftHatX) < 147) && (PS3.getAnalogHat(LeftHatY) < 116)){
PORTA=seg[1];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);

}
//後退
else if((PS3.getAnalogHat(LeftHatX) > 107 && PS3.getAnalogHat(LeftHatX) < 147) && (PS3.getAnalogHat(LeftHatY) > 138)){
PORTA=seg[2];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);

}
//右
else if((PS3.getAnalogHat(LeftHatX) > 138) && (PS3.getAnalogHat(LeftHatY) > 107 && PS3.getAnalogHat(LeftHatY) < 147)){
PORTA=seg[3];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);

}
//左
else if(( PS3.getAnalogHat(LeftHatX) < 116) && (PS3.getAnalogHat(LeftHatY) > 107 && PS3.getAnalogHat(LeftHatY) < 147)){
PORTA=seg[4];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);

}
//すき間
else{
PORTA=seg[0];analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0); 
}
//左旋回
 if (PS3.getAnalogButton(L2) ) {
    PORTA=seg[5];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);  
 }
 //右旋回
 if ( PS3.getAnalogButton(R2)) {
    PORTA=seg[6];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 }
//************************************************************************** 
//digitalWrite(33, 1*(PS3.getButtonPress(DOWN)));
//digitalWrite(32, 1*(PS3.getButtonPress(UP)));
//digitalWrite(30, 1*(PS3.getButtonPress(RIGHT)));
//digitalWrite(31, 1*(PS3.getButtonPress(LEFT)));
//digitalWrite(34, 1*(PS3.getButtonPress(CROSS)));
//digitalWrite(35, 1*(PS3.getButtonPress(TRIANGLE)));
//digitalWrite(36, 1*(PS3.getButtonPress(CIRCLE)));
//digitalWrite(37, 1*(PS3.getButtonPress(SQUARE)));
//************************************************
if (PS3.getButtonPress(L1)) {
  servo_M0(1);
}
if (PS3.getButtonPress(R1)) {
  servo_M0(-1);
}
//0番ピンのサーボに角度10度を指示
  //  servo_write(0,i);delay(1000);
//************************************************
if (PS3.getButtonPress(L3)) {
  servo_M15(1);
}
if (PS3.getButtonPress(R3)) { 
  servo_M15(-1);
}    
//************************************************
if ((PS3.getAnalogHat(RightHatX) > 117 && PS3.getAnalogHat(RightHatX) < 137) && (PS3.getAnalogHat(RightHatY) > 117 && PS3.getAnalogHat(RightHatY) < 137)){
 
  
}
//上昇
else if((PS3.getAnalogHat(RightHatX) > 107 && PS3.getAnalogHat(RightHatX) < 147) && (PS3.getAnalogHat(RightHatY) < 116)){
  servo_M1(1);
}
//下降
else if((PS3.getAnalogHat(RightHatX) > 107 && PS3.getAnalogHat(RightHatX) < 147) && (PS3.getAnalogHat(RightHatY) > 138)){
  servo_M1(-1);
}
//出
else if((PS3.getAnalogHat(RightHatX) > 138) && (PS3.getAnalogHat(RightHatY) > 107 && PS3.getAnalogHat(RightHatY) < 147)){
 servo_M3(1);
}
//入
else if(( PS3.getAnalogHat(RightHatX) < 116) && (PS3.getAnalogHat(RightHatY) > 107 && PS3.getAnalogHat(RightHatY) < 147)){
 servo_M3(-1); 
}
else{
 
}
//************************************************ 行きの動作

//if (PS3.getButtonClick(START) && (Flag1==1)){
  if (PS3.getButtonClick(START)){
 
  aaw=100;wat=100;
//ものづくり研究部：計４名：キャプテン：野口晴（１年）：副キャプテン：浅川虎之助（１年）：筆記：田村真悟（１年）：いろいろ：中岡陽向（１年）
  foward(1000);right(1300);
  back(1000); right(1500);
  foward(1250);right(2500);
  back(1250); right(1500);foward(3000);

  Flag1=0;
  Flag2=1;
}

//************************************************　帰りの動作
if (PS3.getButtonClick(SELECT) && (Flag2==1)){
  aaw=100;wat=100;
   back(2900);left(3000);
   foward(1250);left(1100);
   back(1250);left(1250);
  foward(1250);left(1200);back(1250);
}

//************************************************
//if (PS3.getButtonPress(TRIANGLE)) {servo_M14(1);} 
if (PS3.getButtonPress(TRIANGLE)) {servo_M14(1);} 
if (PS3.getButtonPress(CROSS)) {servo_M14(-1);}    
if (PS3.getButtonPress(CIRCLE)) {servo_M13(1);} 
if (PS3.getButtonPress(SQUARE)) {servo_M13(-1);} 

if (PS3.PS3Connected) {

if (PS3.getButtonClick(PS)) {
PS3.disconnect();
}
}
}
}
//************** servo_M() **************************** 
void servo_M0(int n){ 

    j0=j0+n;
    if(j0>28){j0=28;}
    if(j0<2){j0=2;}
    servo_write(0,j0);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(30);
}
void servo_M1(int n){ 

    ja=ja+n;
    if(ja>28){ja=28;}
    if(ja<2){ja=2;}
    servo_write(1,ja);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(30);
}
void servo_M3(int n){ 

    jb=jb+n;
    if(jb>15){jb=15;}
    if(jb<0){jb=0;}
    servo_write(3,jb);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(20);
}   
void servo_M15(int n){ 
 
    jz=jz+n;
    if(jz>25){jz=25;}
    if(jz<1){jz=1;}
    servo_write(15,jz);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(30);
}
void servo_M14(int n){ 
 
    jy=jy+n;
    if(jy>25){jy=25;}
    if(jy<1){jy=1;}
    servo_write(14,jy);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(30);
}   
void servo_M13(int n){ 
 
    jx=jx+n;
    if(jx>25){jx=25;}
    if(jx<1){jx=1;}
    servo_write(13,jx);//一番下//1番ピンのサーボにservo_write(1,j);delay(1000); 
    //m=0;一番下/m=1;2番下/m=3;3番下/m=13;四番先/m=14;五番先/m=15;一番先
    delay(30);
}     
   /* servo_write(1,j);//二番目下
    servo_write(3,j);//三番番先
    servo_write(13,1);//四番先
    servo_write(14,1);delay(50);//五番先
    i=i+FlagS1; 
    if(i>20){FlagS1=-1;}
    if(i<5){FlagS1=1;}
    servo_write(15,i);delay(50);//一番先*/

//************** servo_write ***************************
void servo_write(int n, int ang){ //動かすサーボと角度を引数に持つ
ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～600）へ変換
pwm.setPWM(n, 0, ang);
}
//*****************************************
void foward(int a){
 PORTA=seg[1];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a); 
 PORTA=seg[0];analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(100);
}
void back(int a){
 PORTA=seg[2];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a);
 PORTA=seg[0]; analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(wat);
}
void left(int a){
 PORTA=seg[4];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a);
 PORTA=seg[0]; analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(wat); 
}
void right(int a){
  PORTA=seg[3];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a);
 PORTA=seg[0]; analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(wat);
}
void rotL(int a){
 PORTA=seg[5];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a);
 PORTA=seg[0]; analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(wat); 
}
void rotR(int a){
  PORTA=seg[6];analogWrite(2,aaw);analogWrite(3,aaw);analogWrite(4,aaw);analogWrite(5,aaw);
 delay(a);
 PORTA=seg[0]; analogWrite(2,0);analogWrite(3,0);analogWrite(4,0);analogWrite(5,0);
 delay(wat);
}
//********************************
void denatu1(){
  digitalWrite(40, 0);
  digitalWrite(41, 1);//電圧半分 
}
void denatu2(){
  digitalWrite(40, 1);
  digitalWrite(41, 1);//電圧半分4/6 
}
//********************************warikomi
void warikomi() {
  tcnt++;
  //swc++;
  if(tcnt>=20){
    servo();
    tcnt=0;
  }

}
void servo(){
  digitalWrite(51, 1);
  delayMicroseconds(pw);
  digitalWrite(51, 0); 
}