ulysse
/
Robot_Principal_2024-2025
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Coded motion control

This commit is contained in:
Ulysse Cura 2025-05-08 14:11:10 +02:00
parent ee8a1b9616
commit d33c9eb4f3
16 changed files with 206 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
main controller code/.vscode/settings.json vendored
View File

@ -5,6 +5,7 @@
"i2c.h": "c", "i2c.h": "c",
"stdlib.h": "c", "stdlib.h": "c",
"stdint.h": "c", "stdint.h": "c",
"gyro.h": "c" "gyro.h": "c",
"motors.h": "c"
} }
} }

52
main controller code/Readme.md
View File

@ -14,16 +14,16 @@ The robot’s I2C communication works as follows:
This code is designed to be the master in the i2c communication. This code is designed to be the master in the i2c communication.
|Adress |R/W|Description |Encoding | |Register |R/W|Description |Encoding |
|-------|:-:|-------------------------------|:-----------------:| |---------|:-:|-------------------------------|:-----------------:|
| 0x00 | W | Speed motor 1 |**-128** - **127** | | 0x00 | W | Speed motor 1 |**-128** - **127** |
| 0x01 | W | Speed motor 2 |**-128** - **127** | | 0x01 | W | Speed motor 2 |**-128** - **127** |
| 0x02 | W | Speed motor 3 |**-128** - **127** | | 0x02 | W | Speed motor 3 |**-128** - **127** |
| 0x03 | W | Speed motor 4 |**-128** - **127** | | 0x03 | W | Speed motor 4 |**-128** - **127** |
| 0x04 | W | Servo 1 position selection | **0** - **1** | | 0x04 | W | Servo 1 position selection | **0** - **1** |
| 0x05 | W | Servo 2 position selection | **0** - **1** | | 0x05 | W | Servo 2 position selection | **0** - **1** |
| 0x06 | W | Servo 3 position selection | **0** - **1** | | 0x06 | W | Servo 3 position selection | **0** - **1** |
| 0x07 | W | Servo 4 position selection | **0** - **1** | | 0x07 | W | Servo 4 position selection | **0** - **1** |
Motors communication description Motors communication description
@ -54,6 +54,26 @@ To control a servo motor you need to write data to its adress of the form :
Value is 0 or 1 for the open or the close pos. Value is 0 or 1 for the open or the close pos.
Internet communication description
-----------------------------------------------
The robot main_controller is a client connected to the wireless controller which is an udp server host. A buffer is used to store data received from host.
Speed on X and Y axis are not depending of the robot oriantation.
Servo motors keep the same regitster in i2c buffer and udp buffer.
|Register |Description |Encoding |
|---------|-------------------------------------------------|:-----------------:|
| 0x00-01 | Robot angle (0x00 is the last significant byte) | **0** - **360** |
| 0x02 | Speed x axis |**-128** - **127** |
| 0x03 | Speed y axis |**-128** - **127** |
| 0x04 | Servo 1 position selection | **0** - **1** |
| 0x05 | Servo 2 position selection | **0** - **1** |
| 0x06 | Servo 3 position selection | **0** - **1** |
| 0x07 | Servo 4 position selection | **0** - **1** |
Pinout description Pinout description
----------------------------------------------- -----------------------------------------------
@ -61,3 +81,15 @@ Pinout description
|----|----------------------------------|-----------| |----|----------------------------------|-----------|
| 4 | I2C Bus SDA | I2C | | 4 | I2C Bus SDA | I2C |
| 5 | I2C Bus SCL | I2C | | 5 | I2C Bus SCL | I2C |
Motors placement
-----------------------------------------------
,-~***~-,
/1 4\
| |
| |
| |
\3 2/
`-.....-'

25
main controller code/src/gyro.c
View File

@ -11,10 +11,9 @@
* https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c * * https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ \* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#define SAMPLE_MIN_ELAPSED_TIME 2 // ms
#define DPS_PER_DIGIT 0.00875f #define DPS_PER_DIGIT 0.00875f
#define SAMPLE_MIN_ELAPSED_TIME 10 // ms
int init_gyro(void) int init_gyro(void)
{ {
// Verify gyro initialisation // Verify gyro initialisation
@ -56,7 +55,7 @@ static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t
void gyro_calibrate(void) void gyro_calibrate(void)
{ {
const uint nb_samples = 1000; const uint nb_samples = 10000;
int16_t x, y, z; int16_t x, y, z;
int32_t x_sum = 0, y_sum = 0, z_sum = 0; int32_t x_sum = 0, y_sum = 0, z_sum = 0;
@ -70,18 +69,19 @@ void gyro_calibrate(void)
z_sum += z; z_sum += z;
sleep_ms(SAMPLE_MIN_ELAPSED_TIME); sleep_ms(SAMPLE_MIN_ELAPSED_TIME);
//printf(">cal_x:%d\n", x);
//printf(">cal_y:%d\n", y);
//printf(">cal_z:%d\n", z);
} }
robot.gyro_data.x_offset = (float)x_sum / (float)nb_samples * DPS_PER_DIGIT; robot.gyro_data.x_offset = (float)x_sum / (float)nb_samples * DPS_PER_DIGIT;
robot.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT; robot.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT;
robot.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT; robot.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT;
//robot.gyro_data.x_offset = 0.0;
//robot.gyro_data.y_offset = 0.0;
//robot.gyro_data.z_offset = 0.0;
//printf("x_cal:%f\n", robot.gyro_data.x_offset); printf("\nx_cal:%.5f\n", robot.gyro_data.x_offset);
//printf("y_cal:%f\n", robot.gyro_data.y_offset); printf("\ny_cal:%.5f\n", robot.gyro_data.y_offset);
//printf("z_cal:%f\n", robot.gyro_data.z_offset); printf("\nz_cal:%.5f\n", robot.gyro_data.z_offset);
} }
static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps) static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps)
@ -113,6 +113,13 @@ void gyro_update(void)
robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms / 1000.0f; robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms / 1000.0f;
robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms / 1000.0f; robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms / 1000.0f;
while(robot.gyro_data.x_angle > 180) robot.gyro_data.x_angle -= 360;
while(robot.gyro_data.x_angle < -180) robot.gyro_data.x_angle += 360;
while(robot.gyro_data.y_angle > 180) robot.gyro_data.y_angle -= 360;
while(robot.gyro_data.y_angle < -180) robot.gyro_data.y_angle += 360;
while(robot.gyro_data.z_angle > 180) robot.gyro_data.z_angle -= 360;
while(robot.gyro_data.z_angle < -180) robot.gyro_data.z_angle += 360;
printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle); printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle);
printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle); printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle);
printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle); printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle);

5
main controller code/src/i2c_master.c
View File

@ -14,6 +14,11 @@ void i2c_master_init(void)
i2c_init(I2C_MASTER_INSTANCE, I2C_MASTER_BAUD_RATE); i2c_init(I2C_MASTER_INSTANCE, I2C_MASTER_BAUD_RATE);
} }
void i2c_master_deinit(void)
{
i2c_deinit(I2C_MASTER_INSTANCE);
}
inline void i2c_master_write(uint8_t address, const uint8_t *src, size_t len) inline void i2c_master_write(uint8_t address, const uint8_t *src, size_t len)
{ {
i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false); i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false);

2
main controller code/src/include/i2c_master.h
View File

@ -10,6 +10,8 @@
// Init master i2c // Init master i2c
void i2c_master_init(void); void i2c_master_init(void);
// Deinit master i2c
void i2c_master_deinit(void);
// Send [src] of [len] to [address] and close communication // Send [src] of [len] to [address] and close communication
void i2c_master_write(uint8_t address, const uint8_t *src, size_t len); void i2c_master_write(uint8_t address, const uint8_t *src, size_t len);
// Receive [dst] of [len] from [address] and close communication // Receive [dst] of [len] from [address] and close communication

15
main controller code/src/include/motion_control.h
View File

@ -0,0 +1,15 @@
#ifndef MOTION_CONTROL_H
#define MOTION_CONTROL_H
#include <stdint.h>
typedef struct motion_control_data_t {
int16_t angle;
int8_t x_axis_speed;
int8_t y_axis_speed;
} motion_control_data_t;
// Update motion control buffer from udp buffer and gyro data
void i2c_update_motion_control(void);
#endif // MOTION_CONTROL_H

8
main controller code/src/include/motors.h
View File

@ -19,7 +19,7 @@ typedef struct motor_def_t {
uint pwm_pin; uint pwm_pin;
uint dir1_pin; uint dir1_pin;
uint dir2_pin; uint dir2_pin;
uint8_t buffer_address; uint8_t buffer_reg;
} motor_def_t; } motor_def_t;
extern const motor_def_t MOTORS_DEFS[]; extern const motor_def_t MOTORS_DEFS[];
@ -38,7 +38,7 @@ typedef struct {
uint pwm_pin; uint pwm_pin;
uint open_pos; uint open_pos;
uint close_pos; uint close_pos;
uint8_t buffer_address; uint8_t buffer_reg;
} servo_motor_def_t; } servo_motor_def_t;
extern const servo_motor_def_t SERVO_MOTORS_DEFS[]; extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
@ -47,5 +47,9 @@ extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
void i2c_set_motor_speed(motors_enum_t motor, int8_t value); void i2c_set_motor_speed(motors_enum_t motor, int8_t value);
// Get [motor] speed from motion controller through i2c // Get [motor] speed from motion controller through i2c
int8_t i2c_get_motor_speed(motors_enum_t motor); int8_t i2c_get_motor_speed(motors_enum_t motor);
// Set [servo motor] to [value] through i2c
void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value);
// Get [servo motor] value from i2c
uint8_t i2c_get_servo_motor(servo_motors_enum_t servo_motor);
#endif // MOTORS_H #endif // MOTORS_H

4
main controller code/src/include/robot.h
View File

@ -3,9 +3,13 @@
#include <stdbool.h> #include <stdbool.h>
#include "gyro.h" #include "gyro.h"
#include "udp_buffer.h"
#include "motion_control.h"
typedef struct robot_t { typedef struct robot_t {
gyro_data_t gyro_data; gyro_data_t gyro_data;
udp_buffer_t udp_buffer;
motion_control_data_t motion_control_data;
bool is_running; bool is_running;
double delta_time_ms; double delta_time_ms;

16
main controller code/src/include/udp_buffer.h
View File

@ -1,4 +1,20 @@
#ifndef UDP_BUFFER_H #ifndef UDP_BUFFER_H
#define UDP_BUFFER_H #define UDP_BUFFER_H
#include <stdint.h>
#define UDP_BUFFER_ANGLE_L_REG 0x00
#define UDP_BUFFER_ANGLE_H_REG 0x01
#define UDP_BUFFER_X_AXIS_SPEED_REG 0x02
#define UDP_BUFFER_Y_AXIS_SPEED_REG 0x03
typedef struct udp_buffer_t {
uint8_t buffer[256];
} udp_buffer_t;
// Update motion control data from buffer
void update_motion_control_data(void);
// Update servo motors from data in udp buffer
void i2c_update_servo_motors(void);
#endif // UDP_BUFFER_H #endif // UDP_BUFFER_H

31
main controller code/src/motion_control.c
View File

@ -0,0 +1,31 @@
#include "include/motion_control.h"
#include "include/motors.h"
#include "include/robot.h"
#define GAIN_KD 1
void i2c_update_motion_control(void)
{
// Motion control work as follow :
// - motors are rotated on-board at 45*.
// - we prcess them by paire of two
// - we calculate the error of the targeted angle and the actual angle
// - then we calculate motors speed from targeted speed, the error and the offset
float actual_angle = robot.gyro_data.x_angle;
float target_angle = (float)robot.motion_control_data.angle;
float error = target_angle - actual_angle;
float correction = error * GAIN_KD;
int8_t motor1_speed = robot.motion_control_data.x_axis_speed + correction;
int8_t motor2_speed = robot.motion_control_data.x_axis_speed - correction;
int8_t motor3_speed = robot.motion_control_data.y_axis_speed + correction;
int8_t motor4_speed = robot.motion_control_data.y_axis_speed - correction;
i2c_set_motor_speed(MOTOR1, motor1_speed);
i2c_set_motor_speed(MOTOR2, motor2_speed);
i2c_set_motor_speed(MOTOR3, motor3_speed);
i2c_set_motor_speed(MOTOR4, motor4_speed);
}

25
main controller code/src/motors.c
View File

@ -22,7 +22,7 @@ void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
const motor_def_t *motor_def = &MOTORS_DEFS[motor]; const motor_def_t *motor_def = &MOTORS_DEFS[motor];
uint8_t x = *(uint8_t *)&value; uint8_t x = *(uint8_t *)&value;
uint8_t data[] = {motor_def->buffer_address, x}; uint8_t data[] = {motor_def->buffer_reg, x};
i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2); i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
} }
@ -31,10 +31,29 @@ int8_t i2c_get_motor_speed(motors_enum_t motor)
{ {
const motor_def_t *motor_def = &MOTORS_DEFS[motor]; const motor_def_t *motor_def = &MOTORS_DEFS[motor];
uint8_t reg = motor_def->buffer_address; // Register of the i2c_buffer to read
uint8_t data; uint8_t data;
i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, reg, &data, 1); i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, motor_def->buffer_reg, &data, 1);
int8_t value = *(int8_t *)&data; int8_t value = *(int8_t *)&data;
return value; return value;
} }
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 x = *(uint8_t *)&value;
uint8_t data[] = {servo_motor_def->buffer_reg, x};
i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
}
uint8_t i2c_get_servo_motor(servo_motors_enum_t servo_motor)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
uint8_t value;
i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, servo_motor_def->buffer_reg, &value, 1);
return value;
}

15
main controller code/src/robot.c
View File

@ -3,14 +3,11 @@
#include <pico/stdlib.h> #include <pico/stdlib.h>
#include <time.h> #include <time.h>
#include "include/i2c_master.h" #include "include/i2c_master.h"
#include "include/gyro.h"
int robot_init(void) int robot_init(void)
{ {
stdio_init_all(); stdio_init_all();
sleep_ms(5000);
i2c_master_init(); i2c_master_init();
if(init_gyro()) return -1; if(init_gyro()) return -1;
@ -24,9 +21,9 @@ int robot_init(void)
static inline void update_time(void) static inline void update_time(void)
{ {
static clock_t last_clock_state = 0; static clock_t last_clock_state = 0;
clock_t start_clock = clock(); clock_t start_clock_state = clock();
robot.delta_time_ms = (double)(start_clock - last_clock_state) * 1000.0 / CLOCKS_PER_SEC; robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
last_clock_state = start_clock; last_clock_state = start_clock_state;
} }
void robot_handle_inputs_outputs(void) void robot_handle_inputs_outputs(void)
@ -34,8 +31,14 @@ void robot_handle_inputs_outputs(void)
update_time(); update_time();
gyro_update(); gyro_update();
update_motion_control_data();
i2c_update_motion_control();
i2c_update_servo_motors();
} }
void robot_deinit(void) void robot_deinit(void)
{ {
i2c_master_deinit();
} }

23
main controller code/src/udp_buffer.c
View File

@ -1 +1,22 @@
#include "include/udp_buffer.h" #include "include/udp_buffer.h"
#include "include/motors.h"
#include "include/robot.h"
void update_motion_control_data(void)
{
robot.motion_control_data.angle = ((robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_H_REG] << 8) | robot.udp_buffer.buffer[UDP_BUFFER_ANGLE_L_REG]);
robot.motion_control_data.x_axis_speed = robot.udp_buffer.buffer[UDP_BUFFER_X_AXIS_SPEED_REG];
robot.motion_control_data.y_axis_speed = robot.udp_buffer.buffer[UDP_BUFFER_Y_AXIS_SPEED_REG];
}
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++)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
i2c_set_servo_motor(actual_servo_motor, robot.udp_buffer.buffer[servo_motor_def->buffer_reg]);
}
}

20
motion controller code/src/i2c_buffer.c
View File

@ -8,28 +8,28 @@ void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event)
switch(event) switch(event)
{ {
case I2C_SLAVE_RECEIVE: // master has written some data case I2C_SLAVE_RECEIVE: // master has written some data
if(!robot.i2c_buffer.buffer_address_written) if(!robot.i2c_buffer.buffer_reg_written)
{ {
// writes always start with the memory address // writes always start with the memory address
robot.i2c_buffer.buffer_address = i2c_slave_read_byte(); robot.i2c_buffer.buffer_reg = i2c_slave_read_byte();
robot.i2c_buffer.buffer_address_written = true; robot.i2c_buffer.buffer_reg_written = true;
} }
else else
{ {
// save into memory // save into memory
robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address] = i2c_slave_read_byte(); robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg] = i2c_slave_read_byte();
robot.i2c_buffer.buffer_address++; robot.i2c_buffer.buffer_reg++;
} }
break; break;
case I2C_SLAVE_REQUEST: // master is requesting data case I2C_SLAVE_REQUEST: // master is requesting data
// load from memory // load from memory
i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address]); i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]);
robot.i2c_buffer.buffer_address++; robot.i2c_buffer.buffer_reg++;
break; break;
case I2C_SLAVE_FINISH: // master has signalled Stop / Restart case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
robot.i2c_buffer.buffer_address_written = false; robot.i2c_buffer.buffer_reg_written = false;
break; break;
default: default:
@ -43,7 +43,7 @@ void update_motors_from_buffer(void)
{ {
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor]; const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
motor_set(actual_motor, robot.i2c_buffer.buffer[motor_def->buffer_address]); motor_set(actual_motor, robot.i2c_buffer.buffer[motor_def->buffer_reg]);
} }
} }
@ -53,6 +53,6 @@ void update_servo_motors_from_buffer(void)
{ {
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[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_address]); servo_motor_set(actual_servo_motor, robot.i2c_buffer.buffer[servo_motor_def->buffer_reg]);
} }
} }

4
motion controller code/src/include/i2c_buffer.h
View File

@ -6,8 +6,8 @@
typedef struct i2c_buffer_t { typedef struct i2c_buffer_t {
uint8_t buffer[256]; uint8_t buffer[256];
uint8_t buffer_address; uint8_t buffer_reg;
bool buffer_address_written; bool buffer_reg_written;
} i2c_buffer_t; } i2c_buffer_t;
// I2c slave buffer handler for writing and reading data to the buffer // I2c slave buffer handler for writing and reading data to the buffer

4
motion controller code/src/include/motors.h
View File

@ -17,7 +17,7 @@ typedef struct motor_def_t {
uint pwm_pin; uint pwm_pin;
uint dir1_pin; uint dir1_pin;
uint dir2_pin; uint dir2_pin;
uint8_t buffer_address; uint8_t buffer_reg;
} motor_def_t; } motor_def_t;
extern const motor_def_t MOTORS_DEFS[]; extern const motor_def_t MOTORS_DEFS[];
@ -36,7 +36,7 @@ typedef struct {
uint pwm_pin; uint pwm_pin;
uint open_pos; uint open_pos;
uint close_pos; uint close_pos;
uint8_t buffer_address; uint8_t buffer_reg;
} servo_motor_def_t; } servo_motor_def_t;
extern const servo_motor_def_t SERVO_MOTORS_DEFS[]; extern const servo_motor_def_t SERVO_MOTORS_DEFS[];