# 電路設計與程式

## 概述

Arduino 讀取搖桿移動時，接收子相應的訊號大小，並且用該數值大小作為控制各個馬達的依據。目前為止，我們已經實做出可以用 SPEkTRUM DXe 遙控器直接控制所有馬達運動的電路與程式。在附錄中會附上該程式。

雖然直接使用遙控器附的接收子便可完成最基礎的遙控功能。然而，使用 Arduino 做控制，可以使得機構整體的擴充性更強，在功能發想方面亦有更多元的空間，未來若需做改良（如：增加感測器、使用更多馬達等等、PID 控制等等），也可以使用 Arduino 方便的可程式功能做更快速的開發。


## 電路架構

基礎的架構如下：


以下分別說明各個部分：

## SPEkTRUM DXe 遙控器與接收子

我們面臨的第一個問題是：接收子所接收到的訊號形式為何？關於這點，我們做了一下的觀察：

1. 若使用 Arduino 讀取類比電壓的 `analogRead()` 函數，無論如何轉動搖桿，電壓均為定值。
2. 將接收子的訊號輸出接到伺服馬達，發現在兩者均在工作電壓的狀況下，可以直接控制伺服馬達。

由上述兩項觀察，我們猜測接收子可能是接收 PWM 訊號，搖桿的轉動幅度不同，對應的腳位會出現工作週期的 PWM 。於是將輸出腳位連接至 Arduino，並且使用 `pulseIn()` 函數讀取不同動作時對應的 duty cycle，發現數字會依照搖桿動作變化，可以更加確定該訊號為 PWM 。

我們更進一步，使用以下的程式讀取各個動作相應的 PWM:

#define REER_NO 13
#define ROTATE_NO 12
#define ELEV_NO 11
#define CLIMB_NO 10

double ctrl_data[4] = {0, 0, 0, 0};

void get_control_data()
{
  for(int i = 0; i < 4; i++){
    ctrl_data[i] = pulseIn(10 + i, HIGH, 10e6);  
  }  
}

結果如下：

不動時：


右方搖桿往上：


右方搖桿往下：


可以發現 REER 會依照搖桿位置，得到約 1100 ~ 1700 左右的數字。這是我們控制的第一步。

## MG996R 伺服馬達

為輝勝公司生產的伺服馬達。我們採用該馬達的理由為：

1. 內部為金屬齒輪，耐磨耗。
2. 扭力大：在 6V 供電下，最大扭矩可以達 11 kgf·cm^[http://www.electronicoscaldas.com/datasheet/MG996R_Tower-Pro.pdf]。
3. 方便控制：Arduno 預設的 Servo.h 函式庫可以直接控制。

我們另外改裝該馬達，使得該馬達可以旋轉完整的 360 度。

## L298N


一般來說，直流馬達的轉速，是透過施加在馬達兩端的電壓決定的。然而，這樣一來便產生以下疑問：

1. Arduino 僅有 5V 供電。若馬達需要更強的電壓，該如何解決？
2. 如何用 Arduino 控制馬達兩端的電壓？
3. 若馬達急轉，則容易產生 backlash EMF 傷害電路。需要有改善方法。

幸運地，使用 L298N^[https://cdn.instructables.com/ORIG/FCN/YABW/IHNTEND4/FCNYABWIHNTEND4.pdf] 電機驅動板，便可以解決上述所有問題。

L298N 的使用方法大致分成 3 部分：

1. 供電：分成 2 部分：
* 馬達電壓：驅動馬達的電壓，必須介於 5V ~ 36V 之間。
* 邏輯供電：供給 L298N 自身 IC 。約 5V。
需注意的是若使用 Arduino 控制，則所有供電都必須與 Arduino 共地。

2. 正/反轉控制：一個馬直流馬達有 2 個輸入邏輯訊號的腳位，以及 1 個 enable 腳位：
* 邏輯訊號：將直流馬達輸入「高電位 - 低電位」時，與輸入「低電位 - 高電位」時，會給予馬達相反的電壓，其大小由 enable 送入的 PWM 訊號來決定。
* enable 訊號：若 enable 訊號為 0，則無論邏輯輸入為何，馬達均不會轉動; 若將 enable 輸入 PWM 訊號，則可以依照 PWM 訊號，調整電壓供給的大小。
而一片 L298N 可控制兩個直流馬達，亦即有 4 個邏輯輸入腳位，以及 2 個 enable 腳位。

3. 輸出：由左右兩側的 2P 端子平台輸出。

如此一來，便解決了我們的 1. 與 2. 點。另外，第 3. 點因為 L298N 內建保護電路，因此亦解決了該問題。可以發現該 IC 為一個功能強大的 IC 。

以前進為例，我們使用下程式實作控制前進與後退的功能：

#define LEFT_EN 9
#define LEFT_1 8
#define LEFT_2 7
#define RIGHT_EN 3
#define RIGHT_1 5
#define RIGHT_2 4

...

int reer(){

  if(ctrl_data[GET_CTRL_DATA(REER_NO)] > 1700){
    /* move forward */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, HIGH);
    digitalWrite(LEFT_2, LOW);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, HIGH);
    digitalWrite(RIGHT_2, LOW);
    Serial.println("\tFORWARD\t");
    return 1;
    
  } else if(ctrl_data[GET_CTRL_DATA(REER_NO)] < 1250) {
    /* move backward */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, LOW);
    digitalWrite(LEFT_2, HIGH);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, LOW);
    digitalWrite(RIGHT_2, HIGH);
    Serial.println("\tBACKWARD\t");
    return -1;
        
  } else {
    analogWrite(LEFT_EN, 0);
    analogWrite(RIGHT_EN, 0);
    return 0;
    
  }
}

