FC2カウンター FPGAの部屋 2台の Zybot での隊列走行2(先頭のZybotのアプリケーション)

FPGAやCPLDの話題やFPGA用のツールの話題などです。 マニアックです。 日記も書きます。

FPGAの部屋

FPGAの部屋の有用と思われるコンテンツのまとめサイトを作りました。Xilinx ISEの初心者の方には、FPGAリテラシーおよびチュートリアルのページをお勧めいたします。

2台の Zybot での隊列走行2(先頭のZybotのアプリケーション)

2台の Zybot での隊列走行1(構想編)”の続き。

Vivadoを使用してZYBO_0_163_6フォルダのプロジェクトにRGB2HSV IPを追加6(UbuntuでRGB2HSVを試す)”でRGB2HSV変換 IP をZYBO のPL に入れたプラットフォームを使用してUbuntu 14.04 を起動してRGB2HSV変換を試すことができた。それで、隊列走行をやってみたい。

まずは、自律走行する先頭Zybot のアプリケーションを作ることにした。
最初は右に曲がった状態にして追従するように書いたが、まっすぐにしたほうが良かったかな?まあ、それはすぐに直せる。
アプリケーション名は platoon_car0.c だ。
起動するとまずは直進する。こうしないと右旋回したときに少ない値のPWM値では動かないからだ。直進で動かしてから1秒後に右旋回する。その後はキー入力で直進、右旋回、左旋回、停止を選べる。

// platoon_car0.cpp
// Autonomously running car
// 2016/11/15 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <string.h>

#include "xpwm.h"
#include "xmotor_monitor.h"

#define DIR_LEFT_NORMAL        1
#define DIR_LEFT_REVERSE    0
#define DIR_RIGHT_NORMAL    0
#define DIR_RIGHT_REVERSE    1

#define PIXEL_NUM_OF_BYTES    4
#define SVGA_HORIZONTAL_PIXELS  800
#define SVGA_VERTICAL_LINES     600
#define SVGA_ALL_DISP_ADDRESS   (SVGA_HORIZONTAL_PIXELS * SVGA_VERTICAL_LINES * PIXEL_NUM_OF_BYTES)

// Motor
//
void motor_settings(XPwm *motorLp, XPwm *motorRp){
    XPwm_DisableAutoRestart(motorLp);
    while(!XPwm_IsIdle(motorLp)) ;
    XPwm_Start(motorLp);
    XPwm_EnableAutoRestart(motorLp);
    
     XPwm_DisableAutoRestart(motorRp);
    while(!XPwm_IsIdle(motorRp)) ;
    XPwm_Start(motorRp);
    XPwm_EnableAutoRestart(motorRp);
}

void Stopped_Zybot(XPwm *motorLp, XPwm *motorRp){
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_sw_late_V(motorRp, 0);
}

void motor_initialize(XPwm &motorL, XPwm &motorR, XMotor_monitor &mmL, XMotor_monitor &mmR){
    XPwm *motorLp, *motorRp;
    XMotor_monitor *mmLp, *mmRp;
    
    motorLp = &motorL;
    motorRp = &motorR;
    mmLp = &mmL;
    mmRp = &mmR;
    
    // Initialization of motor
    if (XPwm_Initialize(motorLp, "pwm_0") != XST_SUCCESS){
        fprintf(stderr,"pwm_0 (Left) open error\n");
        exit(-1);
    }
    if (XPwm_Initialize(motorRp, "pwm_1") != XST_SUCCESS){
        fprintf(stderr,"pwm_1 (Right) open error\n");
        exit(-1);
    }
    
    
    // Initialization of motor monitor
    if (XMotor_monitor_Initialize(mmLp, "motor_monitor_0") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_0 (Left) open error\n");
        exit(-1);
    }
    if (XMotor_monitor_Initialize(mmRp, "motor_monitor_1") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_1 (Right) open error\n");
        exit(-1);
    }

    // The Motors is rotated in the forward direction.
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_dir_V(motorLp, 1);

    XPwm_Set_sw_late_V(motorRp, 0);
     XPwm_Set_dir_V(motorRp, 0);

    motor_settings(motorLp, motorRp);
}

int main(){
    XPwm motorL, motorR;
    XMotor_monitor mmL, mmR;
    int c;

    // Motor Initialize
    motor_initialize(motorL, motorR, mmL, mmR);

    XPwm_Set_sw_late_V(&motorL, 25);
    XPwm_Set_sw_late_V(&motorR, 25);
    
    sleep(1);
    
    // right turn
    XPwm_Set_sw_late_V(&motorL, 30);
    XPwm_Set_sw_late_V(&motorR, 10);
    
    // Key input
    // r - right turn, l - left turn, g - Go straight, s - stop, q - quit
    printf("r - right turn, l - left turn, g - Go straight, s - stop, q - quit\n");
    c = getc(stdin);
    while(c != 'q'){
        switch ((char)c) {
            case 'r' : // right turn
                XPwm_Set_sw_late_V(&motorL, 30);
                XPwm_Set_sw_late_V(&motorR, 10);
                break;
            case 'l' : // left turn
                XPwm_Set_sw_late_V(&motorL, 10);
                XPwm_Set_sw_late_V(&motorR, 30);
                break;
            case 'g' : // Go straight
                XPwm_Set_sw_late_V(&motorL, 20);
                XPwm_Set_sw_late_V(&motorR, 20);
                break;
            case 's' : // stop
                XPwm_Set_sw_late_V(&motorL, 0);
                XPwm_Set_sw_late_V(&motorR, 0);
                break;
        }
        c = getc(stdin);
    }

    XPwm_Set_sw_late_V(&motorL, 0);
    XPwm_Set_sw_late_V(&motorR, 0);

    return(0);
}


Zybot でテストしてみたが、十分に良い感じで走行できている。あまり早いと、どこかにぶつかってまずいので、これでよいかな?でも2台相手するのは大変なので、一定時間走ったら停止する機能を付けたほうが良いかな?
これで、先頭のZybot のアプリケーションはできたので、自動追従する 2 番目のZybot の追従方法を考えよう。
  1. 2016年11月24日 04:07 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


管理者にだけ表示を許可する

トラックバック URL
http://marsee101.blog19.fc2.com/tb.php/3640-e6ca8b9d
この記事にトラックバックする(FC2ブログユーザー)