通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能

87
0
2021年2月10日 09时29分

之前的博客为了实现延时特定时间(4ms)并在这段时间内产生PWM波形,使用了两种方法,第一种通过计数的方式,比较low;第二种使用PRU的工业级定时器IEP,时钟频率200MHZ,使用也很简单。

 

但是随着实验的进行,因为是六自由度的机械臂,计划使用三块beaglebone来控制,也就是一块板子控制两个伺服电机,beaglebone自带两个PRU核,PRU1和PRU2,每个核都具有31个寄存器, 所以基本的PWM信号产生和电机方向信号产生的汇编代码完全一样,唯一不一样的就是IEP定时器只有一个,PRU1和PRU0共用,想要实现动态计数,就必须在寻找一个类似与IEP功能的东西,经过一天的AM335X手册洗礼,终于找到了beaglebone的普通定时器DMTIMER。

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图

 

想要使用DM定时器,必须要使能OCP主口。DM定时器一共有8个,其中DMTIMER0 和 DMTIMER3到7 默认非使能状态,DMTIMER1是一个比较特别的计时器,使用方法也不太一样,而DMTIMER2被默认使能,使用的时钟频率是24MHZ,简单起见,直接使用系统默认的DMTIMER2。如下所示,DMTIMER0使用默认的时钟频率是32KHZ,应该可以将默认失踪频率换成100MHZ的,不深究了。

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图(1)

 

对于DMTIMER2,它记录的数据类似于时间戳(这点我没有细看),两次读取做差值就可以得到经过的时间,再与24MHZ对应的4ms系统嘀嗒数做比较即可。4ms对应200MHZ的滴答数是4000*1000/5,所以24MHZ对应的就是4000*1000 / 5 * 24 /200 = 96000

 

如果不想使用DMTIMER2这种需要做差的数据,直接使用类似IEP的从0开始的计时器,需要对TLDR,TCLR,TCRR这三个寄存器进行操作,并且使能DMTIMER3~7.具体参考AM335X手册。

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图(2)

 

下面上代码:

使用了两个PRU核,所以需要重新修改设备树文件,如下,新增pru1对应的两个p8插口:

 

/dts-v1/;
/plugin/;
 
/ {
   compatible = "ti,beaglebone", "ti,beaglebone-black";
   part-number = "EBB-PRU-Example";
   version = "00A0";
   /* This overlay uses the following resources */
   exclusive-use =
         "P9.11", "P9.13", "P9.27", "P9.31", "P8.28", "P8.29",
         "pru0", "pru1";
   fragment@0 {
      target = <&am33xx_pinmux>;
      __overlay__ {
         gpio_pins: pinmux_gpio_pins {         // The GPIO pins
            pinctrl-single,pins = <
               0x070 0x07  // P9_11 MODE7 | OUTPUT | GPIO pull-down
               0x074 0x27  // P9_13 MODE7 | INPUT | GPIO pull-down
            >;
         };
         pru_pru_pins: pinmux_pru_pru_pins {   // The PRU pin modes
            pinctrl-single,pins = <
               0x1a4 0x05  // P9_27 pr1_pru0_pru_r30_5, MODE5 | OUTPUT | PRU
               0x190 0x05  // P9_31 pr1_pru0_pru_r30_0, MODE5 | OUTPUT | PRU
               0x0e8 0x05  // P8_28 pr1_pru1_pru_r30_10  MODE5 | OUTPUT | PRU
               0x0e4 0x05  // p8_29 pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU
            >;
         };
      };
   };
 
   fragment@1 {         // Enable the PRUSS
      target = <&pruss>;
      __overlay__ {
         status = "okay";
         pinctrl-names = "default";
         pinctrl-0 = <&pru_pru_pins>;
      };
   };
 
   fragment@2 {         // Enable the GPIOs
      target = <&ocp>;
      __overlay__ {
         gpio_helper {
            compatible = "gpio-of-helper";
            status = "okay";
            pinctrl-names = "default";
            pinctrl-0 = <&gpio_pins>;
         };
      };
   };
};

 

修改客户端文件redwall_arm_client.cpp,增加PRU1控制部分,其他部分完全一样,只不过调用了两个bin文件,分别控制两个电机

 

#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <sys/time.h>
#include <sys/poll.h>
#include <sys/epoll.h>
#include <iostream>
 
/* 使用vector数组 */
#include <vector>
#include <algorithm>
 
/* PRU指令 */
#include <prussdrv.h>
#include <pruss_intc_mapping.h>
 
#define PORT 7788
#define ADDR "192.168.7.1"
#define K 1000
#define DELAY_US 4000 // Max. value = 21474836 us
#define TICKS_IEP ((DELAY_US / 5) * 1000)   // 200MHZ
#define TICKS_DMT 96000                     // 24MHZ
 
/* 使用的是PRU0 */
#define PRU_NUM0  0
#define PRU_NUM1  1
 
