79 lines
1.6 KiB
C
79 lines
1.6 KiB
C
#pragma once
|
|
|
|
#include "crc.h"
|
|
|
|
typedef enum tCmdId {
|
|
TMSG_REQ_STATE = 0x7F,
|
|
TMSG_SET_WIDTH,
|
|
TMSG_SET_RATIO,
|
|
TMSG_END_CALIB,
|
|
TMSG_SET_SPEED,
|
|
TMSG_ZERO,
|
|
TMSG_DELIM,
|
|
} tCmdId;
|
|
|
|
typedef struct tMsgRatio {
|
|
double ratio;
|
|
unsigned char crc;
|
|
}__attribute__((packed)) tMsgRatio;
|
|
|
|
typedef struct tMsgWidth {
|
|
double width;
|
|
unsigned char crc;
|
|
}__attribute__((packed)) tMsgWidth;
|
|
|
|
typedef struct tMsgSpeed {
|
|
double vl;
|
|
double vr;
|
|
unsigned char crc;
|
|
}__attribute__((packed)) tMsgSpeed;
|
|
|
|
typedef struct tRespState {
|
|
double x;
|
|
double y;
|
|
double theta;
|
|
double vl;
|
|
double vr;
|
|
unsigned char crc;
|
|
}__attribute__((packed)) tRespState;
|
|
|
|
typedef struct tRespEndCalib {
|
|
double left_gain;
|
|
double right_gain;
|
|
unsigned char crc;
|
|
}__attribute__((packed)) tRespEndCalib;
|
|
|
|
typedef union tMsgBuf {
|
|
tMsgRatio msg_ratio;
|
|
tMsgWidth msg_width;
|
|
tMsgSpeed msg_speed;
|
|
}__attribute__((packed)) tMsgBuf;
|
|
|
|
#define TCMD_ID(id) ((uint16_t)(id << 8 | TMSG_DELIM))
|
|
#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF)
|
|
|
|
enum {
|
|
TSTATE_IDLE,
|
|
TSTATE_BUILD_MSG,
|
|
};
|
|
|
|
inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) {
|
|
unsigned size = 0;
|
|
switch (id) {
|
|
case TMSG_SET_RATIO:
|
|
size = sizeof(tMsgRatio);
|
|
break;
|
|
case TMSG_SET_WIDTH:
|
|
size = sizeof(tMsgRatio);
|
|
break;
|
|
case TMSG_SET_SPEED:
|
|
size = sizeof(tMsgRatio);
|
|
break;
|
|
default:
|
|
return 0;
|
|
}
|
|
if(cnt < size) {
|
|
((unsigned char*)buf)[cnt] = byte;
|
|
}
|
|
return cnt == size-1;
|
|
} |