ulysse
/
main_robot_2025-2026
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working motion control

This commit is contained in:
Ulysse Cura 2026-02-24 21:33:29 +01:00
parent 2733c03cfa
commit 8232638fc5
17 changed files with 234 additions and 329 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
program/main_controller/CMakeLists.txt
View File

@ -17,9 +17,9 @@ pico_sdk_init()
add_executable(main_controller
src/main.c
src/robot.c
src/i2c/motors.c
src/i2c/i2c_master.c
src/motors.c
src/motion_control.c
src/i2c/i2c_master.c
src/i2c/gyro.c
src/i2c/mcp23017.c
src/wifi/wifi_operator.c

13
program/main_controller/src/headers/motion_control.h
View File

@ -2,21 +2,16 @@
#define MOTION_CONTROL_H
#include <stdint.h>
#include "i2c/headers/motors.h"
#define I2C_MOTION_CONTROLLER_ADDRESS 0x09
typedef struct motion_control_data_t {
int16_t angle;
int8_t x_axis_speed;
int8_t y_axis_speed;
int16_t target_speed;
float target_angle;
} motion_control_data_t;
// Init values for motion control
void motion_control_init(void);
// Update motion control buffer from motion control data and gyro data
void i2c_update_motion_control(void);
// Update servo motors from motion control data
void i2c_update_servo_motors(void);
void motion_control_update(void);
#endif // MOTION_CONTROL_H

15
program/main_controller/src/headers/motors.h Normal file
View File

@ -0,0 +1,15 @@
#ifndef MOTORS_H
#define MOTORS_H
#include <stdint.h>
typedef enum motors_t {
MOTOR1,
MOTOR2,
NB_MOTORS
} motors_t;
void motor_set_dir(motors_t motor, int16_t value);
#endif // MOTORS_H

49
program/main_controller/src/i2c/headers/motors.h
View File

@ -1,49 +0,0 @@
#ifndef MOTORS_H
#define MOTORS_H
#include <pico/types.h>
// Motors
typedef enum motors_enum_t {
MOTOR1,
MOTOR2,
MOTOR3,
MOTOR4,
NB_MOTORS
} motors_enum_t;
typedef struct motor_def_t {
uint pwm_pin;
uint dir1_pin;
uint dir2_pin;
uint8_t buffer_reg;
} motor_def_t;
extern const motor_def_t MOTORS_DEFS[];
// Servo Motors
typedef enum {
SERVO_MOTOR1,
SERVO_MOTOR2,
SERVO_MOTOR3,
SERVO_MOTOR4,
NB_SERVO_MOTORS
} servo_motors_enum_t;
typedef struct {
uint pwm_pin;
uint open_pos;
uint close_pos;
uint8_t buffer_reg_and_payload_byte;
} servo_motor_def_t;
extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
// Send [motor] to [value] through i2c to motion controller
void i2c_set_motor(motors_enum_t motor, int8_t value);
// Set [servo motor] to [value] through i2c
void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value);
#endif // MOTORS_H

35
program/main_controller/src/i2c/motors.c
View File

@ -1,35 +0,0 @@
#include "headers/motors.h"
#include <stdlib.h>
#include "headers/i2c_master.h"
#include "headers/motion_control.h"
const motor_def_t MOTORS_DEFS[] = {
{0, 4, 5, 0x00},
{1, 6, 7, 0x01},
{2, 8, 9, 0x02},
{3, 10, 11, 0x03},
};
const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
{12, 0, 25000, 0x04},
{13, 0, 25000, 0x05},
{14, 0, 25000, 0x06},
{15, 0, 25000, 0x07},
};
void i2c_set_motor(motors_enum_t motor, int8_t value)
{
const motor_def_t *MOTOR_DEF = &MOTORS_DEFS[motor];
uint8_t buf[] = {MOTOR_DEF->buffer_reg, *(uint8_t *)&value};
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, buf, 2, false);
}
void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value)
{
const servo_motor_def_t *SERVO_MOTOR_DEF = &SERVO_MOTORS_DEFS[servo_motor];
uint8_t buf[] = {SERVO_MOTOR_DEF->buffer_reg_and_payload_byte, *(uint8_t *)&value};
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, buf, 2, false);
}

120
program/main_controller/src/motion_control.c
View File

