271 lines
7.0 KiB
C
271 lines
7.0 KiB
C
#include <stdio.h>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
#include "bsp/board_api.h"
|
|
#include "tusb.h"
|
|
|
|
|
|
#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
|
|
#endif
|
|
|
|
#include "pico/stdlib.h"
|
|
#include "pico/multicore.h"
|
|
#include "pico/time.h"
|
|
#include "quadrature.h"
|
|
#include "quadrature.pio.h"
|
|
|
|
#include "config.h"
|
|
#include "stepper.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 wheel_separation = WHEEL_SEPARATION;
|
|
static double wheel_ratio_l = 1;
|
|
static double wheel_ratio_r = 1;
|
|
|
|
static uint prev_time;
|
|
static int prev_position_l = 0;
|
|
static int prev_position_r = 0;
|
|
|
|
|
|
static unsigned char stepper_instr[2+sizeof(double)*2] = {};
|
|
|
|
typedef struct instr_buf_t{
|
|
char pos;
|
|
char data[2+sizeof(double) * 2];
|
|
} instr_buf_t;
|
|
|
|
instr_buf_t stepper_buf = {};
|
|
|
|
typedef struct calib_diff_t {
|
|
long long left_pulse;
|
|
long long right_pulse;
|
|
double left_gain;
|
|
double right_gain;
|
|
} calib_diff_t;
|
|
|
|
static calib_diff_t calib_enc = {
|
|
.left_gain = 0.997648,
|
|
.right_gain = 1.002333
|
|
};
|
|
|
|
bool update_pos_cb() {
|
|
|
|
substep_update(&state_l);
|
|
substep_update(&state_r);
|
|
|
|
int position_l= state_l.position;
|
|
int position_r= state_r.position;
|
|
|
|
const int diff_l = position_l - prev_position_l;
|
|
const int diff_r = position_r - prev_position_r;
|
|
|
|
calib_enc.left_pulse += diff_l;
|
|
calib_enc.right_pulse -= diff_r;
|
|
|
|
double vel_l = diff_l;
|
|
double vel_r = diff_r;
|
|
|
|
prev_position_l = state_l.position;
|
|
prev_position_r = state_r.position;
|
|
|
|
vel_l /=64 * ENCODER_CPR;
|
|
vel_r /=64 * ENCODER_CPR;
|
|
vel_l *= WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
|
|
vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
|
|
|
|
const double linear = (vel_l + vel_r) / 2;
|
|
const double angular = (vel_r - vel_l) / wheel_separation;
|
|
const double r = linear / angular;
|
|
|
|
if(fabs(r) > 100) {
|
|
const double dir = base_theta + angular * 0.5;
|
|
|
|
base_x += linear * cos(dir);
|
|
base_y += linear * sin(dir);
|
|
base_theta += angular;
|
|
}
|
|
else {
|
|
const double base_theta_old = base_theta;
|
|
base_theta += angular;
|
|
base_x += r * (sin(base_theta) - sin(base_theta_old));
|
|
base_y += -r * (cos(base_theta) - cos(base_theta_old));
|
|
}
|
|
}
|
|
|
|
void zero() {
|
|
base_x = 0;
|
|
base_y = 0;
|
|
base_theta = 0;
|
|
calib_enc.left_pulse = 0;
|
|
calib_enc.right_pulse = 0;
|
|
}
|
|
|
|
void start_calib() {
|
|
calib_enc.left_pulse = 0;
|
|
calib_enc.right_pulse = 0;
|
|
}
|
|
|
|
void end_calib() {
|
|
const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
|
|
const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
|
|
const double delta_angle = l_adjust - r_adjust;
|
|
const double distance = 0.5 * (l_adjust + r_adjust);
|
|
const double factor = delta_angle / distance * 0.7;
|
|
calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
|
|
calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
|
|
}
|
|
|
|
|
|
void core2_entry()
|
|
{
|
|
|
|
pio_add_program(pio1, &quadrature_encoder_substep_program);
|
|
substep_init_state(pio1, 0, ENCODER_LEFT_PIN_A , &state_l);
|
|
substep_init_state(pio1, 1, ENCODER_RIGHT_PIN_A, &state_r);
|
|
|
|
// The sets the positions to initial values
|
|
substep_update(&state_l);
|
|
substep_update(&state_r);
|
|
prev_position_l = state_l.position;
|
|
prev_position_r = state_r.position;
|
|
|
|
prev_time = time_us_32();
|
|
|
|
// alarm_pool_t *ap = alarm_pool_create_with_unused_hardware_alarm(2);
|
|
// repeating_timer_t rt;
|
|
// alarm_pool_add_repeating_timer_us(ap, TIMER_CYCLE_US, update_pos_cb, NULL, &rt);
|
|
|
|
uint16_t cmd = 0;
|
|
uint64_t data = 0;
|
|
uint readNum = 0;
|
|
char type = 'w';
|
|
|
|
while (true) {
|
|
uint ch;
|
|
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
|
|
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 == (((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();
|
|
sleep_ms(1);
|
|
}
|
|
}
|
|
|
|
double btod(unsigned char * input) {
|
|
uint64_t data = 0;
|
|
for (int i = 0; i<8; i++){
|
|
uint a = *(input + i);
|
|
data >>= 8;
|
|
data |= (uint64_t)a<<56;
|
|
}
|
|
const double ret = *((double*)&data);
|
|
return ret;
|
|
}
|
|
|
|
void stepper_fifo(char c, uint8_t itf) {
|
|
char instrh = stepper_buf.data[(stepper_buf.pos + 1) % sizeof(stepper_buf.data)];
|
|
char instrl = stepper_buf.data[(stepper_buf.pos + 2) % sizeof(stepper_buf.data)];
|
|
stepper_buf.data[stepper_buf.pos] = c;
|
|
stepper_buf.pos = (stepper_buf.pos + 1) % sizeof(stepper_buf.data);
|
|
|
|
if(instrh == 's' && instrl == ';') {
|
|
memcpy(stepper_instr,stepper_buf.data + stepper_buf.pos, sizeof(stepper_buf.data) - stepper_buf.pos);
|
|
memcpy(stepper_instr - stepper_buf.pos + sizeof(stepper_buf.data),stepper_buf.data, stepper_buf.pos);
|
|
|
|
memset(&stepper_buf, 0, sizeof(stepper_buf));
|
|
double vl = btod(stepper_instr + 2);
|
|
double vr = btod(stepper_instr + 10);
|
|
|
|
set_speeds(vl, vr);
|
|
|
|
// tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
|
|
// tud_cdc_n_write_flush(itf);
|
|
}
|
|
|
|
}
|
|
|
|
void tud_cdc_rx_cb(uint8_t itf) {
|
|
// read the available data
|
|
// | IMPORTANT: also do this for CDC0 because otherwise
|
|
// | you won't be able to print anymore to CDC0
|
|
// | next time this function is called
|
|
if (itf == 1) {
|
|
int c;
|
|
while((c = tud_cdc_n_read_char(itf)) > -1) {
|
|
stepper_fifo(c, itf);
|
|
}
|
|
// tud_cdc_n_write(itf, (uint8_t const *) "OK\r\n", 4);
|
|
// tud_cdc_n_write_flush(itf);
|
|
}
|
|
}
|
|
|
|
void run_init()
|
|
{
|
|
board_init();
|
|
tusb_init();
|
|
tud_init(BOARD_TUD_RHPORT);
|
|
|
|
|
|
|
|
if (board_init_after_tusb) {
|
|
board_init_after_tusb();
|
|
}
|
|
|
|
if(!stdio_usb_init()) {
|
|
board_led_write(1);
|
|
}
|
|
stepper_init();
|
|
|
|
}
|
|
|
|
|
|
int main()
|
|
{
|
|
run_init();
|
|
|
|
multicore_launch_core1(core2_entry);
|
|
|
|
|
|
|
|
while (true) {
|
|
tud_task();
|
|
sleep_ms(1);
|
|
}
|
|
}
|
|
|