void *lumbar_and_big_arm_motor(void *)
{
    while(1)
    {
        usleep(1000);
        if(v_lumbar.size() == vector_len_)
        {
            cout<< "插补规划的数组长度: "<< vector_len_<<endl;
            tpruss_intc_initdata pruss_intc_initdata = PRUSS_INTC_INITDATA;
            prussdrv_init ();
            prussdrv_open (PRU_EVTOUT_0);
            prussdrv_open (PRU_EVTOUT_1);
            prussdrv_pruintc_init(&pruss_intc_initdata);
 
            // 存储周期数组,谐波减速器的减速比是50,速度数组的单位是弧度每秒
            // n = K*f = K / T;注意单位!!!
            // 周期单位是ns,延迟因子单位是us
            // 现在要储存负数了,不能使用unsigned int了,反正范围也不溢出
            int lumbar_period[vector_len_];
            int big_arm_period[vector_len_];
            for (int i=0; i<vector_len_; i++){
                // 转换成延时因子
                // lumbar_period[i] = int(K / v_lumbar[i] * 0.001);     // PRU稳定循环开销1.6u,计算周期的时候需要考虑
                if(i%10==0)
                {
                    lumbar_period[i] = 0;
                    big_arm_period[i] = 0;
                }
                else
                {
                    lumbar_period[i] = i%2==0?-20:10;
                    big_arm_period[i] = i%2==0?-8:18;
                }
            }
 
            // 映射内存
            static void *pru0DataMemory;
            static int *pru0DataMemory_int;
            prussdrv_map_prumem(PRUSS0_PRU0_DATARAM, &pru0DataMemory);
            pru0DataMemory_int = (int *) pru0DataMemory;
            static void *pru1DataMemory;
            static int *pru1DataMemory_int;
            prussdrv_map_prumem(PRUSS0_PRU1_DATARAM, &pru1DataMemory);
            pru1DataMemory_int = (int *) pru1DataMemory;
 
            // 数据写入PRU内核空间
            *(pru0DataMemory_int) = TICKS_IEP;          //4ms
            *(pru0DataMemory_int+1) = vector_len_;  //number of samples
            *(pru1DataMemory_int) = TICKS_DMT;          //4ms
            *(pru1DataMemory_int+1) = vector_len_;  //number of samples
            for (int i=0; i< vector_len_; i++)
            {
                *(pru0DataMemory_int+2+i) = lumbar_period[i];
                *(pru1DataMemory_int+2+i) = big_arm_period[i];
            }
 
            // PRU开始时间
            struct timeval start_lumbar;
            gettimeofday(&start_lumbar,NULL);
 
            // 加载并执行 PRU 程序
            prussdrv_exec_program (PRU_NUM0, "./redwall_arm_lumbar.bin");
            prussdrv_exec_program (PRU_NUM1, "./redwall_arm_big_arm.bin");
 
            // 等待来自pru的事件完成,返回pru 事件号
            //prussdrv_pru_wait_event (PRU_EVTOUT_0);
            prussdrv_pru_wait_event (PRU_EVTOUT_1);
 
            // pru结束时间
            struct timeval end_lumbar;
            gettimeofday(&end_lumbar,NULL);
 
            double diff;
            diff = end_lumbar.tv_sec -start_lumbar.tv_sec + (end_lumbar.tv_usec - start_lumbar.tv_usec)*0.000001;
            cout<< "lumbar 和 big_arm 程序已完成,历时约 "<< diff << "秒!" << endl;
 
            // 清空数组
            p_lumbar.clear();
            v_lumbar.clear();
            a_lumbar.clear();
            p_big_arm.clear();
            v_big_arm.clear();
            a_big_arm.clear();
            time_from_start.clear();
 
            // 初始化数组长度
            vector_len_ = -1;
 
            // 禁用pru并关闭内存映射
            prussdrv_pru_clear_event (PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);
            prussdrv_pru_clear_event (PRU_EVTOUT_1, PRU1_ARM_INTERRUPT);
            prussdrv_pru_disable(PRU_NUM0);
            prussdrv_pru_disable(PRU_NUM1);
            prussdrv_exit ();
        }
    }
}

 

第一个bin文件使用的是IEP控制时间,新增的第二个bin使用DMTIMER2控制时间,代码的逻辑都是一样的:

 

// PRUSS program to output a simple PWM signal at fixed sample rate (100)
// Output is r30.10 (P8_28) and r30.9 (P8_29)
 
 
.origin 0
.entrypoint START
 
#define PRU1_R31_VEC_VALID 32    // 允许程序完成通知
#define PRU_EVTOUT_1    4        // 发送回的事件号
 
 
// Power, reset, and clock management (PRCM)
#define CM_PER 0x44e00000  // Clock module for peripherals
#define CM_DPLL 0x44e00500 // Clock module for phase-locked loops
#define CONTROL_MODULE 0x44e10000 // Control module (AM335x Tech Ref sec. 9)
 