@ -2,66 +2,100 @@
#include <stdio.h>
#include <math.h>
#include "i2c/headers/motors.h"
#include <hardware/i2c.h>
#include "i2c/headers/i2c_master.h"
#include "headers/motors.h"
#include "headers/robot.h"
#define ANGLE_PER_S 30.0f
#define GAIN_KD 10
#define MSG_LEN sizeof(uint8_t) * 2
#define MSG_DELAY_MS 5.0f
void motion_control_init(void)
{
robot.motion_control_data.angle = 0;
robot.motion_control_data.x_axis_speed = 0;
robot.motion_control_data.y_axis_speed = 0;
robot.motion_control_data.target_speed = 0;
robot.motion_control_data.target_angle = 0.0f;
}
void i2c_update_motion_control(void)
void motion_control_update(void)
{
// Motion control work as follow :
// - Motors are rotated on-board at 45*.
// - we calculate the error of the targeted angle and the actual angle
// - First we estimate the targeted speed on irl board on X and Y axis
// - Then we calculate motors speed from targeted speed and the error
// - And we put limits because motors speed are send by i2c on 1 byte
// First we update motion control data from received data
robot.motion_control_data.target_angle += (float)robot.udp_client.data.hard.inputs.joystick_y / 256.0f * robot.delta_time_ms / 1000.0f * ANGLE_PER_S;
robot.motion_control_data.target_speed = robot.udp_client.data.hard.inputs.joystick_x;
float actual_angle = robot.gyro_data.x_angle - 45.0f;
// Go to nearest 90* angle if L button pressed
if(robot.udp_client.data.hard.inputs.buttons.button_l)
{
float angle_offset = fmodf(robot.motion_control_data.target_angle, 90.0f);
float target_angle = (float)robot.motion_control_data.angle - 45.0f;
float target_angle_radian = target_angle / 180.0f * M_PI;
if(fabsf(angle_offset) < 45.0f)
{
robot.motion_control_data.target_angle -= angle_offset;
}
else
{
robot.motion_control_data.target_angle += (90.0f - abs(angle_offset)) * ((angle_offset > 0.0f) - (angle_offset < 0.0f));
}
}
float error = target_angle - actual_angle;
if(error > 180) error -= 360;
if(error < -180) error += 360;
// Wrap angle
while(robot.motion_control_data.target_angle > 180.0f) robot.motion_control_data.target_angle -= 360.0f;
while(robot.motion_control_data.target_angle < -180.0f) robot.motion_control_data.target_angle += 360.0f;
//printf(">target_angle:%f\n", robot.motion_control_data.target_angle);
//printf(">target_speed:%d\n", robot.motion_control_data.target_speed);
float target_angle_radian = robot.motion_control_data.target_angle / 180.0f * M_PI;
float error = robot.motion_control_data.target_angle - robot.gyro_data.x_angle;
while(error > 180) error -= 360;
while(error < -180) error += 360;
float correction = error * GAIN_KD;
float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +
sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed -
sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed;
float motor1_speed = (float)robot.motion_control_data.target_speed - correction;
float motor2_speed = (float)robot.motion_control_data.target_speed + correction;
int motor1_speed = target_x_axis_speed + (int)correction;
int motor2_speed = target_x_axis_speed - (int)correction;
int motor3_speed = target_y_axis_speed + (int)correction;
int motor4_speed = target_y_axis_speed - (int)correction;
if(motor1_speed > 255) motor1_speed = 255;
if(motor2_speed > 255) motor2_speed = 255;
if(motor1_speed > 127) motor1_speed = 127;
if(motor2_speed > 127) motor2_speed = 127;
if(motor3_speed > 127) motor3_speed = 127;
if(motor4_speed > 127) motor4_speed = 127;
if(motor1_speed < -255) motor1_speed = -255;
if(motor2_speed < -255) motor2_speed = -255;
if(motor1_speed < -128) motor1_speed = -128;
if(motor2_speed < -128) motor2_speed = -128;
if(motor3_speed < -128) motor3_speed = -128;
if(motor4_speed < -128) motor4_speed = -128;
// Set dir pins
motor_set_dir(MOTOR1, (int16_t)motor1_speed);
motor_set_dir(MOTOR2, (int16_t)motor2_speed);
i2c_set_motor(MOTOR1, motor1_speed);
i2c_set_motor(MOTOR2, motor2_speed);
i2c_set_motor(MOTOR3, motor3_speed);
i2c_set_motor(MOTOR4, motor4_speed);
}
void i2c_update_servo_motors(void)
{
//for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
// i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]);
static float elapsed_time_ms = 0.0f;
elapsed_time_ms += robot.delta_time_ms;
if(elapsed_time_ms >= MSG_DELAY_MS)
{
// Send motors speed via I2C
union {
struct {
uint8_t motor1_speed;
uint8_t motor2_speed;
} hard;
uint8_t raw[MSG_LEN];
} data;
data.hard.motor1_speed = (uint8_t)abs((int)motor1_speed);
data.hard.motor2_speed = (uint8_t)abs((int)motor2_speed);
uint8_t reg = 0x00;
if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, &reg, 1, true) == PICO_ERROR_GENERIC)
{
puts("Motion controller not reachable");
}
if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data.raw, MSG_LEN, false) == PICO_ERROR_GENERIC)
{
puts("Motion controller not reachable");
}
}
}

