FC2カウンター FPGAの部屋 Zybot が走った

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

FPGAの部屋

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

Zybot が走った

ZybotのZYBOをLinuxで動作させる1(BOOT.bin, devicetree.dtb)”でPWMモジュールIP やモーター・モニタIP を追加したハードウェアにしたBOOT.bin とデバイス・ツリーをSDカードに追加して、Linux を立ち上げることができた。
今回は、Ubuntu 上でPWMモジュールIP とモーター・モニタIP を制御するアプリケーション・ソフトウェアを作成して、Zybot を走らせてみることにする。

まずは、Ubuntu 14.04 LTS で、Zybot を動かすアプリケーションを作成した。
zybot_motor.cpp を示す。

// zybot_motor.cpp
// 2016/07/07 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"

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

int main(){
    XPwm motorL, motorR;
    XPwm *motorLp, *motorRp;
    XMotor_monitor mmL, mmR;
    XMotor_monitor *mmLp, *mmRp;
    int c = 0;
    int motorvalL, motorvalR;
    int mmonitorL, mmonitorR;
    
    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(&motorL, 0);
    XPwm_Set_dir_V(&motorL, 1);    

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

    motor_settings(motorLp, motorRp);
    
    // main loop
    printf("Zybot Motor Control Application Start...\n");    
    while(c != 'q'){
        c = getc(stdin);
        switch ((char)c) {
            case 'l':{ // Left motor value setting
                printf("Please input Left motor value = ");
                scanf("%d", &c);
                motorvalL = c;
                break;
            }
            case 'r':{ // Right motor value setting
                printf("Please input Right motor value = ");            
                scanf("%d", &c);
                motorvalR = c;
                break;
            }
            case 't':{ // To change the power of the right and left motor.
                printf("Left motor = %d, Right motor = %d\n", motorvalL, motorvalR);
                XPwm_Set_sw_late_V(&motorL, motorvalL);
                XPwm_Set_sw_late_V(&motorR, motorvalR);
                break;
            }                                
            case 'm':{ // Enter the difference between the motor value.
                printf("motor value = L %d, R %d\n", motorvalL, motorvalR);
                printf("Please input Motor output difference = ");
                scanf("%d", &c);
                motorvalL = motorvalL + c;
                motorvalR = motorvalR + c;
                if (motorvalL < 0)
                    motorvalL = 0;
                if (motorvalR < 0)
                     motorvalR = 0;
                if (motorvalL > 100)
                     motorvalL = 100;
                if (motorvalR > 100)
                     motorvalR = 100;
                XPwm_Set_sw_late_V(&motorL, motorvalL);    
                XPwm_Set_sw_late_V(&motorR, motorvalR);
                break;
            }
            case 'a':{ // The Motors is rotated in the forward or reverse direction.
                if (XPwm_Get_dir_V(&motorL)){ // forward
                    Stopped_Zybot(motorLp, motorRp);
                    sleep(1);
                    
                    XPwm_Set_sw_late_V(&motorL, motorvalL);    
                    XPwm_Set_sw_late_V(&motorR, motorvalR);
                    
                    XPwm_Set_dir_V(&motorL, 0); // reverse
                    XPwm_Set_dir_V(&motorR, 1);
                    
                }else{
                    Stopped_Zybot(motorLp, motorRp);
                    sleep(1);
                    
                    XPwm_Set_sw_late_V(&motorL, motorvalL);    
                    XPwm_Set_sw_late_V(&motorR, motorvalR);

                    XPwm_Set_dir_V(&motorL, 1); // forward
                    XPwm_Set_dir_V(&motorR, 0);
                }
                break;
            }
            case 'd':{ // Displayed the motor monitor value
                while (!XMotor_monitor_IsIdle(&mmL));
                XMotor_monitor_Start(&mmL);
                         
                while (!XMotor_monitor_IsIdle(&mmR));
                XMotor_monitor_Start(&mmR);
            
                while (!XMotor_monitor_IsIdle(&mmL));
                while (!XMotor_monitor_IsIdle(&mmR));
            
                unsigned int sa_countL = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmL);        
                unsigned int sa_countR = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmR);        

                unsigned int sb_levelL = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmL);
                unsigned int sb_levelR = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmR);  
     
                 unsigned int returnL = (unsigned int)XMotor_monitor_Get_return(&mmL);
                 unsigned int returnR = (unsigned int)XMotor_monitor_Get_return(&mmR);
                 
                 unsigned int sum_cntL = (unsigned int)XMotor_monitor_Get_sum_count(&mmL);
                 unsigned int sum_cntR = (unsigned int)XMotor_monitor_Get_sum_count(&mmR);
                 
                printf("sa_countL = %d, sb_levelL = %d, retrunL = %d, sum_cntL = %d\n", sa_countL, sb_levelL, returnL, sum_cntL);
                 printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countL/1000000)), (float)sa_countL/1000);

                printf("sa_countR = %d, sb_levelR = %d, retrunR = %d, sum_cntR = %d\n", sa_countR, sb_levelR, returnR, sum_cntR);
                 printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countR/1000000)), (float)sa_countR/1000);
                 break;
             }
             case 's':{ // Stop
                 Stopped_Zybot(motorLp, motorRp);
                 motorvalL = 0;
                 motorvalR = 0;
                 break;
             }
             case 'h':{ // help
                 printf("l: Left motor value setting\n");
                 printf("r: Right motor value setting\n");
                 printf("t: To change the power of the right and left motor\n");
                 printf("m: Enter the difference between the motor value. eg. 10, -10\n");
                 printf("a: The Motors is rotated in the forward or reverse direction. Altarnative\n");
                 printf("d: Displayed the motor monitor value\n");
                 printf("s: Stop\n");
                 printf("q: Quit\n");
                 break;
             }
         }
    }

     return 0;
}


次にMakefile を示す。

#Makefile
# Referred to http://www.ie.u-ryukyu.ac.jp/~e085739/c.makefile.tuts.html

PROGRAM = zybot_motor
OBJS = zybot_motor.o xpwm_linux.o xpwm.o xmotor_monitor_linux.o xmotor_monitor.o

CC = gcc
CFLAGS = -Wall -O2

.SUFFIXES: .c .o

.PHONY: all

all: zybot_motor

zybot_motor: $(OBJS)
    $(CC) -Wall -o $@ $(OBJS)
    
.c.o:
    $(CC) $(CFLAGS) -c $<

    
.PHONY: clean
clean:
    $(RM) $(PROGRAM) $(OBJS)
    


make コマンドでコンパイルを行った。その様子を下図に示す。
ZYBO_0_motor_12_160711.png

zybot_motor コマンドが作成された。

zybot_motor コマンドを使っている様子を示す。なお、これはまだ走らせてなくタイヤは空転状態だったので、かなり速く回っている。走らせているときは、PWM 値が 30 くらいだと、190 Hz 程度だった。
ZYBO_0_motor_13_160711.png

Zybot をうまく走らせることができた。結構速い。左右のタイヤは独立に固定しあるので、左右のタイヤの向きがいい加減になっているのをどうしたらよいかな?と思っている。

Zybot の今の様子を下に示す。まだ仮組で走行アプリケーションの具合を確かめるために走らせてみただけだ。あとで、レーザー加工機と3Dプリンタで車体を作ることにしようと思う。
ZYBO_0_motor_14_160711.jpg

なお、無線LANアダプタを付けてあるので、無線操縦になっている。
  1. 2016年07月13日 04:40 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


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

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