//offsets
#define CM_PER_L4LS_CLKSTCTRL 0x00  // L4 clock state control register?
#define CM_PER_TIMER2_CLKCTRL 0x80  // timer activity registers...
#define CLKSEL_TIMER2_CLK 0x08      // timer clock source registers..
#define DMTIMER2 0x48040000
 
// offsets  (from sec. 20.1.5 of ARM 335x Technical Reference)
#define TCLR 0x38           // timer control register
#define TCRR 0x3c           // timer counter register
#define TLDR 0x40           // timer load register
#define TTGR 0x44           // timer trigger register
 
 
START:
    // 使能OCP主口
	LBCO	r0, c4, 4, 4
	CLR 	r0, r0, 4
	SBCO	r0, c4, 4, 4
 
    // r0 保存数组元素地址, r1 保存滴答数(4ms), r2 保存数组长度
    // r3 保存延迟因子, r4 保存占空比50
    MOV	r0, 0x00000000
    LBBO	r1, r0, 0, 4
    MOV	r0, 0x00000004
    LBBO	r2, r0, 0, 4    // r2 == 1或者2 说明数组执行完毕
    MOV	r0, 0x00000008
 
CONFIGUETIMER:
	mov	r5, CM_DPLL
	mov	r6, 0x00000001
	sbbo	r6, r5, CLKSEL_TIMER2_CLK, 4  // Set source to CLK_M_OSC
 
	mov	r5, CM_PER
	mov	r6, 0x00000002
	sbbo	r6, r5, CM_PER_TIMER2_CLKCTRL, 4 // Enable DMTIMER2
 
    mov r5, DMTIMER2
    LBBO    r7,r5,TCRR,4        // 获取初始值
 
CONFIGUEPWM:
	ADD	r0, r0, 4           // 跳过第一个速度为0的点
	SUB	r2, r2, 1           // r2 自减
 
    LBBO	r3, r0, 0, 4    // 获取此时速度对应的延迟因子
 
    LBBO    r7,r5,TCRR,4        // 获取初始值
    QBEQ    IFSPEEDZERO, r3, 0      // 判断r3是否为0
    LSR     r12,r3,31
    QBGT    GPIOHIGH,r12, 1  // r3表示速度为正方向
    JMP     GPIOLOW
 
IFSPEEDZERO:
    LBBO    r8,r5,TCRR,4            // 读取timer数值
    SUB     r8,r8,r7                // 这里和IEP有点区别
    QBLT	IFSPEEDZERO, r1, r8     // 执行IFSPEEDZERO, 除非 超时
    QBNE    CONFIGUEPWM, r2, 2     // 下一个点
    JMP     END                    // 如果数据执行完毕了,直接跳转到结束
 
GPIOHIGH:
    SET r30.t9
    JMP     TIMERSTART
GPIOLOW:
    NOT r3,r3
    ADD r3,r3,1
    CLR r30.t9
 
TIMERSTART:
    LBBO    r7,r5,TCRR,4        // 获取初始值
 
PWMCONTROL:
    MOV	r4, 50              // 占空比50
	SET	r30.t10	            // 输出引脚 P9_27 high
SIGNAL_HIGH:
	MOV	r10, r3             // 延迟因子
DELAY_HIGH:
	SUB	r10, r10, 1
	QBNE	DELAY_HIGH, r10, 0
	SUB	r4, r4, 1
	QBNE	SIGNAL_HIGH, r4, 0
	MOV	r4, 50             // 占空比50
	CLR	r30.t10	           // 输出引脚 P9_27 low
SIGNAL_LOW:
	MOV	r10, r3            // 延迟因子
DELAY_LOW:
	SUB	r10, r10, 1
	QBNE	DELAY_LOW, r10, 0
	SUB	r4, r4, 1
	QBNE	SIGNAL_LOW, r4, 0
 
DELAYON:
    LBBO    r8,r5,TCRR,4            // 读取timer数值
    SUB     r8,r8,r7
    QBLT	PWMCONTROL, r1, r8     // 执行PWMCONTROL, 除非 超时
 
TIMERSTOP:
    QBNE    CONFIGUEPWM, r2, 2      // r2 == 1或者2 说明数组执行完毕
 
END:
	MOV	R31.b0, PRU1_R31_VEC_VALID | PRU_EVTOUT_1
	HALT
 

 

上述程序小写部分是增加修改的地方,本来使用DMTIMER2不要使能和设置时钟频率的,直接读取即可,但是考虑到以后可能会用到其他的DMTIMER,所以特地记录一下。运行程序,示波器伺候,波形长度还是4ms,还是相当精确的。

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图(3)

 

下面两张图说明了由于先启动 PRU0,所以第二个波形要比第一个波形晚启动一定的时间,大约是240us。

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图(4)

 

通过ROS控制真实机械臂(13) — PRU-ICSS 定时器 DMTIMER 实现定时功能插图(5)

 

发表评论

后才能评论