2025年通过ROS控制真实机械臂(11) --- 通过 PRU-ICSS 访问 GPIO 实现电机正反转

通过ROS控制真实机械臂(11) --- 通过 PRU-ICSS 访问 GPIO 实现电机正反转每个 PRU 都连接着一个 OCP 主口 它允许访问 linux 主机设备对应的内存地址 此功能允许 PRU 控制通用 GPIO 的输入和输出状态 PRU 可访问 Linux 主机内存 但是访问速度要慢上好几倍 因为内存访问需要路由到外部的 PRU ICSS 在通过 PRU ICSS 接口从 OCP 从口接收返回结果 首先测试用 PRU 通过 OCP 主口访问 通用

大家好,我是讯享网,很高兴认识大家。

每个PRU都连接着一个OCP主口,它允许访问linux主机设备对应的内存地址。此功能允许PRU控制通用GPIO的输入和输出状态。PRU可访问Linux主机内存,但是访问速度要慢上好几倍,因为内存访问需要路由到外部的PRU-ICSS,在通过PRU-ICSS接口从/OCP从口接收返回结果。

首先测试用 PRU 通过/OCP主口访问 通用 GPIO 口。

设备树覆盖层如下,用示波器连接beaglebone的GND和P9_11,板子开机发现示波器一直显示的是高电平,如下操作将环境变量写入文件,这样就不用每次开机手动加载环境变量了。

对于通用GPIO口,模式和地址如下查询手册即可。对于增强型GPIO口,0x05表示输出模式,0x26表示输入模式。

__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 0x19c 0x26 // P9_28 pr1_pru0_pru_r31_3, MODE6 | INPUT | PRU >; }; }; 
讯享网
讯享网$ vim ./bashrc     export SLOTS=/sys/devices/bone_capemgr.9/slots     export PINS=/sys/kernel/debug/pinctrl/44e10800.pinmux/pins
$ sudo sh -c "echo EBB-PRU-Example > $SLOTS"
讯享网#include <stdio.h> #include <sys/time.h> #include <sys/poll.h> #include <sys/epoll.h> #include <iostream> #include "prussdrv.h" #include <pruss_intc_mapping.h> #define DELAY_US 4000 // Max. value =  us #define TICKS ((DELAY_US / 5) * 1000) #define PRU_NUM 0 using namespace std; int main(void) {     if(getuid()!=0){         printf("必须使用root权限,否则会提示段错误\n");     }     tpruss_intc_initdata pruss_intc_initdata = PRUSS_INTC_INITDATA;     prussdrv_init();     prussdrv_open(PRU_EVTOUT_0);     prussdrv_pruintc_init( &pruss_intc_initdata);     // PRU开始时间     struct timeval start;     gettimeofday(&start,NULL);     prussdrv_exec_program (PRU_NUM, "./motor_direction.bin");     prussdrv_pru_wait_event (PRU_EVTOUT_0);     // pru结束时间     struct timeval end;     gettimeofday(&end,NULL);     double diff;     diff = end.tv_sec -start.tv_sec + (end.tv_usec - start.tv_usec)*0.000001;     cout<< "EBB PRU程序已完成,历时约 "<< diff << "秒!" << endl;     // prussdrv_pru_clear_event (PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);     prussdrv_pru_disable(PRU_NUM);     prussdrv_exit ();     return 0; }

PRU程序如下motor_direction.p:

