100 lines
2.1 KiB
C
100 lines
2.1 KiB
C
#pragma once
|
|
|
|
#include <pico/stdio.h>
|
|
#include "crc.h"
|
|
#include "stdio.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,
|
|
};
|
|
|
|
static inline void write_raw(void* data, size_t n) {
|
|
const uint8_t* a = data;
|
|
for(size_t i = 0; i < n; i++){
|
|
putchar_raw(a[i]);
|
|
}
|
|
}
|
|
|
|
static inline void send_tRespState(tRespState *resp) {
|
|
resp->crc = crcFooter(*resp);
|
|
write_raw(resp, sizeof(tRespState));
|
|
fflush(stdout);
|
|
}
|
|
|
|
static inline void send_tRespEndCalib(tRespEndCalib *state) {
|
|
state->crc = crcFooter(*state);
|
|
write_raw(state, sizeof(tRespEndCalib));
|
|
fflush(stdout);
|
|
}
|
|
|
|
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;
|
|
} |