电机控制方案
直播中

golabs

8年用户 891经验值
擅长:可编程逻辑 电源/新能源 MEMS/传感技术 测量仪表
私信 关注
[问答]

请问怎样通过PRU-ICSS访问GPIO去实现电机正反转?

怎样用PRU通过OCP主口去访问通用GPIO口?

怎样通过PRU-ICSS访问GPIO去实现电机正反转?
怎样通过ROS去控制真实机械臂?

回帖(1)

杨阳

2021-6-29 11:13:42
  每个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” 加载好设备树之后,发现示波器上的高电平变成了低电平,说明设备树加载成功。
  使用如下主程序测试motor_direction.cpp:
  #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 = 21474836 us#define TICKS ((DELAY_US / 5) * 1000)#define PRU_NUM 0using 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 = 21474836 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 31ENABLEOCP: // 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 将处理好的再写回C4GPIOOUThigh: // 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, TICKSDELAYON: SUB r0, r0, 1 QBNE DELAYON, r0, 0GPIOOUTlow: // 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, TICKSDELAYOFF: 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 // 判断是否置位,也就是如果没有按按钮,则继续MAINLOOPEND: MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0 HALT 因为在电机正反转实验当中,我只需要在电机速度大于0 的时候,让电机正传,只需要让特定GPio口持续输出高电平,速度小于0的时候,gpio输出持续输出低电平即可。所以将上述GPIP输入模式注释掉。上述程序事先的功能是让GPIO在TICK次滴答时间内先输出高电平,然后接下来TICKS次滴答时间内输出高电平,然后结束程序。这个切换时间在几毫秒之内,所以想要用电压表可能很难观测的到(增长延时时间则能清楚观测),因为机械臂规划的速度时间间隔在4ms,所以还是使用示波器来观察,调节到Normal模式,执行程序,发现示波器电压根本没有触发。
  尝试手动加载GPIO:
  root@beaglebone:~# cd /sys/class/gpio/root@beaglebone:/sys/class/gpio# lsexport gpiochip0 gpiochip32 gpiochip64 gpiochip96 unexportroot@beaglebone:/sys/class/gpio# echo 30 》 export root@beaglebone:/sys/class/gpio# lsexport gpio30 gpiochip0 gpiochip32 gpiochip64 gpiochip96 unexportroot@beaglebone:/sys/class/gpio# cd gpio30root@beaglebone:/sys/class/gpio/gpio30# lsactive_low direction edge power subsystem uevent valueroot@beaglebone:/sys/class/gpio/gpio30# cat direction inroot@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 modespinctrl-single,pins = 《0x190 0x05 // P9_31 pr1_pru0_pru_r30_0, MODE7 | OUTPUT | PRU pr0 out spi sclk0x194 0x05 // P9_29 pr1_pru0_pru_r30_1, MODE7 | OUTPUT | PRU pr0 out spi MOSI0x198 0x05 // P9_30 pr1_pru0_pru_r30_2, MODE7 | OUTPUT | PRU pr0 out spi sync0x19c 0x05 // P9_28 pr1_pru0_pru_r30_3, MODE7 | OUTPUT | PRU pr0 in spi CONV0x1ac 0x26 // P9_25 pr1_pru0_pru_r31_7, MODE6 | INPUT | PRU pr0 out spi MISO0x1a4 0x05 // P9_27 pr1_pru0_pru_r30_5, MODE7 | OUTPUT | PRU pr0 in spi sclk0x1a8 0x26 // P9_41 pr1_pru0_pru_r31_6, MODE6 | INPUT | PRU pr0 in spi MISO0x1a0 0x3e // P9_42 。。。 25 and 27 are mucked up on the switch circuit0x0a0 0x05 // P8_45 pr1_pru1_pru_r30_0, MODE7 | OUTPUT | PRU pr1 out spi sclk0x0a4 0x05 // P8_46 pr1_pru1_pru_r30_1, MODE7 | OUTPUT | PRU pr1 out spi MOSI0x0a8 0x05 // P8_43 pr1_pru1_pru_r30_2, MODE7 | OUTPUT | PRU pr1 out spi sync0x0ac 0x05 // P8_44 pr1_pru1_pru_r30_3, MODE7 | OUTPUT | PRU pr1 in spi sclk0x0b8 0x05 // P8_39 pr1_pru1_pru_r30_6, MODE7 | OUTPUT | PRU pr1 in spi CONV0x0b4 0x26 // P8_42 pr1_pru1_pru_r31_5, MODE6 | INPUT | PRU pr1 in spi MISO0x0bc 0x26 // P8_40 pr1_pru1_pru_r31_7, MODE6 | INPUT | PRU pr1 out spi MISO0x0b0 0x26 // P8_410x0e0 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 = int(K / v_lumbar * 0.001); // PRU稳定循环开销1.6u,计算周期的时候需要考虑 //lumbar_period = rand()% 20 - 10; //if(lumbar_period==0){ // lumbar_period = -10; //} lumbar_period = 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; } // 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, 0x00000008CONFIGUETIMER: // 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 bitCONFIGUEPWM: 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 GPIOLOWGPIOHIGH: // p9_31 输出高电平 SET r30.t0 JMP TIMERSTARTGPIOLOW: // p9_31 输出低电平 NOT r3,r3 // 取反+1得到负数的相反数 ADD r3,r3,1 CLR r30.t0TIMERSTART: ***bo r7,r5,0,4 // 使能IEP并且开始计数PWMCONTROL: MOV r4, 50 // 占空比50 SET r30.t5 // 输出引脚 P9_27 highSIGNAL_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 lowSIGNAL_LOW: MOV r10, r3 // 延迟因子DELAY_LOW: SUB r10, r10, 1 QBNE DELAY_LOW, r10, 0 SUB r4, r4, 1 QBNE SIGNAL_LOW, r4, 0DELAYON: 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 = int(K / v_lumbar * 0.001); // PRU稳定循环开销1.6u,计算周期的时候需要考虑 if(i%10==0) { lumbar_period = 0; } else { lumbar_period = 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, 0x00000008CONFIGUETIMER: // 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 bitCONFIGUEPWM: 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 GPIOLOWIFSPEEDZERO: 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 TIMERSTARTGPIOLOW: NOT r3,r3 ADD r3,r3,1 CLR r30.t0TIMERSTART: SBBO r7,r5,0,4 // 使能IEP并且开始计数PWMCONTROL: MOV r4, 50 // 占空比50 SET r30.t5 // 输出引脚 P9_27 highSIGNAL_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 lowSIGNAL_LOW: MOV r10, r3 // 延迟因子DELAY_LOW: SUB r10, r10, 1 QBNE DELAY_LOW, r10, 0 SUB r4, r4, 1 QBNE SIGNAL_LOW, r4, 0DELAYON: 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
举报

更多回帖

发帖
×
20
完善资料,
赚取积分