Arduino 自走車 & 機械手臂-JackKuo's Blog

Monday, June 4, 2018

微算機實驗課程期末專題有些人接線做 8051 喇叭，但因為器材不太夠，就有幾個人可以玩 Arduino 自走車，剛好 MOLi 實驗室有舉辦機械手臂相關活動也是使用 Arduino 去弄，就順便將這兩樣東西兜起來當作期末報告。

使用器材

Arduino UNO
Arduino Sensor Shield V5.0 感測器擴展板
自走車相關零組件
PS2 副廠搖桿

組裝教學

自走車

基本上可以參考這篇，其中我拿到的零件比較不同的是馬達驅動模組，但一樣接上 IN1~IN4 當作訊號即可，因為另一邊有通電了。聽同學說有個跳線的地方，蓋子拔掉就不會通電了，這樣就得自己接電接地。對於擴充版不熟悉的話可以參考這篇。

搖桿

網路上很多接線圖，大部分都一樣，這邊就貼個我看的順眼的。

遇到的坑

馬達

一開始測試馬達，code 寫進去要他怎麼轉就怎麼轉，可是之後重接重寫就發現不會動了。先檢查程式，發現是忘了在最前面寫上 pinMode 指定 OUTPUT。後來還是不會動，從馬達開始接電測試，不管正轉逆轉都沒問題，再來是馬達驅動模組，螺絲重新鎖、接電測試也都沒問題，那該不會是我的腦袋有問題吧!?最後終於發現是杜邦線太鬆、或是模組的針腳太細，總之就是接觸不良，因為這批線是當天助教拿新的給我，沒想到就有這個問題。後來詢問老師，說可能是針腳太短，且杜邦線價格差異很大，品質可能也有差。

PS2 搖桿

沒想到這搖桿也能拿來接 Arduino，上網找來找去好像就 PS2X 這個 Library 最好用，基本上就是範例 code 貼上去就要會動，可是我怎麼測試就是不會動，弄了約 3.4 小時聽到同學早就成功了，跟他借線測試後才知道可能有些訊號線斷掉了，之前還有請老師幫忙找問題，可惜沒找到，老師表示原來杜邦線品質這麼差。之後弄弄手臂再回來弄搖桿發現又不會動了。。。換線過後又正常，Arduino 這東西怎麼那麼難搞阿，光是接線測試就花了 8 小時，雖然很倒楣線材總是出包，但也只能怪自己沒玩過多少硬體，對於 debug 不熟悉。

IDE 問題

在 Mac 上如果輸入法是在內建的注音模式，複製貼上或是快捷鍵註解等操錯可能會有問題，建議切換到英文模式操作。
如果 Arduino 同時接電又接 USB，傳輸時可能會出現 stk500_recv(): programmer is not responding 錯誤，只要將電源拔掉即可

DEMO 影片

因為是一手操控搖桿一手攝影，稍微晃動

程式碼

#include <PS2X_lib.h> //for v1.6
#include <Servo.h>
#define LEFT_FORWARD 6
#define LEFT_BACKWARD 7
#define RIGHT_FORWARD 9
#define RIGHT_BACKWARD 8
#define MIDDLE_VALUE 127
#define CLAW 5
#define BASE 2
#define LEFT_ARM 3
#define RIGHT_ARM 4

PS2X ps2x;

int error = 0;
byte type = 0;
// 手把是否要震動
byte vibrate = 0;
// 總共有幾顆伺服馬達
const int SERVOS = 4;
// 類比搖桿至少扳動多少才開始動作
const int sensityOffset = 50;
Servo myservo[SERVOS];
int servoConfig[SERVOS][5] = {
        // Order, servoPin, minAngle, maxAngle, initialAngle
        {0, CLAW, 0, 50, 50},        // 夾子
        {1, BASE, 60, 140, 90},      // 基座
        {2, LEFT_ARM, 0, 70, 0},     // 左臂
        {3, RIGHT_ARM, 40, 110, 80}  // 右臂
    };
void setup() {
    Serial.begin(57600);
    pinMode(LEFT_FORWARD, OUTPUT);
    pinMode(LEFT_BACKWARD, OUTPUT);
    pinMode(RIGHT_FORWARD, OUTPUT);
    pinMode(RIGHT_BACKWARD, OUTPUT);
    error = ps2x.config_gamepad(13, 11, 10, 12, true, true); //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

    if (error == 0) {
        Serial.println("Found Controller, configured successful");
        Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
        Serial.println("holding L1 or R1 will print out the analog stick values.");
        Serial.println("Go to [url]www.billporter.info[/url] for updates and to report bugs.");
    } else if (error == 1)
        Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit [url]www.billporter.info[/url] for troubleshooting tips");

    else if (error == 2)
        Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit [url]www.billporter.info[/url] for troubleshooting tips");

    else if (error == 3)
        Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

    type = ps2x.readType();
    switch (type) {
    case 0:
        Serial.println("Unknown Controller type");
        break;
    case 1:
        Serial.println("DualShock Controller Found");
        break;
    case 2:
        Serial.println("GuitarHero Controller Found");
        break;
    }
    // 馬達初始化
    for(int i = 0; i < SERVOS; i++) {
        myservo[i].attach(servoConfig[i][1]);
        myservo[i].write(servoConfig[i][4]);
    }
}

