Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ported over odometry and control using new serial communication

Browse Source
This commit is contained in:
Pimpest
2026-02-08 00:49:45 +01:00
parent 05e7398731
commit fc5fecdfc1
20 changed files with 1213 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
firmware/base/src/crc.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#include <stdint.h>
const unsigned char CRC_TABLE[256] = {
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
#define crcFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)-1)
#define crcNoFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type))
inline unsigned char _crc(const uint8_t *data, unsigned int size) {
uint8_t crc = 0;
while(size--) {
crc = CRC_TABLE[ (crc ^ *(data++)) & 0xff];
}
return crc;
}

80
firmware/base/src/main.c
View File

@@ -1,4 +1,6 @@
#include <pico/stdio.h>
#include <pico/types.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
@@ -21,12 +23,18 @@
#include "config.h"
#include "stepper.h"
#include "msgs.h"
#include "crc.h"
static substep_state_t state_l;
static substep_state_t state_r;
static double base_x = 0;
static double base_y = 0;
static double base_theta = 0;
static double base_vl = 0;
static double base_vr = 0;
static double wheel_separation = WHEEL_SEPARATION;
static double wheel_ratio_l = 1;
static double wheel_ratio_r = 1;
@@ -80,6 +88,10 @@ bool update_pos_cb() {
vel_l /=64 * ENCODER_CPR;
vel_r /=64 * ENCODER_CPR;
base_vl = vel_l;
base_vr = vel_r;
vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
@@ -142,45 +154,60 @@ void core2_entry()
prev_time = time_us_32();
uint16_t cmd = 0;
uint64_t data = 0;
uint readNum = 0;
char type = 'w';
uint8_t cmd_id = 0;
tMsgBuf data;
uint16_t cmd_timeout = 100;
while (true) {
uint ch;
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
cmd_timeout = 100;
cmd = (cmd << 8) | ch;
if(readNum > 0) {
data = (data >> 8ll) | ((uint64_t)ch<<56ll);
readNum--;
if(!readNum) {
if(type == 'w')
wheel_separation = *((double*)&data);
else {
wheel_ratio_l = 1 + (*((double *)&data));
wheel_ratio_r = 1 - (*((double *)&data));
if(cmd_id > 0) {
if(push_tMsgBuf(&data, readNum++, ch,cmd_id)) {
if(cmd_id == TMSG_SET_WIDTH && crcFooter(data.msg_width) == data.msg_width.crc){
wheel_separation = data.msg_width.width;
} else if (cmd_id == TMSG_SET_RATIO && crcFooter(data.msg_ratio) == data.msg_width.crc) {
wheel_ratio_l = 1 + data.msg_ratio.ratio;
}
cmd_id = 0;
}
} else {
if(cmd == TCMD_ID(TMSG_REQ_STATE)) {
tRespState resp = {
base_x,
base_y,
base_theta,
base_vl,
base_vr,
};
send_tRespState(&resp);
} else if(cmd == TCMD_ID(TMSG_SET_RATIO)) {
readNum = 0;
cmd_id = TMSG_SET_RATIO;
} else if(cmd == TCMD_ID(TMSG_SET_WIDTH)){
readNum = 0;
cmd_id = TMSG_SET_WIDTH;
} else if(cmd == TCMD_ID(TMSG_ZERO)) {
zero();
} else if(cmd == TCMD_ID(TMSG_END_CALIB)) {
end_calib();
tRespEndCalib resp = {
calib_enc.left_gain,
calib_enc.right_gain
};
send_tRespEndCalib(&resp);
}
}
if(cmd == (((uint16_t)'g' << 8) | ';')) {
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
cmd = 0;
} else if(cmd == (((uint16_t)'w' << 8) | ';')) {
readNum = 8;
type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'r' << 8) | ';')){
readNum = 8;
type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero();
} else if(cmd == (((uint16_t)'c' << 8) | ';')) {
end_calib();
printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
}
}
update_pos_cb();
if(!cmd_timeout--) {
cmd_id = 0;
}
sleep_ms(1);
}
}
@@ -247,7 +274,6 @@ int main()
while (true) {
if((c = stdio_getchar_timeout_us(0) ) != PICO_ERROR_TIMEOUT) {
stepper_fifo(c);
stdio_putchar(c);
}
sem_acquire_blocking(&stdio_dual_usb_mutex);
tud_task();

92
firmware/base/src/msgs.h Normal file
View File

@@ -0,0 +1,92 @@
#pragma once
#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,
};
inline void send_tRespState(tRespState *resp) {
resp->crc = crcFooter(*resp);
fwrite(resp, 1, sizeof(tRespState), stdout);
fflush(stdout);
}
inline void send_tRespEndCalib(tRespEndCalib *state) {
state->crc = crcFooter(*state);
fwrite(state, 1, sizeof(tRespEndCalib), stdout);
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;
}