【通訊】e32DUAL | GS/GA搖桿模式

簡介

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

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

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

接線圖

PS2搖桿接腳

腳位數	PS2搖桿腳位/名稱	e32DUAL腳位/GPIO
1	DAT / DI	25
2	CMD / DO	17
3	VCC	5V
4	GND	GND
5	SEL	16
6	CLK	26

---

範例程式

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

/*-- 宣告區(全域變數) --*/
int SeekCnt = 0;
unsigned long prevMillis = 0;  // 暫存經過時間（毫秒）
const long interval = 5000;     // 指定間隔時間5秒(5000毫秒)。

//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        25  //input pin,  DI - 25
#define PS2_CMD        17  //output pin, DO - 17/TX2
#define PS2_SEL        16  //output pin, SEL- 16/RX2
#define PS2_CLK        26  //output pin, CLK- 26
#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()
{
  GoSUMOInital();  //進行GoSUMO機器人初始化, Serial baud rate = 115200
  Serial.begin(115200);
  //初始化PS2搖桿
  PS2X_INIT();

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

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

    if(ps2x.Analog(PSS_RY) <= 118){ GS_FW(map(ps2x.Analog(PSS_RY), 128, 0, PWMSpeed_MIN, PWMSpeed_MAX)); Serial.println("F"); }          //前進 
    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) < 130)&&(ps2x.Analog(PSS_RX) > 125)) 
      && 
      ((ps2x.Analog(PSS_RY) < 130)&&(ps2x.Analog(PSS_RY) > 125))
      ){ 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", "");  } //Servo復位90度

    if(ps2x.Button(PSB_L2) || ps2x.Button(PSB_R2)) {  prevMillis = millis(); SpecificAction();   } //進入指定動作模式
  }  
  delay(15); 
} //END OF LOOP

/*====== 副程式區，start here. ========*/
void SpecificAction(){ //指定動作程式區
  int _SPEED = 1000; //速度設定1000
  GS_FW(_SPEED);     delay(3000);  //前進3秒
  GS_LEFT(_SPEED);   delay(300);  //左旋0.3秒
  GS_FW(_SPEED);     delay(1000);  //前進1秒
  GS_RIGHT(_SPEED);  delay(300);  //右璇0.3秒
  GS_FW(_SPEED);     delay(1000);  //前進1秒
  GS_STOP();
}

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;
   }
}