//PS2 ワイヤレスコントローラーで動かす
//PS2ライブラリー
#include <PS2X_lib.h>

/******************************************************************
* set pins connected to PS2 controller:
* - 1e column: original
* - 2e colmun: Stef?
* replace pin numbers by the ones you use
******************************************************************/
#define PS2_DAT 13 //14
#define PS2_CMD 11 //15
#define PS2_SEL 10 //16
#define PS2_CLK 12 //17

/******************************************************************
* select modes of PS2 controller:
* - pressures = analog reading of push-butttons
* - rumble = motor rumbling
* uncomment 1 of the lines for each mode selection
******************************************************************/
//#define pressures true
#define pressures false
//#define rumble true
#define rumble false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;


//モーターシールドとArduinoのピンコネクション
#define DIRA1 7
#define DIRA2 8
#define PWMA 5

/***************************************************
Servo run forward(512), and backward(102) depending on LX,LY position....
****************************************************/
//サーボ関連ライブラリー
#include <Servo.h>
Servo myservo1; // create servo object to control a servo
// twelve servo objects can be created on most boards
int posX = 113; // variable to store the servo position
int posX_ex =113;
int servo_angle_min_X = 94; //right
int servo_angle_max_X = 127; //left

//モータースピード変数
int speedFL;
int speedBL;

//ここからsetup関数

void setup(){
//PS2 コントロール
Serial.begin(57600);
// Serial.begin(115200);
delay(500); //added delay to give wireless ps2 module some time to startup, before configuring it

//CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************

//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

if(error == 0){
Serial.print("Found Controller, configured successful ");
Serial.print("pressures = ");
if (pressures)
Serial.println("true ");
else
Serial.println("false");
Serial.print("rumble = ");
if (rumble)
Serial.println("true)");
else
Serial.println("false");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
}
else if(error == 1)
Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

else if(error == 2)
Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

else if(error == 3)
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type found ");
break;
case 1:
Serial.println("DualShock Controller found ");
break;
case 2:
Serial.println("GuitarHero Controller found ");
break;
case 3:
Serial.println("Wireless Sony DualShock Controller found ");
break;
}

//モーター駆動用ピン出力
pinMode(DIRA1, OUTPUT);
pinMode(DIRA2, OUTPUT);
pinMode(PWMA, OUTPUT);


//サーボのセットアップ
myservo1.attach(9); // attaches the servo on pin 9 to the servo object
myservo1.write(posX_ex);

} //setup関数の最後


//Motor Drive の関数

void forwardLeftMotor(int speedFL)//正転
{
digitalWrite(DIRA1,HIGH);
digitalWrite(DIRA2,LOW);
analogWrite(PWMA, speedFL);
}

void backLeftMotor(int speedBL)//逆転
{
digitalWrite(DIRA1,LOW);
digitalWrite(DIRA2,HIGH);
analogWrite(PWMA, speedBL);
}

void stopLeftMotor() //停止
{
digitalWrite(DIRA1,HIGH);
digitalWrite(DIRA2,HIGH);
analogWrite(PWMA, 0);
}


// loop関数の始まり
void loop(){
/* You must Read Gamepad to get new values and set vibration values
ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
if you don't enable the rumble, use ps2x.read_gamepad(); with no values
You should call this at least once a second
*/
if(error == 1){ //skip loop if no controller found
resetFunc();
}

if(type == 2){ //Guitar Hero Controller
ps2x.read_gamepad(); //read controller
}
else { //DualShock Controller
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);

//Joystickの位置を読んで変数に代入する
int LY=ps2x.Analog(PSS_LY);
int LX=ps2x.Analog(PSS_LX);
int RY=ps2x.Analog(PSS_RY);
int RX=ps2x.Analog(PSS_RX);

if (RY<118) //Forward前進
{
speedFL = 2*(127-RY);
forwardLeftMotor(speedFL); //過電圧(5v)が130モーターにかかり続けないよう擬似的に50%maxでPWMさせる
}

if (RY>138) //右Backward後進 Fast dekay (smaller torque)DRV8833
{
speedBL =2*(RY-128) ;
backLeftMotor(speedBL); //過電圧(5v)が130モーターにかかり続けないよう擬似的に55%maxでPWMさせる
}

if (RY>=118 && RY<=138) //Stop motor
{
stopLeftMotor();
}



/* driving servoes*/
//サーボの 駆動

if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128;
myservo1.write(posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128;
myservo1.write(posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
myservo1.write(posX);
}


} //else Dualshock controllerのブレース
delay(50);

} //loop関数の終わり、loop関数のはじめに戻ってloop関数をエンドレスで繰り返す