這個程式目前僅能使直流馬達做單一速度的轉與反轉。然而，只要修改當中 PWM 的數值，即可以達成控制轉速的功能。

## N20 減速直流馬達

是一種具有減速機構的馬達，用來作為爬繩機前後運動與轉彎的驅動。前後運動較為直觀，然而就轉彎部分，我們一開始有以下想法：

1. 設計類似車輛方向盤的轉彎機構，使用伺服馬達控制方向盤。
2. 使用伺服馬達控制萬向輪。
3. 分別控制兩顆直流馬達，以兩顆馬達的正、反轉來控制轉向。

我們認為爬繩機想解決的核心問題並不在前後運動與轉彎，而是在升降機構與爬繩機構，加上我們有 L298N 可以控制個別直流馬達的轉速，因此決定以最簡單的的方式，也就是方法 3. 來解決這個問題。

int rotate(){

  if(ctrl_data[GET_CTRL_DATA(ROTATE_NO)] < 1250){
    /* Turn right */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, HIGH);
    digitalWrite(LEFT_2, LOW);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, LOW);
    digitalWrite(RIGHT_2, HIGH);
    Serial.print("\tRIGHT\t");
    return 1;
  } else if(ctrl_data[GET_CTRL_DATA(ROTATE_NO)] > 1700) {
    /* Turn left */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, LOW);
    digitalWrite(LEFT_2, HIGH);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, HIGH);
    digitalWrite(RIGHT_2, LOW);
    Serial.print("\tLEFT\t");
    return -1;
  } else {
    analogWrite(LEFT_EN, 0);
    analogWrite(RIGHT_EN, 0);
    return 0;
  }
  
}

程式邏輯與前進後退部分雷同，惟轉彎方向係透過輪子正反轉來達成，即：

1. 右轉：左輪正轉，右輪反轉。
2. 左轉：右輪正轉，左輪反轉。
3. 不轉：將 enable 設為邏輯 0。

## 附錄

### 控制用程式碼

#include<Servo.h>
#define LEFT_EN 9
#define LEFT_1 8
#define LEFT_2 7
#define RIGHT_EN 3
#define RIGHT_1 5
#define RIGHT_2 4
#define LIFT_PIN A0
#define CLIMB_PIN A1
#define REER_NO 13
#define ROTATE_NO 12
#define ELEV_NO 11
#define CLIMB_NO 10
#define GET_CTRL_DATA(x) (((x) - (10)))

Servo lift;
Servo climb_t;
double ctrl_data[4] = {0, 0, 0, 0};