// 选择gpio0_30和gpio0_31 对应 p9_11(OUT)和p9_13(IN) .origin 0 .entrypoint ENABLEOCP #define DELAY_US 4000 // Max. value =  us #define TICKS ((DELAY_US / 5) * 1000) #define PRU0_R31_VEC_VALID 32 #define PRU_EVTOUT_0    3 #define GPIO0 0x44e07000         // GPIO 0 See the AM335x TRM,Table 2.2 Peripheral Map #define GPIO1 0x4804c000         // GPIO 1 #define GPIO2 0x481ac000         // GPIO 2 #define GPIO3 0x481ae000         // GPIO 3 #define GPIO_CLEARDATA 0x190     // for clearing the GPIO registers, See the TRM section 25.4.1 #define GPIO_DATAOUT   0x194     // for setting the GPIO registers #define GPIO_DATAIN    0x138     // to read the register data read from GPIO pins #define GPIO0_30 1<<30           // P9_11 gpio0[30] Output - bit 30 #define GPIO0_31 1<<31           // P9_13 gpio0[31] Input - bit 31 ENABLEOCP: // c4表示常量表的入口地址4,即ROU_ICSS CFG地址,加上4偏移量就可以访问SYSCFG寄存器     LBCO    r0, C4, 4, 4     // load SYSCFG reg into r0 (use c4 const addr)     加载C4地址     CLR     r0, r0, 4        // clear bit 4 (STANDBY_INIT)                         enable OCP master ports     SBCO    r0, C4, 4, 4     // store the modified r0 back at the load addr        将处理好的再写回C4 GPIOOUThigh:     // P9_11 为 out,并且一直是高电平(GPIO_DATAOUT)     MOV    r1, GPIO0 | GPIO_DATAOUT    // 基址 | 偏移地址加载gpio并设置     MOV    r2, GPIO0_30                // 将 GPIO0_30 的输出状态写入 r2     SBBO    r2, r1, 0, 4            // 将r2的数据写到r1+0 开始的4个字节地址     MOV    r0, TICKS DELAYON:     SUB    r0, r0, 1     QBNE    DELAYON, r0, 0 GPIOOUTlow:     // P9_11 为 out,并且一直是低电平(GPIO_CLEARDATA)     MOV    r1, GPIO0 | GPIO_CLEARDATA  // 加载 GPIO 并清除数据     MOV    r2, GPIO0_30                // 将 GPIO0_30 的输出状态写入 r2     SBBO    r2, r1, 0, 4            // 将r2的数据写到r1+0 开始的4个字节地址     MOV    r0, TICKS DELAYOFF:     SUB    r0, r0, 1     QBNE    DELAYOFF, r0, 0 //GPIOOUTIN: //    MOV    r5, GPIO0 | GPIO_DATAIN     // 加载 GPIO 并 检测数据的输入 //    LBBO    r6, r5, 0, 4            // 加载r5中的数据放到r6中 //    QBBC    MAINLOOP, r6.t31        // 判断是否置位,也就是如果没有按按钮,则继续MAINLOOP END:                                MOV    R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0     HALT                     
root@beaglebone:~# cd /sys/class/gpio/ root@beaglebone:/sys/class/gpio# ls export    gpiochip0  gpiochip32  gpiochip64  gpiochip96  unexport root@beaglebone:/sys/class/gpio# echo 30 > export  root@beaglebone:/sys/class/gpio# ls export    gpio30    gpiochip0  gpiochip32  gpiochip64  gpiochip96  unexport root@beaglebone:/sys/class/gpio# cd gpio30 root@beaglebone:/sys/class/gpio/gpio30# ls active_low  direction  edge  power  subsystem  uevent  value root@beaglebone:/sys/class/gpio/gpio30# cat direction  in root@beaglebone:/sys/class/gpio/gpio30# echo out > direction  root@beaglebone:/sys/class/gpio/gpio30# cat value  0

再次执行程序,这次观测到了目标的波形,这说明使用PRU访问gpio也需要先将gpio口写入export。

$ reboot

再次从头开始,这次先加载gpio

$ sudo sh -c "echo EBB-PRU-Example > $SLOTS" $ sudo sh -c "echo 30 >/sys/class/gpio/export" $ sudo sh -c "echo out >/sys/class/gpio/gpio30/direction" $ g++ motor_direction.cpp -o motor_direction -lpthread -lprussdrv $ pasm -b motor_direction.p $ sudo ./motor_direction

使用通用型GPIO不仅需要加载设备树,还需要手动写入模式,而且速度相对与增强型GPIO速度要慢上不少。既然都是输出,虽然不过仅仅是高低电平,但是增强型GPIO还是有用武之地的。

现在测试使用PRU内部的增强型GPIO(EGP)。

修改上述设备树覆盖层如下,如果使用的输入输出口比较多,可以参考下面的这些。然后重新编译加载:

