159 lines
3.5 KiB
C
159 lines
3.5 KiB
C
#include <stdio.h>
|
|
#include <math.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"
|
|
|
|
//===================== CONFIG =========================
|
|
#define ENCODER_LEFT_PIN_A 14 // Lupio sam ove vrednosti
|
|
#define ENCODER_LEFT_PIN_B 15
|
|
#define ENCODER_RIGHT_PIN_A 18
|
|
#define ENCODER_RIGHT_PIN_B 19
|
|
#define ENCODER_CPR 3840
|
|
|
|
#define WHEEL_RADIUS 0.035
|
|
#define WHEEL_SEPRATATION 0.04
|
|
#define TIMER_CYCLE_US 1000
|
|
//======================================================
|
|
|
|
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 uint prev_time;
|
|
static int prev_position_l = 0;
|
|
static int prev_position_r = 0;
|
|
|
|
bool update_pos_cb() {
|
|
|
|
substep_update(&state_l);
|
|
substep_update(&state_r);
|
|
|
|
int position_l= state_l.position;
|
|
int position_r= state_r.position;
|
|
|
|
double vel_l = position_l - prev_position_l;
|
|
double vel_r = position_r - prev_position_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;
|
|
vel_r *= -WHEEL_RADIUS * 2 * M_PI;
|
|
|
|
const double linear = (vel_l + vel_r) / 2;
|
|
const double angular = (vel_r - vel_l) / WHEEL_SEPRATATION;
|
|
|
|
if(fabs(angular) < 1e-6) {
|
|
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;
|
|
const double r = linear / angular;
|
|
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;
|
|
}
|
|
|
|
|
|
void core2_entry()
|
|
{
|
|
|
|
pio_add_program(pio0, &quadrature_encoder_substep_program);
|
|
substep_init_state(pio0, 0, ENCODER_LEFT_PIN_A , &state_l);
|
|
substep_init_state(pio0, 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;
|
|
|
|
while (true) {
|
|
int ch;
|
|
if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
|
|
cmd = (cmd << 8) | ch;
|
|
|
|
if(cmd == (((uint16_t)'g' << 8) | ';')) {
|
|
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
|
|
cmd = 0;
|
|
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
|
|
zero();
|
|
}
|
|
}
|
|
|
|
update_pos_cb();
|
|
sleep_ms(1);
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
int main()
|
|
{
|
|
run_init();
|
|
|
|
multicore_launch_core1(core2_entry);
|
|
|
|
|
|
|
|
while (true) {
|
|
tud_task();
|
|
sleep_ms(1);
|
|
}
|
|
}
|