Zybotの車輪のセンサー・フィードバックのデータを取得した

”Zybot の車輪が回転した”で、Vivado HLS で作成したPWMモジュールIP でZybot の車輪を回転させた。

今回はPmodHB5 のセンサー・フィードバック・ピンのSA と SB から車輪の回転角速度を検出してみた。車輪の回転角速度を検出には、”PmodHB5 のセンサー・フィードバック処理IP を作ってみた3”で作ったPmodHB5 のセンサー・フィードバック処理IP を使っている。

ZYBO_0_162_2 プロジェクトでPWMモジュールIP とセンサー・フィードバック処理IP を使用している。そのブロック・デザインを示す。


2つのIP は PmodHB5_inf_right と PmodHB5_inf_left の2つのモジュール内に入っている。そのブロック・デザインを示す。


IP のアドレスマップを示す。


ビットストリームの生成までを行った。レポートを示す。


ハードウェアをエクスポートして、SDKを立ち上げた。
mm_test プロジェクトに mm_test.c を作成した。mm_test.c を示す。

/* * mm_test.c * *  Created on: 2016/06/29 *      Author: ono */
#include <stdio.h>
#include <stdlib.h>
#include "xil_io.h"
#include "xparameters.h"
#include "sleep.h"
#include "xmotor_monitor.h"

 int main(){
    XMotor_monitor XMmoniL, XMmoniR;
    XMotor_monitor_Config *XMmoniLPTR, *XMmoniRPTR;

    XMmoniLPTR = XMotor_monitor_LookupConfig(0);
    if(!XMmoniLPTR){
        fprintf(stderr, "Left XMotor monitor configuration failed.\n");
        return(-1);
    }
    int XMmL_status = XMotor_monitor_CfgInitialize(&XMmoniL, XMmoniLPTR);
    if (XMmL_status != XST_SUCCESS){
        fprintf(stderr, "Could not Initialize Left XMotor monitor\n");
        return(-1);
    }

    XMmoniRPTR = XMotor_monitor_LookupConfig(1);
    if(!XMmoniRPTR){
        fprintf(stderr, "Right XMotor monitor configuration failed.\n");
        return(-1);
    }
    int XMmR_status = XMotor_monitor_CfgInitialize(&XMmoniR, XMmoniRPTR);
    if (XMmR_status != XST_SUCCESS){
        fprintf(stderr, "Could not Initialize Right XMotor monitor\n");
        return(-1);
    }

    while (!XMotor_monitor_IsIdle(&XMmoniL));
    while (!XMotor_monitor_IsIdle(&XMmoniR));

    XMotor_monitor_Start(&XMmoniL);
    //XMotor_monitor_EnableAutoRestart(&XMmoniL);
    while (!XMotor_monitor_IsIdle(&XMmoniL));

    XMotor_monitor_Start(&XMmoniR);
    //XMotor_monitor_EnableAutoRestart(&XMmoniR);
    while (!XMotor_monitor_IsIdle(&XMmoniR));

    u32 sa_countL = XMotor_monitor_Get_sa_count_V(&XMmoniL);
    u32 sa_countR = XMotor_monitor_Get_sa_count_V(&XMmoniR);

    u32 sb_levelL = XMotor_monitor_Get_sb_level_V(&XMmoniL);
    u32 sb_levelR = XMotor_monitor_Get_sb_level_V(&XMmoniR);

    u32 returnL = XMotor_monitor_Get_return(&XMmoniL);
    u32 returnR = XMotor_monitor_Get_return(&XMmoniR);

    printf("sa_countL = %d, sb_levelL = %d, returnL = %d\n", (unsigned int)sa_countL,
            (unsigned int)sb_levelL, (unsigned int)returnL);
    printf("sa_countR = %d, sb_levelR = %d, returnR = %d\n", (unsigned int)sa_countR,
            (unsigned int)sb_levelR, (unsigned int)returnR);


    return 0;
}

これを実行してみた。


sb_level はL, R 共にばらついてしまう。これはなぜかわからないが、回転方向についてはPWMモジュールに設定した値を信じることにする。
次に、回転角速度については、6回を平均することにした。
sa_countL は (2364 + 2361 + 2331 + 2398 + 2396 + 2328) * 1.04 / 6 ≒ 2457 us （100MHz クロックで 100 回平均しているので 1 / 100 のクロックになっている）
sa_countR は (2265 + 2262 + 2324 + 2298 + 2295 + 2239) * 1.04 / 6 ≒ 2371 us （100MHz クロックで 100 回平均しているので 1 / 100 のクロックになっている）

sa_countL は 2457 us = 407 Hz 、sa_countR は 421 Hz

オシロスコープで Right 側のSA と SB を見たところ、約 400 Hz だった。精度的にはあまり良くないか？もしかして、RとLを間違っている？