47
program/main_controller/src/motors.c Normal file
View File

@ -0,0 +1,47 @@
#include "headers/motors.h"
#include <pico/types.h>
#include "i2c/headers/mcp23017.h"
#define MOTOR1_DIR1_MCP23017_GPIO 12
#define MOTOR1_DIR2_MCP23017_GPIO 13
#define MOTOR2_DIR1_MCP23017_GPIO 14
#define MOTOR2_DIR2_MCP23017_GPIO 15
void motor_set_dir(motors_t motor, int16_t value)
{
uint gpio_dir1_pin;
uint gpio_dir2_pin;
switch(motor)
{
case MOTOR1:
gpio_dir1_pin = MOTOR1_DIR1_MCP23017_GPIO;
gpio_dir2_pin = MOTOR1_DIR2_MCP23017_GPIO;
break;
case MOTOR2:
gpio_dir1_pin = MOTOR2_DIR1_MCP23017_GPIO;
gpio_dir2_pin = MOTOR2_DIR2_MCP23017_GPIO;
break;
default:
break;
}
if(value > 0)
{
mcp23017_gpio_put(gpio_dir1_pin, false);
mcp23017_gpio_put(gpio_dir2_pin, true);
}
else if(value < 0)
{
mcp23017_gpio_put(gpio_dir1_pin, true);
mcp23017_gpio_put(gpio_dir2_pin, false);
}
else
{
mcp23017_gpio_put(gpio_dir1_pin, false);
mcp23017_gpio_put(gpio_dir2_pin, false);
}
}

10
program/main_controller/src/robot.c
View File