pru_pru_pins: pinmux_pru_pru_pins { // The PRU pin modes pinctrl-single,pins = < 0x190 0x05 // P9_31 pr1_pru0_pru_r30_0, MODE7 | OUTPUT | PRU pr0 out spi sclk 0x194 0x05 // P9_29 pr1_pru0_pru_r30_1, MODE7 | OUTPUT | PRU pr0 out spi MOSI 0x198 0x05 // P9_30 pr1_pru0_pru_r30_2, MODE7 | OUTPUT | PRU pr0 out spi sync 0x19c 0x05 // P9_28 pr1_pru0_pru_r30_3, MODE7 | OUTPUT | PRU pr0 in spi CONV 0x1ac 0x26 // P9_25 pr1_pru0_pru_r31_7, MODE6 | INPUT | PRU pr0 out spi MISO 0x1a4 0x05 // P9_27 pr1_pru0_pru_r30_5, MODE7 | OUTPUT | PRU pr0 in spi sclk 0x1a8 0x26 // P9_41 pr1_pru0_pru_r31_6, MODE6 | INPUT | PRU pr0 in spi MISO 0x1a0 0x3e // P9_42 ... 25 and 27 are mucked up on the switch circuit 0x0a0 0x05 // P8_45 pr1_pru1_pru_r30_0, MODE7 | OUTPUT | PRU pr1 out spi sclk 0x0a4 0x05 // P8_46 pr1_pru1_pru_r30_1, MODE7 | OUTPUT | PRU pr1 out spi MOSI 0x0a8 0x05 // P8_43 pr1_pru1_pru_r30_2, MODE7 | OUTPUT | PRU pr1 out spi sync 0x0ac 0x05 // P8_44 pr1_pru1_pru_r30_3, MODE7 | OUTPUT | PRU pr1 in spi sclk 0x0b8 0x05 // P8_39 pr1_pru1_pru_r30_6, MODE7 | OUTPUT | PRU pr1 in spi CONV 0x0b4 0x26 // P8_42 pr1_pru1_pru_r31_5, MODE6 | INPUT | PRU pr1 in spi MISO 0x0bc 0x26 // P8_40 pr1_pru1_pru_r31_7, MODE6 | INPUT | PRU pr1 out spi MISO 0x0b0 0x26 // P8_41 0x0e0 0x26 // P8_27 >;

测试就暂时使用p9_27和p9_31作为PWM和GPIO方向输出(高电平正转,低电**转)。 

__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 >; }; }; 
编译成设备树二进制文件 $ dtc -I dts -O dtb -@ EBB-GPIO-Example-00A0.dts >> EBB-GPIO-Example-00A0.dtbo 覆盖设备树 $ sudo cp EBB-GPIO-Example-00A0.dtbo /lib/firmware 反编译成设备树文本 $ dtc -I dts -O dts -@ EBB-GPIO-Example-00A0.dtbo > EBB-GPIO-Example-00A0.dts

使用和之前一样的加方式,这里有一点需要注意,编译的二进制文件名称必须是-00A0结尾的,也就是要和设备树的内容保持一致,这样加载才是有效的。用示波器连接beaglebone的GND和P9_31,P9_27,修改程序线程代码用于测试:

