配合笔记参考http://note.youdao.com/noteshare?id=4d918ff95b075f08eb49aa081287d90f
#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>
#include<stdlib.h>
#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
static void pabort(const char *s)
{
perror(s);
abort();
}
static const char *device = "/dev/spidev1.0"; //设备名
static uint8_t mode; //设备模式
static uint8_t bits = 8;
static uint32_t speed = 500000;
static uint16_t delay;
//对TMC寄存器的宏定义
unsigned int GCONF = 0x00;
unsigned int CHOPCONF = 0x6c;
unsigned int IHOLD_IRUN = 0x30;
unsigned int TZEROWAIT = 0x2c;
unsigned int PWMCONF = 0x10;
unsigned int VHIGH = 0x32;
unsigned int VCOOLTHRS = 0x31;
unsigned int RAMPMODE = 0x20;
//速度参数寄存器
unsigned int VSTART = 0x23;
unsigned int A1 = 0x24;
unsigned int V1 = 0x25;
unsigned int AMAX = 0x26;
unsigned int VMAX = 0x27;
unsigned int DMAX = 0x28;
unsigned int D1 = 0x2A;
unsigned int VSTOP = 0x2B;
//当前速度寄存器
unsigned int VACTUAL = 0x22;
//当前位置寄存器
unsigned int XACTUAL = 0x21;
//给定位置寄存器
unsigned int XTARGET = 0x2D;
/*******************************************************************
* 名称： sensor_write_register
* 功能： 将信息通过SPI写入外挂设备的指定寄存器的信息
* 入口参数： fd SPI文件描述符
* addr 设备寄存器地址
* data 写入的数据
*出口参数： 无
*******************************************************************/
static void sensor_write_register(int fd,unsigned int addr, unsigned int data)
{
int ret;
//定义待发送的数据
uint8_t tx[5];
tx[0] = 0X80 | addr & 0xff;
tx[1] = 0xff & (data >> 24) ;
tx[2] = 0xff & (data >> 16) ;
tx[3] = 0xff & (data >> 8) ;
tx[4] = 0xff & (data >> 0) ;
uint8_t rx[ARRAY_SIZE(tx)] = {0};
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)tx, //定义发送缓冲区指针
.rx_buf = (unsigned long)rx, //定义接收缓冲区指针
.len = ARRAY_SIZE(tx),
.delay_usecs = delay,
.speed_hz = speed,
.bits_per_word = bits,
};
//进行一次读写操作
ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);//执行spidev.c中ioctl的default进行数据传输
if (ret == 1)
pabort("can't send spi message");
}
/*******************************************************************
* 名称： sensor_read_register
* 功能： 读取外挂设备指定寄存器的数据
* 入口参数： fd SPI文件描述符
* addr 设备寄存器地址
* rev 接收收到的数据
*出口参数： 返回读取的数据长度
*******************************************************************/
int sensor_read_register(int fd,unsigned int addr,uint8_t rev[])
{
int ret;
//定义待发送的数据
uint8_t tx[5];
tx[0] = addr;
int t;
int len;
uint8_t rx[ARRAY_SIZE(tx)] = {0};
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)tx, //定义发送缓冲区指针
.rx_buf = (unsigned long)rx, //定义接收缓冲区指针
.len = ARRAY_SIZE(tx),
.delay_usecs = delay,
.speed_hz = speed,
.bits_per_word = bits,
};
//进行一次读写操作
ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);//执行spidev.c中ioctl的default进行数据传输
if (ret == 1)
pabort("can't send spi message");
for(ret=0; ret<ARRAY_SIZE(rx);ret++)
{
rev[ret] = rx[ret];
}
return len = ARRAY_SIZE(rx);
}
/*******************************************************************
* 名称： Spi_Set
* 功能： 配置spi总线的参数
* 入口参数： fd SPI文件描述符
* addr 设备寄存器地址
* rev 接收收到的数据
*出口参数： 正确返回大于0 错误返回小于0
*******************************************************************/
int Spi_Set(int fd)
{
int ret = -1;
mode |= SPI_CPHA;//时钟相位
mode |= SPI_CPOL;//时钟继续
mode &= ~SPI_CS_HIGH; //片选高电平
ret = ioctl(fd, SPI_IOC_WR_MODE, &mode); //SPI模式设置可写
if (ret == -1)
pabort("can't set spi mode");
ret = ioctl(fd, SPI_IOC_RD_MODE, &mode); //SPI模式设置可读
if (ret == -1)
pabort("can't get spi mode");
ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits); //SPI的bit/word设置可写
if (ret == -1)
pabort("can't set bits per word");
ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits); //SPI的bit/word设置可读
if (ret == -1)
pabort("can't get bits per word");
ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed); //SPI的波特率设置可写
if (ret == -1)
pabort("can't set max speed hz");
ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed); //SPI的波特率设置可读
if (ret == -1)
pabort("can't get max speed hz");
printf("spi mode: %d\n", mode);
printf("bits per word: %d\n", bits);
printf("max speed: %d Hz (%d KHz)\n", speed, speed/1000);
return ret;
}
/*******************************************************************
* 名称： Twe_Addr
* 功能： 计算第二通道的偏移量
* 入口参数： addr 设备寄存器的偏移基地址
*出口参数： 返回偏移后的地址
*******************************************************************/
unsigned int Twe_Addr(unsigned int addr)
{
int new_addr = addr + 0x20;
}
/*******************************************************************
* 名称： Math_Acceleration
* 功能： 计算云台的实际加速度
* 入口参数： Accel
*出口参数： 无
*******************************************************************/
void Math_Acceleration(unsigned int Accel)
{
//计算实际加速度(Acceleration_Speed = 12.4*2^-4*寄存器的值 = 0.775*10000)
double Acceleration = 0.775*Accel;
printf("Acceleration:%02lf\n", Acceleration);
}
/*******************************************************************
* 名称： Tmc_Init
* 功能： 初始化TMC5041的寄存器参数
* 入口参数： fd SPI文件描述符
*出口参数： 无
*******************************************************************/
void Tmc_Init(int fd)
{
//第一组初始化
sensor_write_register(fd,GCONF,0x00000008); // GCONF=8: Enable Position compare pulse and INT outputs
sensor_write_register(fd,CHOPCONF,0x000100C5); // CHOPCONF: TOFF=5, HSTRT=4, HEND=1, TBL=2, CHM=0 (spreadCycle)
sensor_write_register(fd,IHOLD_IRUN,0x00011F05); // IHOLD_IRUN: IHOLD=5, IRUN=31 (max. current), IHOLDDELAY=1000（静止电流为5，检测电阻，控制掉电后的周期）
sensor_write_register(fd,TZEROWAIT,0x00002710); // TZEROWAIT=10000 DECE后的wait时间 （下降到零速度或反向旋转的等待时间）
sensor_write_register(fd,PWMCONF,0x000401C8); // PWM_CONF: AUTO=1, 1/1024 Fclk, Switch amplitude limit=200, Grad=1
sensor_write_register(fd,VHIGH,0x00061A80); // VHIGH=400 000: Set VHIGH to a high value to allow stealthChop
sensor_write_register(fd,VCOOLTHRS,0x00007530); // VCOOLTHRS=30000: Set upper limit for stealthChop to about 30RPM
sensor_write_register(fd,VSTART,0X00000005);// VSTART = 1
sensor_write_register(fd,VSTOP,0X0000000A); // VSTOP = 10 Stop velocity (Near to zero)
//第二组通道初始化
sensor_write_register(fd,(CHOPCONF+0x10),0x000100C5); // CHOPCONF: TOFF=5, HSTRT=4, HEND=1, TBL=2, CHM=0 (spreadCycle)
sensor_write_register(fd,PWMCONF+0x08,0x000401C8); // PWM_CONF: AUTO=1, 1/1024 Fclk, Switch amplitude limit=200, Grad=1
sensor_write_register(fd,Twe_Addr(IHOLD_IRUN),0x00011F05); // IHOLD_IRUN: IHOLD=5, IRUN=31 (max. current), IHOLDDELAY=1000
sensor_write_register(fd,Twe_Addr(TZEROWAIT),0x00002710); // TZEROWAIT=10000 DECE后的wait时间
sensor_write_register(fd,Twe_Addr(VHIGH),0x00061A80); // VHIGH=400 000: Set VHIGH to a high value to allow stealthChop
sensor_write_register(fd,Twe_Addr(VCOOLTHRS),0x00007530); // VCOOLTHRS=30000: Set upper limit for stealthChop to about 30RPM
sensor_write_register(fd,Twe_Addr(VSTART),0X00000005);// VSTART = 1
sensor_write_register(fd,Twe_Addr(VSTOP),0X0000000A); // VSTOP = 10 Stop velocity (Near to zero)
}
/*******************************************************************
* 名称： Curr_Pos
* 功能： 初始化TMC5041的寄存器参数
* 入口参数： fd SPI文件描述符
* rev 保存位置的信息
*出口参数： 无
*******************************************************************/
void Curr_Pos(int fd,uint8_t rev[])
{
int i =0;
int len_rl = sensor_read_register(fd,XACTUAL,rev);
for(i=0;i<len_rl;i++)
{
if(i==0)
printf("RL\n");
printf("%02x ",rev[i]);
}
int len_ud = sensor_read_register(fd,Twe_Addr(XACTUAL),rev);
for(i=0;i<len_ud;i++)
{
if(i==0)
printf("UD\n");
printf("%02x ",rev[i]);
}
}
/*******************************************************************
* 名称： Motion_Left
* 功能： 给定一定速度来驱动电机左转
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Left(int fd)
{
//将v1和vmax设为一样的
sensor_write_register(fd,V1,0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,VMAX,0X00003A98); // VMAX = 100 00
sensor_write_register(fd,A1,0X000000C8); // // A1 = 200
sensor_write_register(fd,AMAX,0X000000C8); // AMAX = 200
sensor_write_register(fd,D1,0X000003E8); // D1 = 1000
sensor_write_register(fd,DMAX,0X000003E8); // DMAX = 1000
//正向旋转
sensor_write_register(fd,RAMPMODE,0x00000001); // RAMPMODE=1 (positive velocity)
Math_Acceleration(200);
return 0;
}
/*******************************************************************
* 名称： Motion_Right
* 功能： 给定一定速度来驱动电机右转
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Right(int fd)
{
//将v1和vmax设为一样的
sensor_write_register(fd,V1,0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,VMAX,0X00003A98); // VMAX = 100 00
sensor_write_register(fd,A1,0X000000C8); // // A1 = 200
sensor_write_register(fd,AMAX,0X000000C8); // AMAX = 200
sensor_write_register(fd,D1,0X000003E8); // D1 = 1000
sensor_write_register(fd,DMAX,0X000003E8); // DMAX = 1000
//反向旋转
sensor_write_register(fd,RAMPMODE,0x00000002); // RAMPMODE=1 (negative velocity)
//计算实际加速度(Acceleration_Speed = 12.4*2^-4*寄存器的值 = 0.775*10000)
Math_Acceleration(200);
return 0;
}
/*******************************************************************
* 名称： Motion_Up
* 功能： 给定一定速度来驱动电机向上转
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Up(int fd)
{
//将v1和vmax设为一样的
sensor_write_register(fd,Twe_Addr(V1),0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,Twe_Addr(VMAX),0X00003A98); // VMAX = 100 00
sensor_write_register(fd,Twe_Addr(A1),0X000000C8); // // A1 = 200
sensor_write_register(fd,Twe_Addr(AMAX),0X000000C8); // AMAX = 200
sensor_write_register(fd,Twe_Addr(D1),0X000003E8); // D1 = 1000
sensor_write_register(fd,Twe_Addr(DMAX),0X000003E8); // DMAX = 1000
//正向旋转
sensor_write_register(fd,Twe_Addr(RAMPMODE),0x00000001); // RAMPMODE=1 (negative velocity)
//计算实际加速度(Acceleration_Speed = 12.4*2^-4*寄存器的值 = 0.775*10000)
Math_Acceleration(200);
return 0;
}
/*******************************************************************
* 名称： Motion_Down
* 功能： 给定一定速度来驱动电机向下转
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Down(int fd)
{
//将v1和vmax设为一样的
sensor_write_register(fd,Twe_Addr(V1),0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,Twe_Addr(VMAX),0X00003A98); // VMAX = 100 00
sensor_write_register(fd,Twe_Addr(A1),0X000000C8); // // A1 = 200
sensor_write_register(fd,Twe_Addr(AMAX),0X000000C8); // AMAX = 200
sensor_write_register(fd,Twe_Addr(D1),0X000003E8); // D1 = 1000
sensor_write_register(fd,Twe_Addr(DMAX),0X000003E8); // DMAX = 1000
//反向旋转
sensor_write_register(fd,Twe_Addr(RAMPMODE),0x00000002); // RAMPMODE=1 (negative velocity)
Math_Acceleration(200);
return 0;
}
/*******************************************************************
* 名称： Motion_Stop_RL
* 功能： 控制左右电机停止
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Stop_RL(int fd)
{
sensor_write_register(fd,V1,0X00000000); // V1 = 50 000 Acceleration threshold velocity V1
sensor_write_register(fd,VMAX,0X00000000); // VMAX = 50 000
return 0;
}
/*******************************************************************
* 名称： Motion_Stop_UD
* 功能： 控制上下电机停止
* 入口参数： fd SPI文件描述符
*出口参数： 0
*******************************************************************/
int Motion_Stop_UD(int fd)
{
sensor_write_register(fd,Twe_Addr(V1),0X00000000); // V1 = 50 000 Acceleration threshold velocity V1
sensor_write_register(fd,Twe_Addr(VMAX),0X00000000); // VMAX = 50 000
return 0;
}
/*******************************************************************
* 名称： Motion_MovPos
* 功能： 控制上下电机停止
* 入口参数： fd SPI文件描述符
* Position_UD 给定需要指定的上下位置
* Position_RL 给定需要指定的左右位置
*出口参数： 0
*******************************************************************/
void Motion_MovPos(int fd, unsigned int Position_UD, unsigned int Position_RL)
{
//设置左右方向的参数
sensor_write_register(fd,V1,0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,VMAX,0X00003A98); // VMAX = 100 00
sensor_write_register(fd,A1,0X000000C8); // // A1 = 200
sensor_write_register(fd,AMAX,0X000000C8); // AMAX = 200
sensor_write_register(fd,D1,0X000003E8); // D1 = 1000
sensor_write_register(fd,DMAX,0X000003E8); // DMAX = 1000
sensor_write_register(fd,RAMPMODE,0x00000000); // RAMPMODE=1 (positive velocity)
//设置上下方向的参数
sensor_write_register(fd,Twe_Addr(V1),0X00003A98); // V1 = 100 00 Acceleration threshold velocity V1
sensor_write_register(fd,Twe_Addr(VMAX),0X00003A98); // VMAX = 100 00
sensor_write_register(fd,Twe_Addr(A1),0X000000C8); // // A1 = 200
sensor_write_register(fd,Twe_Addr(AMAX),0X000000C8); // AMAX = 200
sensor_write_register(fd,Twe_Addr(D1),0X000003E8); // D1 = 1000
sensor_write_register(fd,Twe_Addr(DMAX),0X000003E8); // DMAX = 1000
sensor_write_register(fd,Twe_Addr(RAMPMODE),0x00000000); // RAMPMODE=1 (negative velocity)
//选择上下左右的位置
sensor_write_register(fd,Twe_Addr(XTARGET),Position_UD);
sensor_write_register(fd,Twe_Addr(XTARGET),Position_RL);
}
int main(int argc, char *argv[])
{
int exit=0;
int ret = -1;
int fd;
uint8_t rev[50];
fd = open(device, O_RDWR); //打开"/dev/spidev1.0"
if (fd < 0)
pabort("can't open device");
ret = Spi_Set(fd);
if(ret < 0)
{
printf("spi set error\n");
}
Tmc_Init(fd);
while (!exit)
{
int ch = getchar();
switch(ch)
{
case 'h':
Motion_Right(fd);
sleep(1);
Motion_Left(fd);
sleep(1);
Motion_Up(fd);
sleep(1);
Motion_Down(fd);
sleep(1);
//回到指定位置
Motion_MovPos(fd,0Xfffff155,0X00000000);
break;
case 's':
printf("----------Motion_Stop_RL--------\n");
Motion_Stop_RL(fd);
break;
case 'a':
printf("----------Motion_Stop_UD--------\n");
Motion_Stop_UD(fd);
break;
case 'r':
printf("---------Motion_Right-----------\n");
Motion_Right(fd);
break;
case 'l':
printf("---------Motion_Left------------\n");
Motion_Left(fd);
break;
case 'u':
printf("----------Motion_Up-------------\n");
Motion_Up(fd);
break;
case 'd':
printf("----------Motion_Down-----------\n");
Motion_Down(fd);
break;
case 'p':
Curr_Pos(fd,rev);
default:
break;
}
}
close(fd);
return ret;
}
ranyueの月染霜华
|
ranyueの月染霜华
|
基于海思HI3519V101—SPI应用编程驱动TMC5041
2020-06-05
