/*
* SMSBCL.c
*/
#include "main.h"
#include <string.h>
#include "feetech_STS.h"
extern UART_HandleTypeDef huart3;
uint32_t IOTimeOut = 100;
uint32_t IOTimeOut_zero_correction = 500;
uint8_t wBuf[128];
uint8_t wLen = 0;
void STS_Uart_Send(uint8_t *buf , uint16_t len)
{
HAL_UART_Transmit(&huart3, buf, len,100);//
}
int16_t STS_Uart_Read(uint8_t *buf , uint16_t len, uint32_t timeout)
{
if(HAL_UART_Receive(&huart3, buf, len, timeout)==HAL_OK){
return len;
}else{
return 0;
}
}
int readSCS(unsigned char *nDat, int nLen)
{
return STS_Uart_Read(nDat, nLen, IOTimeOut);
}
int readSCS_zero_correction(unsigned char *nDat, int nLen)
{
return STS_Uart_Read(nDat, nLen, IOTimeOut_zero_correction);
}
int writeSCS(unsigned char *nDat, int nLen)
{
while(nLen--){
if(wLen<sizeof(wBuf)){
wBuf[wLen] = *nDat;
wLen++;
nDat++;
}
}
return wLen;
}
void rFlushSCS()
{
}
void wFlushSCS()
{
if(wLen){
STS_Uart_Send(wBuf, wLen);
wLen = 0;
}
}
static uint8_t Level =1;
static uint8_t End = 0;
void Host2SCS(uint8_t *DataL, uint8_t* DataH, int Data)
{
if(End){
*DataL = (Data>>8);
*DataH = (Data&0xff);
}else{
*DataH = (Data>>8);
*DataL = (Data&0xff);
}
}
int SCS2Host(uint8_t DataL, uint8_t DataH)
{
int Data;
if(End){
Data = DataL;
Data<<=8;
Data |= DataH;
}else{
Data = DataH;
Data<<=8;
Data |= DataL;
}
return Data;
}
int checkHead(void)
{
uint8_t bDat;
uint8_t bBuf[2] = {0, 0};
uint8_t Cnt = 0;
while(1){
if(!readSCS(&bDat, 1)){
return 0;
}
bBuf[1] = bBuf[0];
bBuf[0] = bDat;
if(bBuf[0]==0xff && bBuf[1]==0xff){
break;
}
Cnt++;
if(Cnt>10){
return 0;
}
}
return 1;
}
int checkHead_zero_correction(void)
{
uint8_t bDat;
uint8_t bBuf[2] = {0, 0};
uint8_t Cnt = 0;
while(1){
if(!readSCS_zero_correction(&bDat, 1)){
return 0;
}
bBuf[1] = bBuf[0];
bBuf[0] = bDat;
if(bBuf[0]==0xff && bBuf[1]==0xff){
break;
}
Cnt++;
if(Cnt>10){
return 0;
}
}
return 1;
}
int Ack(uint8_t ID)
{
uint8_t bBuf[4];
uint8_t calSum;
//Error = 0;
if(ID!=0xfe && Level){
if(!checkHead()){
return 0;
}
if(readSCS(bBuf, 4)!=4){
return 0;
}
if(bBuf[0]!=ID){
return 0;
}
if(bBuf[1]!=2){
return 0;
}
calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
return 0;
}
//Error = bBuf[2];
}
return 1;
}
int Ack_zero_correction(uint8_t ID)
{
uint8_t bBuf[4];
uint8_t calSum;
//Error = 0;
if(ID!=0xfe && Level){
if(!checkHead_zero_correction()){
return DC_status_disconnect;
}
if(readSCS_zero_correction(bBuf, 4)!=4){
return DC_status_disconnect;
}
if(bBuf[0]!=ID){
return DC_status_disconnect;
}
if(bBuf[1]!=2){
return DC_status_disconnect;
}
calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
return DC_status_disconnect;
}
//Error = bBuf[2];
}
return DC_status_complete;
}
void writeBuf(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen, uint8_t Fun)
{
uint8_t i;
uint8_t msgLen = 2;
uint8_t bBuf[6];
uint8_t CheckSum = 0;
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = ID;
bBuf[4] = Fun;
if(nDat){
msgLen += nLen + 1;
bBuf[3] = msgLen;
bBuf[5] = MemAddr;
writeSCS(bBuf, 6);
}else{
bBuf[3] = msgLen;
writeSCS(bBuf, 5);
}
CheckSum = ID + msgLen + Fun + MemAddr;
if(nDat){
for(i=0; i<nLen; i++){
CheckSum += nDat[i];
}
writeSCS(nDat, nLen);
}
CheckSum = ~CheckSum;
writeSCS(&CheckSum, 1);
}
int genWrite(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
int genWrite_zero_correction(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
wFlushSCS();
return Ack_zero_correction(ID);
}
int regWrite(uint8_t ID, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
rFlushSCS();
writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
wFlushSCS();
return Ack(ID);
}
int regAction(uint8_t ID)
{
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_REG_ACTION);
wFlushSCS();
return Ack(ID);
}
void syncWrite(uint8_t ID[], uint8_t IDN, uint8_t MemAddr, uint8_t *nDat, uint8_t nLen)
{
uint8_t mesLen = ((nLen+1)*IDN+4);
uint8_t Sum = 0;
uint8_t bBuf[7];
uint8_t i, j;
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = 0xfe;
bBuf[3] = mesLen;
bBuf[4] = INST_SYNC_WRITE;
bBuf[5] = MemAddr;
bBuf[6] = nLen;
rFlushSCS();
writeSCS(bBuf, 7);
Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
for(i=0; i<IDN; i++){
writeSCS(&ID[i], 1);
writeSCS(nDat+i*nLen, nLen);
Sum += ID[i];
for(j=0; j<nLen; j++){
Sum += nDat[i*nLen+j];
}
}
Sum = ~Sum;
writeSCS(&Sum, 1);
wFlushSCS();
}
int writeByte(uint8_t ID, uint8_t MemAddr, uint8_t bDat)
{
rFlushSCS();
writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
int writeWord(uint8_t ID, uint8_t MemAddr, uint16_t wDat)
{
uint8_t buf[2];
Host2SCS(buf+0, buf+1, wDat);
rFlushSCS();
writeBuf(ID, MemAddr, buf, 2, INST_WRITE);
wFlushSCS();
return Ack(ID);
}
int Read(uint8_t ID, uint8_t MemAddr, uint8_t *nData, uint8_t nLen)
{
int Size;
uint8_t bBuf[4];
uint8_t calSum;
uint8_t i;
rFlushSCS();
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
wFlushSCS();
if(!checkHead()){
return 0;
}
//Error = 0;
if(readSCS(bBuf, 3)!=3){
return 0;
}
Size = readSCS(nData, nLen);
if(Size!=nLen){
return 0;
}
if(readSCS(bBuf+3, 1)!=1){
return 0;
}
calSum = bBuf[0]+bBuf[1]+bBuf[2];
for(i=0; i<Size; i++){
calSum += nData[i];
}
calSum = ~calSum;
if(calSum!=bBuf[3]){
return 0;
}
//Error = bBuf[2];
return Size;
}
int readByte(uint8_t ID, uint8_t MemAddr)
{
uint8_t bDat;
int Size = Read(ID, MemAddr, &bDat, 1);
if(Size!=1){
return -1;
}else{
return bDat;
}
}
int readWord(uint8_t ID, uint8_t MemAddr)
{
uint8_t nDat[2];
int Size;
uint16_t wDat;
Size = Read(ID, MemAddr, nDat, 2);
if(Size!=2)
return -1;
wDat = SCS2Host(nDat[0], nDat[1]);
return wDat;
}
int Ping(uint8_t ID)
{
uint8_t bBuf[4];
uint8_t calSum;
rFlushSCS();
writeBuf(ID, 0, NULL, 0, INST_PING);
wFlushSCS();
//Error = 0;
if(!checkHead()){
return -1;
}
if(readSCS(bBuf, 4)!=4){
return -1;
}
if(bBuf[0]!=ID && ID!=0xfe){
return -1;
}
if(bBuf[1]!=2){
return -1;
}
calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
if(calSum!=bBuf[3]){
return -1;
}
//Error = bBuf[2];
return bBuf[0];
}
static uint8_t Mem[SMSBCL_PRESENT_CURRENT_H-SMSBCL_PRESENT_POSITION_L+1];
static int Err = 0;
int getErr(void)
{
return Err;
}
int WritePosEx(uint8_t ID, int16_t Position, uint16_t Speed, uint8_t ACC)
{
uint8_t bBuf[7];
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return genWrite(ID, SMSBCL_ACC, bBuf, 7);
}
int RegWritePosEx(uint8_t ID, int16_t Position, uint16_t Speed, uint8_t ACC)
{
uint8_t bBuf[7];
if(Position<0){
Position = -Position;
Position |= (1<<15);
}
bBuf[0] = ACC;
Host2SCS(bBuf+1, bBuf+2, Position);
Host2SCS(bBuf+3, bBuf+4, 0);
Host2SCS(bBuf+5, bBuf+6, Speed);
return regWrite(ID, SMSBCL_ACC, bBuf, 7);
}
void RegWriteAction(void)
{
regAction(0xfe);
}
void SyncWritePosEx(uint8_t ID[], uint8_t IDN, int16_t Position[], uint16_t Speed[], uint8_t ACC[])
{
uint8_t offbuf[32*7];
uint8_t i;
uint16_t V;
for(i = 0; i<IDN; i++){
if(Position[i]<0){
Position[i] = -Position[i];
Position[i] |= (1<<15);
}
if(Speed){
V = Speed[i];
}els
- 1
- 2
前往页