void *lumbar_motor(void *) { while(1) { usleep(1000); if(v_lumbar.size() == vector_len_) { cout<< "插补规划的数组长度: "<< vector_len_<<endl; // 使用 prussdrv_pruintc_intc 初始化 // PRUSS_INTC_INITDATA 使用的是 pruss_intc_mapping.h头文件 tpruss_intc_initdata pruss_intc_initdata = PRUSS_INTC_INITDATA; // 分配并初始化内存空间 prussdrv_init (); prussdrv_open (PRU_EVTOUT_0); // 映射 PRU 的中断 prussdrv_pruintc_init(&pruss_intc_initdata); // 存储周期数组,谐波减速器的减速比是50,速度数组的单位是弧度每秒 srand((unsigned int)time(NULL)); // n = K*f = K / T;注意单位!!! // 周期单位是ns,延迟因子单位是us // 现在要储存负数了,不能使用unsigned int了,反正范围也不溢出 int lumbar_period[vector_len_]; for (int i=0; i<vector_len_; i++){ // 转换成延时因子 // lumbar_period[i] = int(K / v_lumbar[i] * 0.001); // PRU稳定循环开销1.6u,计算周期的时候需要考虑 //lumbar_period[i] = rand()% 20 - 10; //if(lumbar_period[i]==0){ // lumbar_period[i] = -10; //} lumbar_period[i] = i%2==0?10:-10; // 测试用例,交替更换正负号,方便观察 } // 映射内存 static void *pru0DataMemory; static int *pru0DataMemory_int; prussdrv_map_prumem(PRUSS0_PRU0_DATARAM, &pru0DataMemory); pru0DataMemory_int = (int *) pru0DataMemory; // 数据写入PRU内核空间 *(pru0DataMemory_int) = TICKS; //4ms *(pru0DataMemory_int+1) = vector_len_; //number of samples for (int i=0; i< vector_len_; i++) { *(pru0DataMemory_int+2+i) = lumbar_period[i]; } // PRU开始时间 struct timeval start; gettimeofday(&start,NULL); // 加载并执行 PRU 程序 prussdrv_exec_program (PRU_NUM, "./redwall_arm_client.bin"); // 等待来自pru的事件完成,返回pru 事件号 int n = prussdrv_pru_wait_event (PRU_EVTOUT_0); // pru结束时间 struct timeval end; gettimeofday(&end,NULL); double diff; diff = end.tv_sec -start.tv_sec + (end.tv_usec - start.tv_usec)*0.000001; cout<< "EBB PRU程序已完成,历时约 "<< diff << "秒!" << endl; // 清空数组 p_lumbar.clear(); v_lumbar.clear(); a_lumbar.clear(); time_from_start.clear(); // 初始化数组长度 vector_len_ = -1; // 禁用pru并关闭内存映射 prussdrv_pru_disable(PRU_NUM); prussdrv_exit (); } } }
  • 考虑到速度的正负,需要将原本的无符号unsigned int 转换成有符号整形 int,它们都是占4个字节的,唯一不同的是无符号在寄存器的表示方式是最高位是0对应正数,最高位为1对应负数,最高位作为符号位会导致最大值范围变小,不过操作过程并不会溢出,所以不考虑了。
  • 当速度为负的时候,转换得到的延迟因子也是负,不能通过比较延迟因子和0的大小来判断正负,而是要判断保存延迟因子的二进制数的最高位是0还是1,我通过右移操作实现判断正负。
  • 汇编中可以通过获取绝对值或者对二进制表示的负数取反+1得到其相反数。
// PRUSS program to output a simple PWM signal at fixed sample rate (100) // Output is r30.5 (P9_27) and r30.0 (P9_31) .origin 0 .entrypoint START #define PRU0_R31_VEC_VALID 32 // 允许程序完成通知 #define PRU_EVTOUT_0 3 // 发送回的事件号 #define IEP 0x2E000 // IEP使用常量寄存器 START: // r0 保存数组元素地址, r1 保存滴答数(4ms), r2 保存数组长度 // r3 保存延迟因子, r4 保存占空比50 // r5 保存IEP地址, r6 保存IEP作用地址, r7 保存使能IEP的参数, r8 保存使不能IEP的参数, r9 保存清空操作参数 // r10 临时寄存器,主要用于暂存r3 // r11 保存读取的timer数值,r12用来存储右移的值 MOV r0, 0x00000000 LBBO r1, r0, 0, 4 MOV r0, 0x00000004 LBBO r2, r0, 0, 4 // r2 == 1或者2 说明数组执行完毕 MOV r0, 0x00000008 CONFIGUETIMER: // GLOBAL_CONFIG 0x1 to enable // 0x10 to set default increment // 0x100 to set compensation increment MOV r5, IEP LDI r6, 0 // 使不能作用地址 MOV r7, 0x111 MOV r8, 0 LDI r9, 1 SBBO r8,r5,r6,4 // 使不能 LDI r6, 0xC // 清空作用地址 SBBO r9,r5,r6,4 // clear bit CONFIGUEPWM: ADD r0, r0, 4 // 跳过第一个速度为0的点 SUB r2, r2, 1 // r2 自减 LBBO r3, r0, 0, 4 // 获取此时速度对应的延迟因子 LSR r12,r3,31 // 对r3中的数进行右移操作,右移31位得到最高位的数值,1为负,0为正 QBGT GPIOHIGH,r12, 1 // r3表示速度为正方向 JMP GPIOLOW GPIOHIGH: // p9_31 输出高电平 SET r30.t0 JMP TIMERSTART GPIOLOW: // p9_31 输出低电平 NOT r3,r3 // 取反+1得到负数的相反数 ADD r3,r3,1 CLR r30.t0 TIMERSTART: sbbo r7,r5,0,4 // 使能IEP并且开始计数 PWMCONTROL: MOV r4, 50 // 占空比50 SET r30.t5 // 输出引脚 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.t5 // 输出引脚 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 r11,r5,0xC,4 // 读取timer数值 QBLT PWMCONTROL, r1, r11 // 执行PWMCONTROL, 除非 超时 TIMERSTOP: SBBO r8,r5,0,4 // 停止计数,并停止IEP SBBO r8,r5,0xC,4 // 使计数器的数据为0 QBNE CONFIGUEPWM, r2, 2 // r2 == 1或者2 说明数组执行完毕 END: MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0 HALT 

 

 

----------------------------------------------分割线--------------------------------------------------

实际ROS规划的路径中,机械臂的某个关节的速度有可能很长一段时间都是0,然后上面的测试方法并没有考虑的速度为0,从而导致延迟因子也变成0的情况,所有对测试代码稍作修改,增加了一个IFSPEEDZERO的label,用于处理速度为0 的情况:

int lumbar_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; } else { lumbar_period[i] = i%2==0?-10:10; } } 
// PRUSS program to output a simple PWM signal at fixed sample rate (100) // Output is r30.5 (P9_27) and r30.0 (P9_31) .origin 0 .entrypoint START #define PRU0_R31_VEC_VALID 32 // 允许程序完成通知 #define PRU_EVTOUT_0 3 // 发送回的事件号 #define IEP 0x2E000 // IEP使用常量寄存器 START: // r0 保存数组元素地址, r1 保存滴答数(4ms), r2 保存数组长度 // r3 保存延迟因子, r4 保存占空比50 // r5 保存IEP地址, r6 保存IEP作用地址, r7 保存使能IEP的参数, r8 保存使不能IEP的参数, r9 保存清空操作参数 // r10 临时寄存器,主要用于暂存r3 // r11 保存读取的timer数值,r12用来存储右移的值 MOV r0, 0x00000000 LBBO r1, r0, 0, 4 MOV r0, 0x00000004 LBBO r2, r0, 0, 4 // r2 == 1或者2 说明数组执行完毕 MOV r0, 0x00000008 CONFIGUETIMER: // GLOBAL_CONFIG 0x1 to enable // 0x10 to set default increment // 0x100 to set compensation increment MOV r5, IEP LDI r6, 0 // 使不能作用地址 MOV r7, 0x111 MOV r8, 0 LDI r9, 1 SBBO r8,r5,r6,4 // 使不能 LDI r6, 0xC // 清空作用地址 SBBO r9,r5,r6,4 // clear bit CONFIGUEPWM: ADD r0, r0, 4 // 跳过第一个速度为0的点 SUB r2, r2, 1 // r2 自减 LBBO r3, r0, 0, 4 // 获取此时速度对应的延迟因子 QBEQ IFSPEEDZERO, r3, 0 // 判断r3是否为0 LSR r12,r3,31 QBGT GPIOHIGH,r12, 1 // r3表示速度为正方向 JMP GPIOLOW IFSPEEDZERO: SBBO r7,r5,0,4 // 使能IEP并且开始计数 LBBO r11,r5,0xC,4 // 读取timer数值 QBLT IFSPEEDZERO, r1, r11 // 执行IFSPEEDZERO, 除非 超时 SBBO r8,r5,0,4 // 停止计数,并停止IEP SBBO r8,r5,0xC,4 // 使计数器的数据为0 QBNE CONFIGUEPWM, r2, 2 // 下一个点 JMP END // 如果数据执行完毕了,直接跳转到结束 GPIOHIGH: SET r30.t0 JMP TIMERSTART GPIOLOW: NOT r3,r3 ADD r3,r3,1 CLR r30.t0 TIMERSTART: SBBO r7,r5,0,4 // 使能IEP并且开始计数 PWMCONTROL: MOV r4, 50 // 占空比50 SET r30.t5 // 输出引脚 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.t5 // 输出引脚 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 r11,r5,0xC,4 // 读取timer数值 QBLT PWMCONTROL, r1, r11 // 执行PWMCONTROL, 除非 超时 TIMERSTOP: SBBO r8,r5,0,4 // 停止计数,并停止IEP SBBO r8,r5,0xC,4 // 使计数器的数据为0 QBNE CONFIGUEPWM, r2, 2 // r2 == 1或者2 说明数组执行完毕 END: MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0 HALT

 

 

 

小讯
上一篇 2025-04-07 21:13
下一篇 2025-03-22 18:15

相关推荐

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容,请联系我们,一经查实,本站将立刻删除。
如需转载请保留出处:https://51itzy.com/kjqy/43903.html