コスパ最強!?ロボットアームの作り方_その⑤_プログラム

//-----PS4 Bluetooth通信用設定
#include <PS4BT.h>
#include <usbhub.h>

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>

USB Usb;
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so

/* You can create the instance of the PS4BT class in two ways */
PS4BT PS4(&Btd, PAIR);

bool printAngle, printTouch;
uint8_t oldL2Value, oldR2Value;
//-----PS4 Bluetooth通信end

//-----サーボ制御用設定
#include <PCA9685.h> //PCA9685用ヘッダーファイル
PCA9685 pwm = PCA9685(0x40);    //PCA9685のアドレス指定

//-----各変数の設定
#define SERVOMIN 150            //最小パルス幅 
#define SERVOMAX 500            //最大パルス幅
#define servo_01 0              //サーボ1のピンの設定
#define servo_02 1              //サーボ2のピンの設定
#define servo_03 2              //サーボ3のピンの設定
#define servo_04 3              //サーボ4のピンの設定
#define servo_05 4              //サーボ5のピンの設定
#define servo_06 5              //サーボ6のピンの設定

double arg_01 = 90;             // サーボの角度
double arg_02 = 180;            // サーボの角度
double arg_03 = 135;            // サーボの角度
double arg_04 = 135;            // サーボの角度
double arg_05 = 90;             // サーボの角度
double arg_06 = 90;             // サーボの角度
 
int X_POS_L = 128;              // LスティックのX軸方向の読み取り値
int Y_POS_L = 128;              // LスティックのY軸方向の読み取り値
int X_POS_R = 128;              // RスティックのX軸方向の読み取り値
int Y_POS_R = 128;              // RスティックのY軸方向の読み取り値

int GRIP_servo5 = 128;          // サーボ5の入力(L1,R1)
int GRIP_L2 = 128;              // アームのグリップ部 閉まる方向
int GRIP_R2 = 128;              // アームのグリップ部 開く方向

int i = 0;
int arg_x = 0;
//-----サーボ制御end


//-----初期設定
void setup() {
 pwm.begin();                   //初期設定 (アドレス0x40用)
 pwm.setPWMFreq(50);            //PWM周期を50Hzに設定 (アドレス0x40用)        

 initial_position();  //ポジション初期化

  //-----PS4 Bluetooth通信
   Serial.begin(115200);        // シリアル通信の開始
#if !defined(__MIPSEL__)
  // Wait for serial port to connect - used on Leonardo, 
  // Teensy and other boards with built-in USB CDC serial connection
  while (!Serial);              
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1);                  // Halt
  }
  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
  //-----PS4 Bluetooth通信
}

//-----メインループ
void loop() {
  //-----PS4 Bluetooth通信
  Usb.Task();
  if (PS4.connected()) {
    
    //-----L1、R1の入力値を200 or 0に変換
    if (PS4.getButtonClick(L1))
      GRIP_servo5 = 200;
    if (PS4.getButtonClick(R1))
      GRIP_servo5 = 0;

    //-----6回に1回コントローラーのアナログ入力を取得する（過敏に反応しすぎるため）
    if(i == 0){
      //ボタンの入力を取得
      X_POS_L = PS4.getAnalogHat(LeftHatX);  // LスティックのX軸方向のアナログ値を読み取る
      Y_POS_L = PS4.getAnalogHat(LeftHatY);  // LスティックのY軸方向のアナログ値を読み取る
      X_POS_R = PS4.getAnalogHat(RightHatX); // RスティックのX軸方向のアナログ値を読み取る
      Y_POS_R = PS4.getAnalogHat(RightHatY); // RスティックのY軸方向のアナログ値を読み取る

      GRIP_L2 = PS4.getAnalogButton(L2);     // L2のアナログ値を読み取る
      GRIP_R2 = PS4.getAnalogButton(R2);     // R2のアナログ値を読み取る
    }

    //-----オプションボタンが押されたら通信終了_Aruduinoリセット
    if (PS4.getButtonClick(OPTIONS)) {
      Serial.print(F("\r\nOptions"));
      PS4.disconnect();           //Bluetooth通信を遮断

      /*Arduinoリセット時にサーボ2が暴れる可能性があるので、
        5回に分けて初期位置に戻す*/
      arg_x = (180 - arg_02)/5;
      for(int i=0; i < 5; i++){
          arg_02 += arg_x; 
          servo_write(servo_02, arg_02);
          delay(500); 
      }
      
      software_reset();         //Arduinoリセット
    }

    i = i + 1;

    if(i == 5){
      i = 0;
    }
  }

  //-----PS4 Bluetooth通信

    
  //-----サーボモーター用にコントローラー入力→角度_変換
  arg_01 = arg_01 + convert_01(X_POS_L, arg_01);
  arg_02 = arg_02 + convert_02(Y_POS_L, arg_02);
  arg_03 = arg_03 + convert_03(Y_POS_R, arg_03);
  arg_04 = arg_04 + convert_03(X_POS_R, arg_04);
  arg_05 = arg_05 + 20 * convert_01(GRIP_servo5, arg_05); //2度×20＝40度ずつ制御
  arg_06 = arg_06 + convert_04(GRIP_L2, GRIP_R2, arg_06);


  //-----サーボ制御（関数呼び出し）
  servo_write(servo_01, arg_01);
  servo_write(servo_02, arg_02);
  servo_write(servo_03, arg_03);
  servo_write(servo_04, arg_04);
  servo_write(servo_05, arg_05);
  servo_write(servo_06, arg_06);


  //-----入力リセット
  X_POS_L = 128;      // LスティックのX軸方向の読み取り値
  Y_POS_L = 128;      // LスティックのY軸方向の読み取り値
  X_POS_R = 128;      // RスティックのX軸方向の読み取り値
  Y_POS_R = 128;      // RスティックのY軸方向の読み取り値

  GRIP_L2 = 128;      // アームのグリップ部 閉まる方向
  GRIP_R2 = 128;      // アームのグリップ部 開く方向
  GRIP_servo5 = 128;  //サーボ5

}
//-----メインループend


