MAIN_MiniRBC

#include <PS2X_lib.h>
#include <SimpleKalmanFilter.h>
SimpleKalmanFilter Ch0(10, 10, 5);
SimpleKalmanFilter Ch1(10, 10, 5);
SimpleKalmanFilter Ch2(10, 10, 5);
#define PS2_DAT 13 // data
#define PS2_CMD 11 //command
#define PS2_SEL 10 // attention
#define PS2_CLK 12 //clock
PS2X ps2x; // tạo PS2 điều khiển lớp
int error = 0;
byte type = 0;
byte vibrate = 0;

//Chẵn là dương thì thuận chiều, lẽ là dương thì ngược chiều

const byte MotorInput[4] = {3, 9, 10, 11};
const byte MotorOutput[4] = (5, 8, 12, 13};
//Pittong - Dùng High,Low
#define LeftHand 1
#define RightHand 2
#define MiddleHand 4
bool isCloseL =false; //Ktra tay trái đóng hay mở
bool isCloseR =false;
bool isCloseM = false;
bool isCloseBoth = false; //Ktra cả 2 tay đang đóng hay mở
bool isPress1 =false,isPress2 = false;
//Ổ bi,dùng analogWrite
#define obi_1 6
#define obi_2 7

//---------------------------------------------------------------------------------------------------------------------------------
void setup()
{
    Serial.begin(57600);
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, true,true);
    if (error == 0) {
        Serial.print("Đã tìm thấy bộ điều khiển ");
    }
    else if (error == 1)
        Serial.println("Không kết nối đc, thử lại...");

    else if (error == 2)
        Serial.println("Bộ điều khiển tìm thấy nhưng không chấp nhận lệnh");

    else if (error == 3)
        Serial.println("Bộ điều khiển từ chối để vào chế độ Pressures ,hoặc ko hỗ trợ");

    type = ps2x.readType();
    switch (type) {
    case 0:
        Serial.print("Tay điều khiển không phù hợp ");
        break;
    case 1:
        Serial.print("Đã tìm thấy DualShock ");
        break;
    }
}


int Channels[3] = {0};
long long int sys_start = 0;
int MAX_SPEED = 255;
int MIN_SPEED = -MAX_SPEED;
 //=======================================================================================================================================
void loop(){
  buttonState();
  ProcessMotor();
}

void buttonState(){
  if(error == 0){
      return;
  }else{
      ps2x.read_gamepad(false, vibrate);
      //vibrate = ps2x.Analog(PSAB_BLUE);

  }
    Channels[0] = map(ps2x.Analog(PSS_LY), 256, 0, -255, 255);
    Channels[0] = Ch0.updateEstimate(Channels[0]);
    Channels[1] = map(ps2x.Analog(PSS_LX), 0, 256, -255, 255);
    Channels[1] = Ch1.updateEstimate(Channels[1]);
    Channels[2] = map(ps2x.Analog(PSS_RX), 0, 256, -255, 255);
    Channels[2] = Ch2.updateEstimate(Channels[2]);

    for (int i = 0; i < 3; i++) {
      if (abs(Channels[i]) <= 100) {
        Channels[i] = 0;
      }
    }

    int sum = 0;
    for (int i = 0; i < 3; i++) {
      sum += abs(Channels[i]);
    }
    sum /= 3;
    if (sum < 20) {
      sys_start = millis();
    }

    Channels[2] /= 3.5;
    if (millis() - sys_start < 400) {
      Channels[2] = (float)(Channels[2] * ((millis() - sys_start + 310)) / 710);
    }

    taygap();
}

void taygap(){
  //----------------Ổ bi-------------------------------------------------------------------------

   //Khi bấm nút thì if đầu tiên đc gọi, nếu đang giữ nút PAD_UP thì gọi kéo từ từ lên,
   //Nếu đang giữ nút PAD_UP mà bấm PAD_DOWN sẽ k có gì mới xảy ra
   //Nếu thả nút PAD_UP thì if đầu tiên đc gọi , nếu ko giữ nút nữa thì dừng động cơ, tương tự kéo xuống

    if (ps2x.NewButtonState()){ // chỉ chạy 1 lần khi có bất kỳ 1 nút nào đó chuyển trạng thái on->off,off->on
      if(!isPress2){
         if(ps2x.Button(PSB_PAD_UP)){
           isPress1 = true;
           analogWrite(obi_1,100); //Nâng lên từ từ
           analogWrite(obi_2,0);
         }else{
           analogWrite(obi_1,0);
           analogWrite(obi_2,0);
           isPress1 = false;
         }
      }
      if(!isPress1){
         if(ps2x.Button(PSB_PAD_DOWN)){
           isPress2 = true;
           analogWrite(obi_1,0);
           analogWrite(obi_2,100); //Nâng lên từ từ
         }else{
           isPress2 = false;
           analogWrite(obi_1,0);
           analogWrite(obi_2,0);
         }
      }
    }


    //------------------Tay Gắp--------------------------------------


    if(ps2x.ButtonPressed(PSB_PINK)){ //Tay Trái
      //Code trong này chỉ chạy 1 lần dù có nhấn giữ
       if(!isCloseL){
         digitalWrite(LeftHand,HIGH);
         isCloseL = true;
      }else{
         digitalWrite(LeftHand,LOW);
         isCloseL = false;
      }
    }

    if(ps2x.ButtonPressed(PSB_RED)){ //Tay Phải
      //Code trong này chỉ chạy 1 lần dù có nhấn giữ
       if(!isCloseR){
         digitalWrite(RightHand,HIGH);
         isCloseR = true;
      }else{
         digitalWrite(RightHand,LOW);
         isCloseR = false;
      }
    }

    if(ps2x.ButtonPressed(PSB_GREEN)){ //Tay Giữa
       if(!isCloseM){
         digitalWrite(MiddleHand,HIGH);
         isCloseM = true;
      }else{
         digitalWrite(MiddleHand,LOW);
         isCloseM = false;
      }
    }

    if(ps2x.ButtonPressed(PSB_BLUE)){ //Đóng mở 2 tay cùng lúc
      //Code trong này chỉ chạy 1 lần dù có nhấn giữ
       if(!isCloseBoth){
         digitalWrite(LeftHand,HIGH);
         digitalWrite(RightHand,HIGH);
         isCloseL = true;
         isCloseR = true;
      }else{
         digitalWrite(LeftHand,LOW);
         digitalWrite(RightHand,LOW);
         isCloseL = false;
         isCloseR = false;
      }
    }

}