//https://www.youtube.com/watch?v=FLNgkYBkLjA
//https://github.com/un0038998/PS3ControlledCar
//https://www.youtube.com/watch?v=zgu0nZ9kNl4
//PS3 BT Mac address "C8:21:58:C4:C6:3C"

#include <Ps3Controller.h>
const int dataPin = 12;   /* DS UNO-D4 esP32-IO12*/
const int clockPin = 17;  /* SHCP UNO-D8 esP32-IO17*/
const int latchPin = 19;  /* STCP UNO-D12 esP32-IO19 */

#define PWMA 23 //A電機轉向
#define PWMB 25 //B電機轉向
#define PWMC 16 //C電機轉向
#define PWMD 27 //D電機轉向
int M1_F = 0b11100100; 
int M1_B = 0b00011011;
int M1_L = 0b11000001;
int M1_R = 0b00010110;
int M1_S = 0b11111111;
//控制電機運動  宏定義
//#define MOTOR_GO_FORWARD  {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_F);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, 200);ledcWrite(PWMSpeedChannel4, 200);}
#define MOTOR_GO_FORWARD  {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_F);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, (abs(lY)+120));ledcWrite(PWMSpeedChannel4, (abs(lY)+120));} //車體前進                             
#define MOTOR_GO_BACK     {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_B);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, (abs(lY)+120));ledcWrite(PWMSpeedChannel4, (abs(lY)+120));}   //車體後退
#define MOTOR_GO_LEFT     {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_L);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, (abs(lX)+120));ledcWrite(PWMSpeedChannel4, (abs(lX)+120));}  //車體左轉
#define MOTOR_GO_RIGHT    {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_R);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, (abs(lX)+120));ledcWrite(PWMSpeedChannel4, (abs(lX)+120));}  //車體右轉
#define MOTOR_GO_STOP     {digitalWrite(latchPin, LOW);shiftOut(dataPin, clockPin, LSBFIRST, M1_S);digitalWrite(latchPin, HIGH);ledcWrite(PWMSpeedChannel3, 0);ledcWrite(PWMSpeedChannel4, 0);}       //Brake

const int PWMFreq = 30000; /* 1 KHz */
const int PWMResolution = 8;
const int PWMSpeedChannel1 = 1;
const int PWMSpeedChannel2 = 2;
const int PWMSpeedChannel3 = 3;
const int PWMSpeedChannel4 = 4;
int dutyCycle = 200;


//遙桿位置參數 X由左-128至右127, Y由上-128至下127
 int rX; 
 int rY;
 int lX;
 int lY;

void moveCar()
{
  if(lY < -50){ 
    //Serial.println("Forward");
    MOTOR_GO_FORWARD;                  
  }
  else if (lY > 50){ 
    //Serial.println("Backward");
    MOTOR_GO_BACK;
  }
  else if (lX < -50){ 
    //Serial.println("Left");
    MOTOR_GO_LEFT;
  }
  else if (lX > 50){ 
    //Serial.println("Right");
    MOTOR_GO_RIGHT;
  }
  else{ 
    //Serial.println("Stop");
    MOTOR_GO_STOP; 
  }
}

void setUpPinModes()
{
   pinMode(dataPin, OUTPUT);
   pinMode(clockPin, OUTPUT);
   pinMode(latchPin, OUTPUT);
   pinMode(14, OUTPUT);
   digitalWrite(14, LOW);
   pinMode(PWMA, OUTPUT);
   pinMode(PWMB, OUTPUT);
   pinMode(PWMC, OUTPUT);
   pinMode(PWMD, OUTPUT);
  ledcSetup(PWMSpeedChannel1, PWMFreq, PWMResolution);
  ledcSetup(PWMSpeedChannel2, PWMFreq, PWMResolution);
  ledcSetup(PWMSpeedChannel3, PWMFreq, PWMResolution);
  ledcSetup(PWMSpeedChannel4, PWMFreq, PWMResolution);
  ledcAttachPin(PWMA, PWMSpeedChannel1);
  ledcAttachPin(PWMB, PWMSpeedChannel2); 
  ledcAttachPin(PWMC, PWMSpeedChannel3);
  ledcAttachPin(PWMD, PWMSpeedChannel4);   
  ledcWrite(PWMSpeedChannel1, 0);    
  ledcWrite(PWMSpeedChannel2, 0); 
  ledcWrite(PWMSpeedChannel3, 0);    
  ledcWrite(PWMSpeedChannel4, 0);
}

void setup() {
  Serial.begin(115200);
  setUpPinModes();
  Ps3.begin("e4:5f:01:08:d4:53");//PS3手制MAC地址
  Serial.println("Ready.");
 
}

void loop() {
  if(Ps3.isConnected()){
   lX =(Ps3.data.analog.stick.lx);
   lY =(Ps3.data.analog.stick.ly);
   rX =(Ps3.data.analog.stick.rx);
   rY =(Ps3.data.analog.stick.ry);
   //Serial.print(lX);
  // Serial.print(" ");
   //Serial.print(lY);
   //Serial.print(" ");
   //Serial.print(rX);
   //Serial.print(" ");
   //Serial.println(rY);
   moveCar();
}}