@ -28,14 +28,14 @@ void robot_init(void)
gyro_calibrate();
//motion_control_init();
/*
motion_control_init();
if(wifi_operator_init())
robot.is_running = false;
if(udp_client_init())
robot.is_running = false;
*/
// Initialisation ended
for(uint8_t i = 0, led_state = true; i < 5; i++)
{
@ -75,9 +75,7 @@ void robot_handle_inputs_outputs(void)
gyro_update();
//update_motion_control();
//i2c_update_servo_motors();
motion_control_update();
mcp23017_update();

21
program/main_controller/src/wifi/headers/udp_client.h
View File

@ -6,11 +6,32 @@
#define UDP_CLIENT_PORT 4243
#define DATA_LEN (2 * sizeof(int16_t) + sizeof(uint8_t)) / sizeof(uint8_t)
typedef void (*message_callback_t)(uint8_t *payload, uint16_t len);
typedef struct udp_client_t {
struct udp_pcb *pcb;
message_callback_t message_callback;
union {
struct {
struct {
int16_t joystick_x;
int16_t joystick_y;
struct {
bool button_black : 1;
bool button_blue : 1;
bool button_white : 1;
bool button_green : 1;
bool button_l : 1;
bool button_r : 1;
} buttons;
} inputs;
} hard;
uint8_t raw[DATA_LEN];
} data;
} udp_client_t;
// Init udp client

14
program/main_controller/src/wifi/udp_client.c
View File

@ -1,11 +1,15 @@
#include "headers/udp_client.h"
#include <stdio.h>
#include <string.h>
#include "headers/robot.h"
void __not_in_flash_func(message_callback)(uint8_t *payload, uint16_t len, const ip_addr_t *addr)
static inline void message_callback(uint8_t *payload, uint16_t len)
{
// RECEIVE ALL CONTROLLER DATA
if(len != DATA_LEN)
return;
memcpy(robot.udp_client.data.raw, payload, len);
}
// Default callback func
@ -17,7 +21,7 @@ static inline void default_message_callback(uint8_t *payload, uint16_t len)
puts("\n");
}
static void __not_in_flash_func(udp_receive_callback)(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
static inline void udp_receive_callback(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
robot.udp_client.message_callback((uint8_t *)p->payload, p->len);
pbuf_free(p);
@ -25,8 +29,8 @@ static void __not_in_flash_func(udp_receive_callback)(void *arg, struct udp_pcb
int udp_client_init(void)
{
//udp_client.message_callback = udp_client_message_handler;
robot.udp_client.message_callback = default_message_callback;
robot.udp_client.message_callback = message_callback;
//robot.udp_client.message_callback = default_message_callback;
robot.udp_client.pcb = udp_new();
if(robot.udp_client.pcb == NULL)

3
program/motion_controller/CMakeLists.txt
View File

@ -6,8 +6,6 @@ project(motion_controller C CXX ASM)
set(CMAKE_C_STNDARD 11)
set(CMAKE_CXX_STANDARD 17)
set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
pico_sdk_init()
add_executable(motion_controller
@ -23,7 +21,6 @@ target_include_directories(motion_controller PRIVATE
target_link_libraries(motion_controller
pico_stdlib
hardware_uart
hardware_i2c
pico_i2c_slave
hardware_pwm

46
program/motion_controller/src/headers/motors.h
View File

@ -11,50 +11,14 @@ typedef enum motors_enum_t {
NB_MOTORS
} motors_enum_t;
typedef struct motor_def_t {
uint pwm_pin;
uint dir1_pin;
uint dir2_pin;
uint8_t buffer_reg;
} motor_def_t;
extern const motor_def_t MOTORS_DEFS[];
// Servo Motors
typedef enum {
SERVO_MOTOR1,
SERVO_MOTOR2,
SERVO_MOTOR3,
SERVO_MOTOR4,
NB_SERVO_MOTORS
} servo_motors_enum_t;
typedef struct {
uint pwm_pin;
uint open_pos;
uint close_pos;
uint8_t buffer_reg;
} servo_motor_def_t;
extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
#define MOTOR1_PWM_PIN 9
#define MOTOR2_PWM_PIN 8
// Init all motors defined in the MOTORS_DEF array
void init_motors(void);
// Init all servo motors defined in the SERVO_MOTORS_DEF array
void init_servo_motors(void);
// Set [motor] to 0
void motor_zero(motors_enum_t motor);
void motors_init(void);
// Set [motor] in motor_enum_t at [value] between -127 and 128
void motor_set(motors_enum_t motor, int8_t value);
// Set servo motor to its open pos
void servo_motor_zero(servo_motors_enum_t servo_motor);
// Set servo to its close pos if [close] else open pos
void servo_motor_set(servo_motors_enum_t servo_motor, bool close);
void motor_set_speed(motors_enum_t motor, uint8_t value);
// Update motors from the data in the i2c buffer
void update_motors_from_buffer(void);
// Update servo motors from the data in the i2c buffer
void update_servo_motors_from_buffer(void);
void motors_update(void);
#endif // MOTORS_H

0
program/motion_controller/src/headers/servo_motors.h
View File

16
program/motion_controller/src/i2c/headers/i2c_slave.h
View File

@ -1,9 +1,3 @@
/*
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
*
* SPDX-License-Identifier: MIT
*/
#ifndef I2C_SLAVE_H
#define I2C_SLAVE_H
@ -15,7 +9,15 @@
#define I2C_SLAVE_ADDRESS 0x09
typedef struct i2c_buffer_t {
uint8_t buffer[256];
union {
struct {
uint8_t motor1_speed;
uint8_t motor2_speed;
} hard;
uint8_t raw[256];
} buffer;
uint8_t buffer_reg;
bool buffer_reg_written;
} i2c_buffer_t;

10
program/motion_controller/src/i2c/i2c_slave.c
View File

@ -1,9 +1,3 @@
/*
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
*
* SPDX-License-Identifier: MIT
*/
#include "headers/i2c_slave.h"
#include <hardware/gpio.h>
@ -25,14 +19,14 @@ void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
else
{
// save into memory
robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
robot.i2c_buffer.buffer_reg++;
}
break;
case I2C_SLAVE_REQUEST: // master is requesting data
// load from memory
i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]);
i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg]);
robot.i2c_buffer.buffer_reg++;
break;

147
program/motion_controller/src/motors.c
View File

@ -4,137 +4,50 @@
#include <hardware/pwm.h>
#include <headers/robot.h>
const motor_def_t MOTORS_DEFS[] = {
{0, 4, 5, 0x00},
{1, 6, 7, 0x01},
{2, 9, 8, 0x02},
{3, 11, 10, 0x03},
};
#include <stdio.h>
const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
{12, 0, 25000, 0x04},
{13, 0, 25000, 0x05},
{14, 0, 25000, 0x06},
{15, 0, 25000, 0x07},
};
static inline uint get_motor_pwm_pin(motors_enum_t motor)
{
switch(motor)
{
case MOTOR1:
return MOTOR1_PWM_PIN;
case MOTOR2:
return MOTOR2_PWM_PIN;
default:
return -1;
}
}
// Init all motors defined in the MOTORS_DEF array
void init_motors(void)
void motors_init(void)
{
for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
for(motors_enum_t motor = MOTOR1; motor < NB_MOTORS; motor++)
{
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
// Init PWM
const uint SLICE_NUM = pwm_gpio_to_slice_num(motor_def->pwm_pin);
const uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
pwm_set_wrap(SLICE_NUM, 128);
const uint SLICE_NUM = pwm_gpio_to_slice_num(MOTOR_PWM_PIN);
gpio_set_function(MOTOR_PWM_PIN, GPIO_FUNC_PWM);
pwm_set_wrap(SLICE_NUM, 256);
pwm_set_enabled(SLICE_NUM, true);
// Init dir pins
gpio_init(motor_def->dir1_pin);
gpio_set_dir(motor_def->dir1_pin, GPIO_OUT);
gpio_init(motor_def->dir2_pin);
gpio_set_dir(motor_def->dir2_pin, GPIO_OUT);
motor_zero(actual_motor);
motor_set_speed(motor, 0);
}
}
// Init all servo motors defined in the SERVO_MOTORS_DEF array
void init_servo_motors(void)
// Set [motor] in motor_enum_t at [value] between 0 and 255
void motor_set_speed(motors_enum_t motor, uint8_t value)
{
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
// Init PWM
const uint SLICE_NUM = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
pwm_set_wrap(SLICE_NUM, 25000);
pwm_set_clkdiv(SLICE_NUM, 100);
pwm_set_enabled(SLICE_NUM, true);
servo_motor_zero(actual_servo_motor);
}
uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
}
// Set [motor] to 0
void motor_zero(motors_enum_t motor)
void motors_update(void)
{
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
// Set PWM to zero
pwm_set_gpio_level(motor_def->pwm_pin, 0);
// Set dir pins to false
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, false);
}
// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
void motor_set(motors_enum_t motor, int8_t value)
{
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
if(value < 0)
{
gpio_put(motor_def->dir1_pin, true);
gpio_put(motor_def->dir2_pin, false);
value = -value;
}
else if(value > 0)
{
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, true);
}
else
{
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, false);
}
pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value);
}
// Set servo motor to its open pos
void servo_motor_zero(servo_motors_enum_t servo_motor)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
// Set PWM to zero //
pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos);
}
// Set servo to its close pos if [close] else open pos
void servo_motor_set(servo_motors_enum_t servo_motor, bool close)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
// Set PWM to zero //
pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos);
}
void update_motors_from_buffer(void)
{
for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
{
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
motor_set(actual_motor, robot.i2c_buffer.buffer[motor_def->buffer_reg]);
}
}
void update_servo_motors_from_buffer(void)
{
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
servo_motor_set(actual_servo_motor, robot.i2c_buffer.buffer[servo_motor_def->buffer_reg]);
}
motor_set_speed(MOTOR1, robot.i2c_buffer.buffer.hard.motor1_speed);
motor_set_speed(MOTOR2, robot.i2c_buffer.buffer.hard.motor2_speed);
}

13
program/motion_controller/src/robot.c
View File

@ -17,8 +17,8 @@ void robot_init(void)
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
gpio_put(PICO_DEFAULT_LED_PIN, true);
//init_motors();
//init_servo_motors();
motors_init();
init_i2c_slave();
// Initialisation ended
@ -51,14 +51,19 @@ static inline void update_time(void)
led_state = !led_state;
}
}
void robot_handle_inputs_outputs(void)
{
update_time();
//update_motors_from_buffer();
motors_update();
//printf(">motor1_speed:%d\n", robot.i2c_buffer.buffer.hard.motor1_speed);
//printf(">motor2_speed:%d\n", robot.i2c_buffer.buffer.hard.motor2_speed);
sleep_ms(5);
tight_loop_contents();
}
void robot_deinit(void)