void forward(){
    digitalWrite(LEFT_FORWARD, HIGH);
    digitalWrite(LEFT_BACKWARD, LOW);
    digitalWrite(RIGHT_FORWARD, HIGH);
    digitalWrite(RIGHT_BACKWARD, LOW);
}

void left(){
    digitalWrite(LEFT_FORWARD,LOW);
    digitalWrite(LEFT_BACKWARD,LOW);
    digitalWrite(RIGHT_FORWARD,HIGH);
    digitalWrite(RIGHT_BACKWARD,LOW);
}

void right(){
    digitalWrite(LEFT_FORWARD,HIGH);
    digitalWrite(LEFT_BACKWARD,LOW);
    digitalWrite(RIGHT_FORWARD,LOW);
    digitalWrite(RIGHT_BACKWARD,LOW);
}

void backward(){
    digitalWrite(LEFT_FORWARD, LOW);
    digitalWrite(LEFT_BACKWARD, HIGH);
    digitalWrite(RIGHT_FORWARD, LOW);
    digitalWrite(RIGHT_BACKWARD, HIGH);
}
void loop() {
    boolean shouldMove = false;
    int currentAngle[SERVOS];
    if (error == 1) //skip loop if no controller found
        return;

        //DualShock Controller

        ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

        // 前進
        if (ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed
            Serial.print("Up held this hard: ");
            Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
            forward();
        }

        // 右轉
        if (ps2x.Button(PSB_PAD_RIGHT)) {
            Serial.print("Right held this hard: ");
            Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
            right();
        }

        // 左轉
        if (ps2x.Button(PSB_PAD_LEFT)) {
            Serial.print("LEFT held this hard: ");
            Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
            left();
        }

        // 後退
        if (ps2x.Button(PSB_PAD_DOWN)) {
            Serial.print("DOWN held this hard: ");
            Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
            backward();
        }

        if (ps2x.ButtonPressed(PSB_RED)) // will be TRUE if button was JUST pressed
            Serial.println("Circle just pressed");

        if (ps2x.ButtonReleased(PSB_PINK)) // will be TRUE if button was JUST released
            Serial.println("Square just released");

        if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
        {
            // Serial.print("Stick Values:");
            // Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
            // Serial.print(",");
            // Serial.print(ps2x.Analog(PSS_LX), DEC);
            // Serial.print(",");
            // Serial.print(ps2x.Analog(PSS_RY), DEC);
            // Serial.print(",");
            // Serial.println(ps2x.Analog(PSS_RX), DEC);

            // 以下為手臂移動
            currentAngle[0] = myservo[0].read();
            currentAngle[1] = myservo[1].read();
            currentAngle[2] = myservo[2].read();
            currentAngle[3] = myservo[3].read();
            // 左右移動計算
            if(ps2x.Analog(PSS_LX) < (MIDDLE_VALUE - sensityOffset) ) {
                if(currentAngle[1] < servoConfig[1][3]) {
                    currentAngle[1] += 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[1]);
                }
             }
            else if(ps2x.Analog(PSS_LX) > (MIDDLE_VALUE + sensityOffset) ){
                if(currentAngle[1] > servoConfig[1][2]) {
                    currentAngle[1] -= 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[1]);
                }
            }
            // 左臂計算
            if(ps2x.Analog(PSS_LY) < (MIDDLE_VALUE - sensityOffset) ) {
                if(currentAngle[2] < servoConfig[2][3]) {
                    currentAngle[2] += 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[2]);
                }
            }
            else if(ps2x.Analog(PSS_LY) > (MIDDLE_VALUE + sensityOffset) ){
                if(currentAngle[2] > servoConfig[2][2]) {
                    currentAngle[2] -= 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[2]);
                }
            }
            // 右臂計算
            if(ps2x.Analog(PSS_RY) < (MIDDLE_VALUE - sensityOffset) ) {
                if(currentAngle[3] < servoConfig[3][3]) {
                    currentAngle[3] += 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[3]);
                }
            }
            else if(ps2x.Analog(PSS_RY) > (MIDDLE_VALUE + sensityOffset) ){
                if(currentAngle[3] > servoConfig[3][2]) {
                    currentAngle[3] -= 2;
                    shouldMove = true;
                    // Serial.println(currentAngle[3]);
                }
            }
            // 夾子
            if (ps2x.ButtonPressed(PSB_BLUE)){//will be TRUE if button was JUST pressed OR released
                Serial.println("X just changed");
                // Serial.println(currentAngle[0]);
                if(currentAngle[0] == servoConfig[0][2]){
                    currentAngle[0] = servoConfig[0][3];
                    shouldMove = true;
                    // Serial.println(currentAngle[0]);
                }
                else{
                    currentAngle[0] = servoConfig[0][2];
                    shouldMove = true;
                    // Serial.println(currentAngle[0]);
                }
            }
            // 開始移動
            if(shouldMove) {
                myservo[0].write(currentAngle[0]);
                myservo[1].write(currentAngle[1]);
                myservo[2].write(currentAngle[2]);
                myservo[3].write(currentAngle[3]);
            }
        }

    delay(50);
    // 強制歸零，不然會一直跑
    digitalWrite(LEFT_FORWARD,HIGH);
    digitalWrite(LEFT_BACKWARD,HIGH);
    digitalWrite(RIGHT_FORWARD,HIGH);
    digitalWrite(RIGHT_BACKWARD,HIGH);
}

Arduino 自走車 & 機械手臂