void get_control_data()
{
  for(int i = 0; i < 4; i++){
    ctrl_data[i] = pulseIn(10 + i, HIGH, 10e6);  
  }  
}
void dump_control_data()
{
  Serial.print("REER：  ");
  Serial.print(ctrl_data[3]); 
  Serial.print("  ROTATE：  ");
  Serial.print(ctrl_data[2]);
  Serial.print("  ELEV：  ");
  Serial.print(ctrl_data[1]);  
  Serial.print("  CLIMB：  ");
  Serial.println(ctrl_data[0]);
}

int reer(){
  if(ctrl_data[GET_CTRL_DATA(REER_NO)] > 1700){
    /* move forward */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, HIGH);
    digitalWrite(LEFT_2, LOW);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, HIGH);
    digitalWrite(RIGHT_2, LOW);
    Serial.println("\tFORWARD\t");
    return 1;
    
  } else if(ctrl_data[GET_CTRL_DATA(REER_NO)] < 1250) {
    /* move backward */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, LOW);
    digitalWrite(LEFT_2, HIGH);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, LOW);
    digitalWrite(RIGHT_2, HIGH);
    Serial.println("\tBACKWARD\t");
    return -1;
        
  } else {
    analogWrite(LEFT_EN, 0);
    analogWrite(RIGHT_EN, 0);
    return 0;
    
  }
}

int rotate(){
  
  if(ctrl_data[GET_CTRL_DATA(ROTATE_NO)] < 1250){
    /* Turn right */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, HIGH);
    digitalWrite(LEFT_2, LOW);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, LOW);
    digitalWrite(RIGHT_2, HIGH);
    Serial.print("\tRIGHT\t");
    return 1;
  } else if(ctrl_data[GET_CTRL_DATA(ROTATE_NO)] > 1700) {
    /* Turn left */
    analogWrite(LEFT_EN, 255);
    digitalWrite(LEFT_1, LOW);
    digitalWrite(LEFT_2, HIGH);
    analogWrite(RIGHT_EN, 255);
    digitalWrite(RIGHT_1, HIGH);
    digitalWrite(RIGHT_2, LOW);
    Serial.print("\tLEFT\t");
    return -1;
  } else {
    analogWrite(LEFT_EN, 0);
    analogWrite(RIGHT_EN, 0);
    return 0;
  }
  
}

int elevate()
{
  if (ctrl_data[GET_CTRL_DATA(ELEV_NO)] < 1200) {
    /* up */
    lift.write(180);
    Serial.print("\tUP\t");
    return 1;
  } else if (ctrl_data[GET_CTRL_DATA(ELEV_NO)] > 1700) {
    /* down */
    lift.write(0);
    Serial.print("\tDOWN\t");
    return -1;
  } else {
    lift.write(90);
    return 0;  
  }
}

int climb()
{
  if (ctrl_data[GET_CTRL_DATA(CLIMB_NO)] > 1800) {
    /* climb forward */
    climb_t.write(180);
    Serial.print("\tCLIMB_FORWARD\t");
    return 1;
  } else if (ctrl_data[GET_CTRL_DATA(CLIMB_NO)] < 1100) {
    /* climb backward */
    climb_t.write(0);
    Serial.println("\tCLIMB_BACKWARD\t");
    return -1;
  } else {
    climb_t.write(90);
    return 0;
  }
}


void action()
{
  if(!reer())
    rotate();
  elevate();
  climb();  
}

void setup()
{
  pinMode(RIGHT_EN, OUTPUT);
  pinMode(LEFT_EN, OUTPUT);
  pinMode(RIGHT_1, OUTPUT);
  pinMode(RIGHT_2, OUTPUT);
  pinMode(LEFT_1, OUTPUT);
  pinMode(LEFT_2, OUTPUT);

  lift.attach(LIFT_PIN);
  climb_t.attach(CLIMB_PIN);


  Serial.begin(9600);
}

void loop(){
    get_control_data();
    dump_control_data();

    if(ctrl_data[0] > 1 && ctrl_data[2]) {
      action();
      delay(80);
    } 
}