/////-----ここから下は呼び出し用の関数-----/////

//サーボ出力用_サーボ番号、角度→出力_関数
void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定

　//角度（0～180）をPWMのパルス幅（150～500）に変換
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); 
  pwm.setPWM(ch, 0, ang);
}

//-----サーボ位置初期化用関数
void initial_position(){ //動かすサーボチャンネルと角度を指定

  arg_01 = 90;    // サーボの角度
  arg_02 = 180;   // サーボの角度
  arg_03 = 135;   // サーボの角度
  arg_04 = 135;   // サーボの角度
  arg_05 = 90;    // サーボの角度
  arg_06 = 90;    // サーボの角度

  //サーボ出力
  servo_write(servo_01, arg_01);
  delay(500); 
  servo_write(servo_02, arg_02);
  delay(500); 
  servo_write(servo_03, arg_03);
  delay(500); 
  servo_write(servo_04, arg_04);
  delay(500); 
  servo_write(servo_05, arg_05);
  delay(500); 
  servo_write(servo_06, arg_06);
  delay(500); 
}

//-----リセット用関数
void software_reset() {
  asm volatile ("  jmp 0");  
} 

//-----サーボ1、サーボ5用_コントローラー入力→角度_変換関数
int convert_01(int input_x, int arg_x){
  if(arg_x == 170 && input_x > 190){      //サーボ角度制御、170度以下
    return 0;
  }
  else if (arg_x == 10 && input_x < 65){  //サーボ角度制御、10度以上
    return 0;
  } 
  else if(input_x > 190){     //2度ずつ増加
    return 2;
  } 
  else if(input_x < 65){      //2度ずつ減少
    return -2;
  }
  else{
    return 0;
  }
}

//-----サーボ2用_コントローラー入力→角度_変換関数
int convert_02(int input_x, int arg_x){
  if(arg_x == 180 && input_x > 190){      //サーボ角度制御、180度以下
    return 0;
  }
  else if (arg_x == 45 && input_x < 65){  //サーボ角度制御、45度以上
    return 0;
  } 
  else if(input_x > 190){     //1度ずつ増加
    return 1;
  } 
  else if(input_x < 65){      //1度ずつ減少
    return -1;
  }
  else{
    return 0;
  }
}

//-----サーボ3、サーボ4用_コントローラー入力→角度_変換関数
int convert_03(int input_x, int arg_x){
  if(arg_x == 135 && input_x < 65){        //サーボ角度制御、135度以下
    return 0;
  }
  else if (arg_x == 45 && input_x > 190){  //サーボ角度制御、45度以上
    return 0;
  } 
  else if(input_x > 190){     //1度ずつ減少
    return -1;
  } 
  else if(input_x < 65){      //1度ずつ増加
    return 1;
  }
  else{
    return 0;
  }
}

//サーボ6用_コントローラー入力→角度_変換関数
int convert_04(int input_L, int input_R, int arg_x){
  if(arg_x == 90 && input_L > 190){        //サーボ角度制御、90度以下
    return 0;
  }
  else if (arg_x == 20 && input_R > 190){  //サーボ角度制御、20度以上
    return 0;
  }
  else if(input_L > 190 && input_R > 190){  //L2、R2同時押しは無効化
    return 0;
  }
  else if(input_L > 190){     //5度ずつ増加
    return 5;
  } 
  else if(input_R > 190){     //5度ずつ減少
    return -5;
  }
  else{
    return 0;
  }
}