找資料?
< All Topics
Print

【通訊】GoARM/SUMO PS2搖桿模式

Posted
Updated
Byzerovoyager

簡介

機器人的控制領域中，指導器(TEACH-BOX)是一種常見且重要的裝置，扮演著人機介面中的關鍵環節。在程式學習階段，我們能導入並建立【指導器(TEACH-BOX)】的應用思維與素養嗎?

我想這答案是肯定的。雖然，高階複雜功能的機器人，TEACH-BOX難免功能繁複，一個TEACH-BOX程式開發也非簡單容易上手的事情。但是，由練習通訊、命令定義、程式演算法…的學習角度觀之，導入TEACH-BOX的學習觀念與基礎知識技能，並不是難事，也有其必要性。不管是介面直覺簡單，或是指令導向的TEACH-BOX，其中可程式化的研發、演算法設計過程，是不可少的。

例如，在我們DUAL22專題中APP應用程式已經是建立最基礎簡單的TEACH-BOX基礎觀念了。未來功夫越深，韌體與APP程式能力越好，自然可設計出多元、深度的TEACH-BOX控制器功能。除了APP之外，我們也可以使用常見的PS2搖桿(arduino用)，進行韌體程式的練習。這也是這篇單元的重點囉。

首先，要建立【GoARM | GoSUMO 機器人的PS2搖桿模式】，你需要下列材料(如下圖)。

接線規則如下圖，DUAL22開發板端接線位置：

  1. G34 ： DI (data)
  2. G17 ： DO (command)
  3. G16 ： SEL (select)
  4. G21 ： CLK (clock)
  5. 紅色排針(5V) ： 5V
  6. 黑色排針(GND) ： GND

上述4條訊號線(DI、DO、SEL、CLK)接錯，還可以再調整。電源端(紅色5V、黑色GND)別接錯囉，上電前多看兩眼準沒錯。另外，為了大幅降低小白們接錯機率與保有接線彈性，我提供防呆PS2快接座與接線束給大家使用。

重要提醒：接線操作要斷電處理。上課時經常看到可愛的小白們直接接線抽拔。沒燒，算你好運呦~~

影片

函數庫

函數庫載點，請參考【ArduinoIDE-ESP32編輯環境】。

#include <DUAL22.h>

#include <Servo.h>

#include <PS2X_lib.h>

程式範例

注意：此範例的伺服電機(MG9xx)，控制腳位是【G2】與【G15】，如下程式範例52、53行。

/*-- 註解區
> GoARM - PS2 | ESP32 | DUAL22專題範例程式
--*/
/*-- 標頭檔區(程式庫) --*/
#include <DUAL22.h>
#include <Servo.h>
#include <PS2X_lib.h>

/*-- 宣告區(全域變數) --*/
int SeekCnt = 0;

//Servo Motor 變數區 & 機械臂微調區 -------
//重要，詳見影片介紹：https://youtu.be/PnZQBKgZXok
Servo servoBase;
Servo servoPaw;
int homeBase = 90; //機械臂原點角度。
int homePaw = 90;  //機械爪原點角度。

int posBase = homeBase;
int posBaseMAX = 170; //機械臂最大抬升角度。
int posBaseMIN = 90;  //機械臂最小降低角度。
int posPaw = homePaw;
int posPawMAX = 98;   //機械爪最大開啟角度。
int posPawMIN = 60;   //機械爪最小閉合角度。

//PS2X Pins & Setting-----------------
#define PS2_DAT        34  //input pin,  DI
#define PS2_CMD        17  //output pin, DO
#define PS2_SEL        16  //output pin
#define PS2_CLK        21  //output pin
#define pressures   false
#define rumble      false
PS2X ps2x; // create PS2 Controller Class
int error = -1;
byte PS2X_type = 0;
byte vibrate = 0;
int tryNum = 1;

int PWMSpeed_MAX = 1023; //DC電機最大PWM值(速度)
int PWMSpeed_MIN = 150;  //DC電機最小PWM值(速度)
//-----------------

