47 lines
860 B
C
47 lines
860 B
C
#include "include/robot.h"
|
|
|
|
#include <pico/stdlib.h>
|
|
#include <time.h>
|
|
#include "include/i2c_master.h"
|
|
#include "include/gyro.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
int robot_init(void)
|
|
{
|
|
stdio_init_all();
|
|
|
|
sleep_ms(5000);
|
|
|
|
i2c_master_init();
|
|
|
|
if(init_gyro()) return -1;
|
|
gyro_calibrate();
|
|
|
|
robot.is_running = true;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static inline void update_time(void)
|
|
{
|
|
static clock_t last_clock_state = 0;
|
|
clock_t start_clock = clock();
|
|
robot.delta_time_ms = (double)(start_clock - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
|
|
last_clock_state = start_clock;
|
|
}
|
|
|
|
void robot_handle_inputs_outputs(void)
|
|
{
|
|
update_time();
|
|
|
|
gyro_update();
|
|
|
|
printf(">gyro_x:%f\n", robot.gyro_data.x_angle);
|
|
printf(">gyro_y:%f\n", robot.gyro_data.y_angle);
|
|
printf(">gyro_z:%f\n", robot.gyro_data.z_angle);
|
|
}
|
|
|
|
void robot_deinit(void)
|
|
{
|
|
} |