随着机器人行业如火如荼的高速发展,类人形机器人也诞生了众多产品和实际应用,而人机一体同步控制也必将应运而生,比较有代表性的就是外骨骼穿戴设备。
本单元系统使用STM32F103/GD32F103作为主控,使用高速硬件IIC采集AS5600的磁编码绝对位置信息,并且符合人体运行的过零检测,然后使用高速CAN总线接口将各个关节数据汇总到主控系统,完成人体关节运动采集和同步。
AS5600是一个易于编程的磁旋转位置传感器,具有高分辨率的12位模拟或PWM输出。该传感器采用非接触式系统,可测量磁化轴上直径的绝对角度,并适用于应用于非接触式电位器的稳健设计,消除了外部杂散磁场的影响。
AS5600的设计目的是为了满足工业标准,并通过I²C接口支持简单的用户编程,无需专门的程序员即可设置非易失性参数。默认情况下,输出表示0到360度的范围。此外,用户还可以定义一个较小的输出范围,并通过编程设置零角(起始位置)和最大值角度(停止位置)。
此外,AS5600还配备了智能低功耗模式,可自动降低功耗。在使用时,输入引脚(DIR)可选择输出的极性以反映旋转方向。如果DIR接地,则输出值随顺时针旋转而增加;如果DIR连接到VDD,则输出值逆时针旋转而增加。
AS5600的I2C驱动
#include "as5600.h"
static uint16_t _rawStartAngle = 0;
static uint16_t _zPosition = 0;
static uint16_t _rawEndAngle = 0;
static uint16_t _mPosition = 0;
static uint16_t _maxAngle = 0;
uint8_t highByte(uint16_t value)
{
uint8_t ret;
value = value>>8;
ret = (uint8_t)value;
return ret;
}
uint8_t lowByte(uint16_t value)
{
uint8_t ret;
value = value&0x00ff;
ret = (uint8_t)value;
return ret;
}
/*******************************************************
Method: readOneByte
In: register to read
Out: data read from i2c
Description: reads one byte register from i2c
******************************************************/
uint8_t readOneByte(uint8_t in_adr)
{
// uint8_t retVal = -1;
//
// Sim_I2C_Read8(_ams5600_Address,in_adr,1,&retVal);
// Sim_I2C1_NOP;
// return retVal;
uint8_t tmp = 0;
eeprom_buffer_read(AS5600_ADDRESS_MAG,&tmp,in_adr,1);
return tmp;
}
/*******************************************************
Method: readOnTwoByte
In: two registers to read
Out: data read from i2c as a int16_t
Description: reads two bytes register from i2c
******************************************************/
uint16_t readTwoBytes(uint8_t in_adr_hi, uint8_t in_adr_lo)
{
uint16_t retVal = (uint16_t)-1;
uint8_t low=0,high=0;
/* Read Low Byte */
low = readOneByte(in_adr_lo);
/* Read High Byte */
high = readOneByte(in_adr_hi);
//printf("high:%d,low:%d ",high,low);
retVal = high << 8;
retVal = retVal | low;
//printf("retVal:%d\r\n",retVal);
return retVal;
}
/*******************************************************
Method: writeOneByte
In: address and data to write
Out: none
Description: writes one byte to a i2c register
******************************************************/
void writeOneByte(uint8_t adr_in, uint8_t dat_in)
{
//uint8_t dat = dat_in;
//Sim_I2C1_Write8(_ams5600_Address,adr_in,1,&dat);
eeprom_buffer_write(AS5600_ADDRESS_MAG,&dat_in,adr_in,1);
}
/****************************************************
Method: AMS_5600
In: none
Out: i2c address of AMS 5600
Description: returns i2c address of AMS 5600
**************************************************/
int16_t getAddress()
{
return _ams5600_Address;
}
/*******************************************************
Method: getMaxAngle
In: none
Out: value of max angle register
Description: gets value of maximum angle register.
******************************************************/
int16_t getMaxAngle(void)
{
return readTwoBytes(_mang_hi, _mang_lo);
}
/*******************************************************
Method: getRawAngle
In: none
Out: value of raw angle register
Description: gets raw value of magnet position.
start, end, and max angle settings do not apply
******************************************************/
int16_t getRawAngle(void)
{
return readTwoBytes(_raw_ang_hi, _raw_ang_lo);
}
/*******************************************************
Method: getStartPosition
In: none
Out: value of start position register
Description: gets value of start position register.
******************************************************/
int16_t getStartPosition(void)
{
return readTwoBytes(_zpos_hi, _zpos_lo);
}
/*******************************************************
Method: getEndPosition
In: none
Out: value of end position register
Description: gets value of end position register.
******************************************************/
int16_t getEndPosition(void)
{
int16_t retVal = readTwoBytes(_mpos_hi, _mpos_lo);
return retVal;
}
/*******************************************************
Method: getScaledAngle
In: none
Out: value of scaled angle register
Description: gets scaled value of magnet position.
start, end, or max angle settings are used to
determine value
******************************************************/
int16_t getScaledAngle(void)
{
return readTwoBytes(_ang_hi, _ang_lo);
}
/*******************************************************
Method: get Agc
In: none
Out: value of AGC register
Description: gets value of AGC register.
******************************************************/
int16_t getAgc()
{
return readOneByte(_agc);
}
/*******************************************************
Method: getMagnitude
In: none
Out: value of magnitude register
Description: gets value of magnitude register.
******************************************************/
int16_t getMagnitude()
{
return readTwoBytes(_mag_hi, _mag_lo);
}
/*******************************************************
Method: getBurnCount
In: none
Out: value of zmco register
Description: determines how many times chip has been
permanently written to.
******************************************************/
int16_t getBurnCount()
{
return readOneByte(_zmco);
}
/*******************************************************
Method: getRawAngle
In: none
Out: value of raw angle register
Description: gets raw value of magnet position.
start, end, and max angle settings do not apply
******************************************************/
int16_t AgetRawAngle(void)
{
return readTwoBytes(_raw_ang_hi, _raw_ang_lo);
}
/*******************************************************
Method: setEndtPosition
In: new end angle position
Out: value of end position register
Description: sets a value in end position register.
If no value is provided, method will read position of
magnet.
******************************************************/
int16_t setEndPosition(int16_t endAngle)
{
if(endAngle == -1)
_rawEndAngle = getRawAngle();
else
_rawEndAngle = endAngle;
writeOneByte(_mpos_hi, highByte(_rawEndAngle));
Delay_MS(2);
writeOneByte(_mpos_lo, lowByte(_rawEndAngle));
Delay_MS(2);
_mPosition = readTwoBytes(_mpos_hi, _mpos_lo);
return(_mPosition);
}
/*******************************************************
Method: setStartPosition
In: new start angle position
Out: value of start position register
Description: sets a value in start position register.
If no value is provided, method will read position of
magnet.
******************************************************/
int16_t setStartPosition(int16_t startAngle)
{
if(startAngle == -1)
{
_rawStartAngle = getRawAngle();
}
else
_rawStartAngle = startAngle;
writeOneByte(_zpos_hi, highByte(_rawStartAngle));
HAL_Delay(2);
writeOneByte(_zpos_lo, lowByte(_rawStartAngle));
HAL_Delay(2);
_zPosition = readTwoBytes(_zpos_hi, _zpos_lo);
return(_zPosition);
}
/*******************************************************
Method: setMaxAngle
In: new maximum angle to set OR none
Out: value of max angle register
Description: sets a value in maximum angle register.
If no value is provided, method will read position of
magnet. Setting this register zeros out max position
register.
******************************************************/
int16_t setMaxAngle(int16_t newMaxAngle)
{
int32_t retVal;
if(newMaxAngle == -1)
{
_maxAngle = getRawAngle();
}
else
_maxAngle = newMaxAngle;
writeOneByte(_mang_hi, highByte(_maxAngle));
Delay_MS(2);
writeOneByte(_mang_lo, lowByte(_maxAngle));
Delay_MS(2);
retVal = readTwoBytes(_mang_hi, _mang_lo);
return retVal;
}
/*******************************************************
Method: detectMagnet
In: none
Out: 1 if magnet is detected, 0 if not
Description: reads status register and examines the
MH bit
******************************************************/
uint8_t detectMagnet(void)
{
uint8_t magStatus;
uint8_t retVal = 0;
/*0 0 MD ML MH 0 0 0*/
/* MD high = AGC minimum overflow, Magnet to strong */
/* ML high = AGC Maximum overflow, magnet to weak*/
/* MH high = magnet detected*/
magStatus = readOneByte(_stat);
if(magStatus & 0x20)
retVal = 1;
return retVal;
}
/*******************************************************
Method: getMagnetStrength
In: none
Out: 0 if no magnet is detected
1 if magnet is to weak
2 if magnet is just right
3 if magnet is to strong
Description: reads status register andexamins the MH,ML,MD bits
******************************************************/
uint8_t getMagnetStrength(void)
{
uint8_t magStatus;
uint8_t retVal = 0;
/*0 0 MD ML MH 0 0 0*/
/* MD high = AGC minimum overflow, Magnet to strong */
/* ML high = AGC Maximum overflow, magnet to weak*/
/* MH high = magnet detected*/
magStatus = readOneByte(_stat);
if(detectMagnet() ==1)
{
retVal = 2; /*just right */
if(magStatus & 0x10)
retVal = 1; /*to weak */
else if(magStatus & 0x08)
retVal = 3; /*to strong */
}
return retVal;
}
/*******************************************************
Method: burnAngle
In: none
Out: 1 success
-1 no magnet
-2 burn limit exceeded
-3 start and end positions not set (useless burn)
Description: burns start and end positions to chip.
THIS CAN ONLY BE DONE 3 TIMES
******************************************************/
uint8_t burnAngle()
{
uint8_t retVal = 1;
_zPosition = getStartPosition();
_mPosition = getEndPosition();
_maxAngle = getMaxAngle();
if(detectMagnet() == 1)
{
if(getBurnCount() < 3)
{
if((_zPosition == 0)&&(_mPosition ==0))
retVal = (uint8_t)-3;
else
writeOneByte(_burn, 0x80);
}
else
retVal = (uint8_t)-2;
}
else
retVal = (uint8_t)-1;
return retVal;
}
/*******************************************************
Method: burnMaxAngleAndConfig
In: none
Out: 1 success
-1 burn limit exceeded
-2 max angle is to small, must be at or above 18 degrees
Description: burns max angle and config data to chip.
THIS CAN ONLY BE DONE 1 TIME
******************************************************/
uint8_t burnMaxAngleAndConfig()
{
uint8_t retVal = 1;
_maxAngle = getMaxAngle();
if(getBurnCount() ==0)
{
if(_maxAngle*0.087 < 18)
retVal = (uint8_t)-2;
else
writeOneByte(_burn, 0x40);
}
else
retVal = (uint8_t)-1;
return retVal;
}
/*******************************************************
Function: convertRawAngleToDegrees
In: angle data from AMS_5600::getRawAngle
Out: human readable degrees as float
Description: takes the raw angle and calculates
float value in degrees.
******************************************************/
float convertRawAngleToDegrees(int16_t newAngle)
{
/* Raw data reports 0 - 4095 segments, which is 0.087 of a degree */
float retVal = newAngle * 0.087; // 360 / 4096
return retVal;
}
void AS5600_Init(void)
{
i2c_gpio_config();
i2c_init_config();
Delay_MS(100);
}
void AS5600_Programe_Run(void)
{
uint16_t rawdata = 0;
float degress = 0;
uint8_t dect = 0;
dect = detectMagnet();
printf("detectMagnet is %d\r\n",dect);
rawdata = getRawAngle();
printf("rawdata is %d\r\n",rawdata);
degress = convertRawAngleToDegrees(rawdata);
printf("degress is %f\r\n",degress);
}
uint16_t AS5600_Get_Pos(void)
{
uint16_t rawdata = 0;
rawdata = readTwoBytes(_raw_ang_hi, _raw_ang_lo);
//convert to 2048 rawdata / 4096 * 2048
//0--360°对应0--2047,注意有0和2047两个边界,是绝对位置,每次开机都是上次的位置
//return (uint16_t)(rawdata * 0.5f);
return (uint16_t)rawdata;
}
uint16_t AS5600_Filter_Pos(uint16_t internval_ms)
{
#define CACHE_SIZE (50)
int i;
uint16_t cache[CACHE_SIZE];
for(i = 0;i < CACHE_SIZE;i++)
{
cache[i] = AS5600_Get_Pos();
if(internval_ms)
{
Delay_MS(internval_ms);
}
}
return Median_Filter(cache,CACHE_SIZE);
}
CAN总线驱动
#include "can.h"
/* select can */
#define CAN0_USED
//#define CAN1_USED
#ifdef CAN0_USED
#define CANX CAN0
#else
#define CANX CAN1
#endif
/* select CAN baudrate */
/* 1MBps */
#define CAN_BAUDRATE 1000
/* 500kBps */
/* #define CAN_BAUDRATE 500 */
/* 250kBps */
/* #define CAN_BAUDRATE 250 */
/* 125kBps */
/* #define CAN_BAUDRATE 125 */
/* 100kBps */
/* #define CAN_BAUDRATE 100 */
/* 50kBps */
/* #define CAN_BAUDRATE 50 */
/* 20kBps */
/* #define CAN_BAUDRATE 20 */
#define PRE_PRIORITY CAN0_NVIC_PREEMPTIONPRIORITY
#define SUB_PRIORITY CAN0_NVIC_SUBPRIORITY
//关节&手柄发送
#define TX_SFID 0x100
#define TX_EFID 0x00
//接收
#define RX_SFID 0x200
#define RX_EFID 0x00
static FlagStatus receive_flag;
static can_receive_message_struct receive_message;
static can_trasnmit_message_struct transmit_message;
/*!
\brief initialize CAN and filter
\param[in] can_parameter
\arg can_parameter_struct
\param[in] can_filter
\arg can_filter_parameter_struct
\param[out] none
\retval none
*/
static void can_networking_init(void)
{
can_parameter_struct can_parameter;
can_filter_parameter_struct can_filter;
can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
can_struct_para_init(CAN_FILTER_STRUCT, &can_filter);
/* initialize CAN register */
can_deinit(CANX);
/* initialize CAN */
can_parameter.time_triggered = DISABLE;
can_parameter.auto_bus_off_recovery = DISABLE;
can_parameter.auto_wake_up = DISABLE;
can_parameter.no_auto_retrans = DISABLE;
can_parameter.rec_fifo_overwrite = DISABLE;
can_parameter.trans_fifo_order = DISABLE;
can_parameter.working_mode = CAN_NORMAL_MODE;
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
can_parameter.time_segment_2 = CAN_BT_BS2_3TQ;
/* 1MBps */
#if CAN_BAUDRATE == 1000
can_parameter.prescaler = 6;
/* 500KBps */
#elif CAN_BAUDRATE == 500
can_parameter.prescaler = 12;
/* 250KBps */
#elif CAN_BAUDRATE == 250
can_parameter.prescaler = 24;
/* 125KBps */
#elif CAN_BAUDRATE == 125
can_parameter.prescaler = 48;
/* 100KBps */
#elif CAN_BAUDRATE == 100
can_parameter.prescaler = 60;
/* 50KBps */
#elif CAN_BAUDRATE == 50
can_parameter.prescaler = 120;
/* 20KBps */
#elif CAN_BAUDRATE == 20
can_parameter.prescaler = 300;
#else
#error "please select list can baudrate in private defines"
#endif
can_init(CANX, &can_parameter);
/* initialize filter */
#ifdef CAN0_USED
/* CAN0 filter number */
can_filter.filter_number = 0;
#else
/* CAN1 filter number */
can_filter.filter_number = 15;
#endif
/* initialize filter */
can_filter.filter_mode = CAN_FILTERMODE_MASK;
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
can_filter.filter_list_high = 0x0000;
can_filter.filter_list_low = 0x0000;
can_filter.filter_mask_high = 0x0000;
can_filter.filter_mask_low = 0x0000;
can_filter.filter_fifo_number = CAN_FIFO0;
can_filter.filter_enable = ENABLE;
can_filter_init(&can_filter);
}
/*!
\brief configure the nested vectored interrupt controller
\param[in] none
\param[out] none
\retval none
*/
static void can_nvic_config(void)
{
#ifdef CAN0_USED
/* configure CAN0 NVIC */
//nvic_irq_enable(CAN0_RX1_IRQn,PRE_PRIORITY,SUB_PRIORITY);
nvic_irq_enable(USBD_LP_CAN0_RX0_IRQn,PRE_PRIORITY,SUB_PRIORITY);
#else
/* configure CAN1 NVIC */
nvic_irq_enable(CAN1_RX1_IRQn,PRE_PRIORITY,SUB_PRIORITY);
#endif
/* enable CAN receive FIFO0 not empty interrupt */
can_interrupt_enable(CANX, CAN_INTEN_RFNEIE0);
}
/*!
\brief configure GPIO
\param[in] none
\param[out] none
\retval none
*/
static void can_gpio_config(void)
{
/* enable CAN clock */
rcu_periph_clock_enable(RCU_CAN0);
rcu_periph_clock_enable(RCU_GPIOA);
/* configure CAN0 GPIO */
gpio_init(GPIOA,GPIO_MODE_IPU,GPIO_OSPEED_50MHZ,GPIO_PIN_11);
gpio_init(GPIOA,GPIO_MODE_AF_PP,GPIO_OSPEED_50MHZ,GPIO_PIN_12);
/**
CAN0_REMAP = "00" PA11/PA12 CAN0_RX/CAN0_TX
CAN0_REMAP = "10" PB8/PB9
CAN0_REMAP = "11" PD0/PD1
*/
}
void gd32_can_init(void)
{
can_gpio_config();
can_networking_init();
can_nvic_config();
}
void gd32_can_send(uint8_t id,uint8_t*data,uint8_t len)
{
if(len <= 0) return;
if(len > 8) return;
/* initialize transmit message */
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
//标准帧标识符11位
transmit_message.tx_sfid = TX_SFID | id;
//扩展帧标识符29位
transmit_message.tx_efid = TX_EFID;
transmit_message.tx_ft = CAN_FT_DATA;
transmit_message.tx_ff = CAN_FF_STANDARD;
transmit_message.tx_dlen = len;
memcpy(transmit_message.tx_data,data,len);
/* transmit message */
can_message_transmit(CANX, &transmit_message);
}
void gd32_can_recv(uint8_t*data,uint8_t*len)
{
if(SET == receive_flag)
{
receive_flag = RESET;
data[0] = receive_message.rx_sfid & 0xFF;
memcpy(data + 1,receive_message.rx_data,receive_message.rx_dlen);
*len = receive_message.rx_dlen + 1;
}
else{
*len = 0;
}
}
//void CAN0_RX1_IRQHandler(void) //CAN_FIFO1
void USBD_LP_CAN0_RX0_IRQHandler(void)
{
/* check the receive message */
can_message_receive(CANX, CAN_FIFO0, &receive_message);
if(RX_SFID == (receive_message.rx_sfid & RX_SFID) &&
CAN_FF_STANDARD == receive_message.rx_ff &&
receive_message.rx_dlen > 0){
receive_flag = SET;
}else{
// error_flag = SET;
}
}
///
假设人体关节活动范围不超过180°的AS5600过零检测
/**
获取合法运行路径和过0检测
*/
static int check_route(uint16_t high_val,uint16_t low_val,uint16_t*route)
{
#define AS5600_MAX_POS (4095)
#define AS5600_MID_POS (2047)
#define AS5600_MIN_POS (0)
//路径
uint16_t route0 = 0;
uint16_t route1 = 0;
//过0检测
int check_0 = 0;
/**
当高位和低位采样数据位于中线的一侧,那么它们就没有过0
*/
if(low_val >= AS5600_MIN_POS &&
low_val < AS5600_MID_POS &&
high_val >= AS5600_MIN_POS &&
high_val < AS5600_MID_POS ||
low_val >= AS5600_MID_POS &&
low_val < AS5600_MAX_POS &&
high_val >= AS5600_MID_POS &&
high_val < AS5600_MAX_POS)
{
route0 = abs(high_val - low_val);
}
/**
当高位或低位采样数据穿过中线,那么路径小的为合法采样范围,
因为运动范围不会超过180°
*/
else
{
route0 = abs(high_val - low_val);
route1 = 4096 - route0;
//过了0
if(route0 >= route1)
{
route0 = route1;
check_0 = 1;
}
}
*route = route0;
return check_0;
}
/**
根据采样数据计算出真实舵机位置
*/
static uint16_t get_real_servo_pos(uint16_t high_val,uint16_t low_val,uint16_t id,uint16_t sample)
{
#define SERVO_MID_POS (512)
uint16_t route = 0;
int check_0 = 0;
//暂时定义起始数据为高位数据
uint16_t AS5600_START = high_val;
uint16_t servo_start = 0;
uint16_t servo_route = 0;
uint16_t adcDatacal = 0;
uint16_t tmpAbsAdc = 0;
uint16_t tmpCalSteer = 0;
uint16_t tmpdata = 0;
switch(id)
{
case 1:
servo_start = 511;
servo_route = 360;
break;
case 2:
servo_start = 872;
servo_route = 720;
break;
case 3:
servo_start = 152;
servo_route = 360;
break;
case 4:
servo_start = 115;
servo_route = 720;
break;
case 5:
servo_start = 908;
servo_route = 720;
break;
case 6:
servo_start = 871;
servo_route = 360;
break;
case 7:
servo_start = 151;
servo_route = 720;
break;
case 8:
servo_start = 512;
servo_route = 360;
break;
case 9:
servo_start = 888;
servo_route = 753;
break;
default:
return (uint16_t)-1;
}
check_0 = check_route(high_val,low_val,&route);
//无论是否过0,都有一个大值和一个小值
if(high_val < low_val)
{
tmpdata = high_val;
high_val = low_val;
low_val = tmpdata;
}
if(check_0)
{
//从最大数据开始旋转
if(AS5600_START == high_val)
{
if(sample < AS5600_START)
{
if(sample >= 2047)
{
sample = AS5600_START;
}
}
if(sample > low_val)
{
if(sample < 2047)
{
sample = low_val;
}
}
if(sample >= AS5600_START && sample <= 4095)
{
adcDatacal = sample - AS5600_START;
}
else if(sample >= 0 && sample <= low_val)
{
adcDatacal = 4095 - AS5600_START + 1 + sample;
}
}
//从最小数据开始旋转
else
{
if(sample > AS5600_START)
{
if(sample <= 2047)
{
sample = AS5600_START;
}
}
if(sample < high_val)
{
if(sample > 2047)
{
sample = high_val;
}
}
if(sample >= 0 && sample <= AS5600_START)
{
adcDatacal = AS5600_START - sample;
}
else if(sample >= high_val && sample <= 4095)
{
adcDatacal = 4095 - sample + 1 + AS5600_START;
}
}
tmpAbsAdc = adcDatacal;
}
else
{
if(AS5600_START == high_val)
{
if(sample > AS5600_START)
{
adcDatacal = AS5600_START;
}
else if(sample < AS5600_START - route)
{
adcDatacal = AS5600_START - route;
}
else
{
adcDatacal = sample;
}
tmpAbsAdc = AS5600_START - adcDatacal;
}
else
{
if(sample < AS5600_START)
{
adcDatacal = AS5600_START;
}
else if(sample > AS5600_START + route)
{
adcDatacal = AS5600_START + route;
}
else
{
adcDatacal = sample;
}
tmpAbsAdc = adcDatacal - AS5600_START;
}
}
tmpCalSteer = (uint16_t)(tmpAbsAdc * servo_route * 1.0f / route);
if(servo_start < SERVO_MID_POS)
{
return servo_start + tmpCalSteer;
}
else
{
return servo_start - tmpCalSteer;
}
}
主任务采集和上报关节位置信息
/*!
\file main.c
\brief USART printf
\version 2014-12-26, V1.0.0, firmware for GD32F10x
\version 2017-06-20, V2.0.0, firmware for GD32F10x
\version 2018-07-31, V2.1.0, firmware for GD32F10x
*/
/*
Copyright (c) 2018, GigaDevice Semiconductor Inc.
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/
#include "gd32f10x.h"
#include <stdio.h>
#include "systick.h"
#include "usart0.h"
#include "delay.h"
#include "adc.h"
#include "as5600.h"
#include "can.h"
#define LOCAL_ID (0x01)
#define THRESHOLD (5)
static uint16_t data_before;
/*!
\brief main function
\param[in] none
\param[out] none
\retval none
*/
int main(void)
{
int data;
uint8_t senddata[10] = {0};
uint8_t recvdata[10] = {0};
uint8_t recvdatalen = 0;
Set_Global_NVIC();
systick_config();
USART0_Init();
adc_init();
AS5600_Init();
gd32_can_init();
while(1)
{
data = AS5600_Filter_Pos(0);
if(abs(data - data_before) > THRESHOLD)
{
data_before = data;
senddata[0] = data >> 8;
senddata[1] = data & 0xFF;
gd32_can_send(LOCAL_ID,senddata,2);
//for print
{
uint8_t buf[32] = {0x00};
sprintf(buf,"id = %d\tpos = %d\r\n",LOCAL_ID,data);
USART0_Send(buf,strlen(buf));
}
}
else{
delay_1ms(5);
}
}
}