/*-- 初始化區 --*/
void setup()
{
  DUAL22Inital();  //進行GoSUMO機器人初始化, Serial baud rate = 115200
  Serial.begin(115200);
  //初始化PS2搖桿
  PS2X_INIT();

  //ServoMotor初始化
  servoBase.attach(2);
  servoPaw.attach(15);
  ServoARM("Home", "");  
}

/*-- 主程式區(重複執行) --*/
void loop()
{
   if(PS2X_type != 2){ 
    ps2x.read_gamepad(false, vibrate);  //讀取PS2命令

    if(ps2x.Analog(PSS_RY) <= 118){ GS_FW(map(ps2x.Analog(PSS_RY), 128, 0, PWMSpeed_MIN, PWMSpeed_MAX)); }          //前進 
    if(ps2x.Analog(PSS_RY) >= 138){ GS_BW(map(ps2x.Analog(PSS_RY), 128, 255, PWMSpeed_MIN, PWMSpeed_MAX)); }        //後退    
    if(ps2x.Analog(PSS_RX) <= 118){ GS_LEFT(map(ps2x.Analog(PSS_RX), 128, 0, PWMSpeed_MIN, PWMSpeed_MAX-200)); }    //左旋轉 
    if(ps2x.Analog(PSS_RX) >= 138){ GS_RIGHT(map(ps2x.Analog(PSS_RX), 128, 255,  PWMSpeed_MIN, PWMSpeed_MAX-200)); }//右旋轉
    if((ps2x.Analog(PSS_RX) == 128) && (ps2x.Analog(PSS_RY) == 128)){ GS_STOP(); }

    if(ps2x.Analog(PSS_LX) <= 118){ ServoARM("Paw", "OPEN");  }    
    if(ps2x.Analog(PSS_LX) >= 138){ ServoARM("Paw", "CLOSE"); }  
    if(ps2x.Analog(PSS_LY) <= 118){ ServoARM("Base", "DOWN"); }    
    if(ps2x.Analog(PSS_LY) >= 138){ ServoARM("Base", "UP");   }   

    if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) {
      ServoARM("Home", "");
    }
  }  
  delay(15); 
} //END OF LOOP

/*====== 副程式區，start here. ========*/

void ServoARM(String _ACTION, String _DIR){
	if(_ACTION == "Base"){
	  if((_DIR == "UP")&&(posBase < posBaseMAX )){servoBase.write(++posBase); }
	  if((_DIR == "DOWN")&&(posBase > posBaseMIN )){servoBase.write(--posBase); }
	}
	
	if(_ACTION == "Paw"){
	  if((_DIR == "OPEN")&&(posPaw < posPawMAX )){servoPaw.write(++posPaw); }
	  if((_DIR == "CLOSE")&&(posPaw > posPawMIN )){servoPaw.write(--posPaw);}
	}	
	
	if(_ACTION == "Home"){
    Serial.println("servoHome");
	   servoBase.write(homeBase);
     servoPaw.write(homePaw);
     posPaw = homePaw;
     posBase = homeBase;
	   delay(1000);
	}	
}

/*====================================
// PS2X 搖桿設定 
=====================================*/
void PS2X_INIT(){
  while (error != 0) {
    delay(1000);// 1 second wait
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
    Serial.print("#try config ");
    Serial.println(tryNum);
    tryNum ++;
  }
  Serial.println(ps2x.Analog(1), HEX);

  PS2X_type = ps2x.readType();

  switch(PS2X_type) {
    case 0:
      Serial.printf(" Unknown Controller type found, type is %d\n", PS2X_type);
      break;
    case 1:
      Serial.printf(" DualShock Controller found, type is %d\n", PS2X_type);
      break;
    case 2:
      Serial.printf(" GuitarHero Controller found, type is %d\n", PS2X_type);
      break;
    case 3:
      Serial.printf(" Wireless Sony DualShock Controller found, type is %d\n", PS2X_type);
      break;
   }
}
Tags:
Table of Contents