diff --git a/main controller code/.vscode/settings.json b/main controller code/.vscode/settings.json index b6495bb..cc4668b 100644 --- a/main controller code/.vscode/settings.json +++ b/main controller code/.vscode/settings.json @@ -5,6 +5,7 @@ "i2c.h": "c", "stdlib.h": "c", "stdint.h": "c", - "gyro.h": "c" + "gyro.h": "c", + "motors.h": "c" } } \ No newline at end of file diff --git a/main controller code/Readme.md b/main controller code/Readme.md index 9971772..c73ec31 100644 --- a/main controller code/Readme.md +++ b/main controller code/Readme.md @@ -14,16 +14,16 @@ The robot’s I2C communication works as follows: This code is designed to be the master in the i2c communication. -|Adress |R/W|Description |Encoding | -|-------|:-:|-------------------------------|:-----------------:| -| 0x00 | W | Speed motor 1 |**-128** - **127** | -| 0x01 | W | Speed motor 2 |**-128** - **127** | -| 0x02 | W | Speed motor 3 |**-128** - **127** | -| 0x03 | W | Speed motor 4 |**-128** - **127** | -| 0x04 | W | Servo 1 position selection | **0** - **1** | -| 0x05 | W | Servo 2 position selection | **0** - **1** | -| 0x06 | W | Servo 3 position selection | **0** - **1** | -| 0x07 | W | Servo 4 position selection | **0** - **1** | +|Register |R/W|Description |Encoding | +|---------|:-:|-------------------------------|:-----------------:| +| 0x00 | W | Speed motor 1 |**-128** - **127** | +| 0x01 | W | Speed motor 2 |**-128** - **127** | +| 0x02 | W | Speed motor 3 |**-128** - **127** | +| 0x03 | W | Speed motor 4 |**-128** - **127** | +| 0x04 | W | Servo 1 position selection | **0** - **1** | +| 0x05 | W | Servo 2 position selection | **0** - **1** | +| 0x06 | W | Servo 3 position selection | **0** - **1** | +| 0x07 | W | Servo 4 position selection | **0** - **1** | 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. +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 ----------------------------------------------- @@ -61,3 +81,15 @@ Pinout description |----|----------------------------------|-----------| | 4 | I2C Bus SDA | I2C | | 5 | I2C Bus SCL | I2C | + + +Motors placement +----------------------------------------------- + + ,-~***~-, + /1 4\ + | | + | | + | | + \3 2/ + `-.....-' diff --git a/main controller code/src/gyro.c b/main controller code/src/gyro.c index dc18626..1f32525 100644 --- a/main controller code/src/gyro.c +++ b/main controller code/src/gyro.c @@ -11,10 +11,9 @@ * 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 SAMPLE_MIN_ELAPSED_TIME 10 // ms - int init_gyro(void) { // Verify gyro initialisation @@ -56,7 +55,7 @@ static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t void gyro_calibrate(void) { - const uint nb_samples = 1000; + const uint nb_samples = 10000; int16_t x, y, z; int32_t x_sum = 0, y_sum = 0, z_sum = 0; @@ -70,18 +69,19 @@ void gyro_calibrate(void) z_sum += z; 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.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.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("y_cal:%f\n", robot.gyro_data.y_offset); - //printf("z_cal:%f\n", robot.gyro_data.z_offset); + printf("\nx_cal:%.5f\n", robot.gyro_data.x_offset); + printf("\ny_cal:%.5f\n", robot.gyro_data.y_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) @@ -113,6 +113,13 @@ void gyro_update(void) 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; + 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_y_angle:%f\n", robot.gyro_data.y_angle); printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle); diff --git a/main controller code/src/i2c_master.c b/main controller code/src/i2c_master.c index e254453..b8fa79e 100644 --- a/main controller code/src/i2c_master.c +++ b/main controller code/src/i2c_master.c @@ -14,6 +14,11 @@ void i2c_master_init(void) 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) { i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false); diff --git a/main controller code/src/include/i2c_master.h b/main controller code/src/include/i2c_master.h index 23e825e..9d59aa0 100644 --- a/main controller code/src/include/i2c_master.h +++ b/main controller code/src/include/i2c_master.h @@ -10,6 +10,8 @@ // Init master i2c void i2c_master_init(void); +// Deinit master i2c +void i2c_master_deinit(void); // Send [src] of [len] to [address] and close communication void i2c_master_write(uint8_t address, const uint8_t *src, size_t len); // Receive [dst] of [len] from [address] and close communication diff --git a/main controller code/src/include/motion_control.h b/main controller code/src/include/motion_control.h index e69de29..84d8b84 100644 --- a/main controller code/src/include/motion_control.h +++ b/main controller code/src/include/motion_control.h @@ -0,0 +1,15 @@ +#ifndef MOTION_CONTROL_H +#define MOTION_CONTROL_H + +#include + +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 \ No newline at end of file diff --git a/main controller code/src/include/motors.h b/main controller code/src/include/motors.h index ec9847b..b64abc9 100644 --- a/main controller code/src/include/motors.h +++ b/main controller code/src/include/motors.h @@ -19,7 +19,7 @@ typedef struct motor_def_t { uint pwm_pin; uint dir1_pin; uint dir2_pin; - uint8_t buffer_address; + uint8_t buffer_reg; } motor_def_t; extern const motor_def_t MOTORS_DEFS[]; @@ -38,7 +38,7 @@ typedef struct { uint pwm_pin; uint open_pos; uint close_pos; - uint8_t buffer_address; + uint8_t buffer_reg; } servo_motor_def_t; 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); // Get [motor] speed from motion controller through i2c 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 diff --git a/main controller code/src/include/robot.h b/main controller code/src/include/robot.h index 482928a..2c1e6cf 100644 --- a/main controller code/src/include/robot.h +++ b/main controller code/src/include/robot.h @@ -3,9 +3,13 @@ #include #include "gyro.h" +#include "udp_buffer.h" +#include "motion_control.h" typedef struct robot_t { gyro_data_t gyro_data; + udp_buffer_t udp_buffer; + motion_control_data_t motion_control_data; bool is_running; double delta_time_ms; diff --git a/main controller code/src/include/udp_buffer.h b/main controller code/src/include/udp_buffer.h index 3a8bd53..aec0f4f 100644 --- a/main controller code/src/include/udp_buffer.h +++ b/main controller code/src/include/udp_buffer.h @@ -1,4 +1,20 @@ #ifndef UDP_BUFFER_H #define UDP_BUFFER_H +#include + +#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 \ No newline at end of file diff --git a/main controller code/src/motion_control.c b/main controller code/src/motion_control.c index e69de29..c0f1fd3 100644 --- a/main controller code/src/motion_control.c +++ b/main controller code/src/motion_control.c @@ -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); +} diff --git a/main controller code/src/motors.c b/main controller code/src/motors.c index 411865b..f0779c4 100644 --- a/main controller code/src/motors.c +++ b/main controller code/src/motors.c @@ -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]; 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); } @@ -31,10 +31,29 @@ int8_t i2c_get_motor_speed(motors_enum_t 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; - 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; 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; +} diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c index 0224ef1..3ba461d 100644 --- a/main controller code/src/robot.c +++ b/main controller code/src/robot.c @@ -3,14 +3,11 @@ #include #include #include "include/i2c_master.h" -#include "include/gyro.h" int robot_init(void) { stdio_init_all(); - sleep_ms(5000); - i2c_master_init(); if(init_gyro()) return -1; @@ -24,9 +21,9 @@ int robot_init(void) 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; + clock_t start_clock_state = clock(); + robot.delta_time_ms = (double)(start_clock_state - last_clock_state) * 1000.0 / CLOCKS_PER_SEC; + last_clock_state = start_clock_state; } void robot_handle_inputs_outputs(void) @@ -34,8 +31,14 @@ void robot_handle_inputs_outputs(void) update_time(); gyro_update(); + + update_motion_control_data(); + i2c_update_motion_control(); + + i2c_update_servo_motors(); } void robot_deinit(void) { + i2c_master_deinit(); } \ No newline at end of file diff --git a/main controller code/src/udp_buffer.c b/main controller code/src/udp_buffer.c index 89efe85..2825cd6 100644 --- a/main controller code/src/udp_buffer.c +++ b/main controller code/src/udp_buffer.c @@ -1 +1,22 @@ -#include "include/udp_buffer.h" \ No newline at end of file +#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]); + } +} diff --git a/motion controller code/src/i2c_buffer.c b/motion controller code/src/i2c_buffer.c index e7bf6f1..22d8000 100644 --- a/motion controller code/src/i2c_buffer.c +++ b/motion controller code/src/i2c_buffer.c @@ -8,28 +8,28 @@ void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event) switch(event) { 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 - robot.i2c_buffer.buffer_address = i2c_slave_read_byte(); - robot.i2c_buffer.buffer_address_written = true; + robot.i2c_buffer.buffer_reg = i2c_slave_read_byte(); + robot.i2c_buffer.buffer_reg_written = true; } else { // save into memory - robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address] = i2c_slave_read_byte(); - robot.i2c_buffer.buffer_address++; + robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg] = i2c_slave_read_byte(); + robot.i2c_buffer.buffer_reg++; } break; case I2C_SLAVE_REQUEST: // master is requesting data // load from memory - i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address]); - robot.i2c_buffer.buffer_address++; + i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]); + robot.i2c_buffer.buffer_reg++; break; case I2C_SLAVE_FINISH: // master has signalled Stop / Restart - robot.i2c_buffer.buffer_address_written = false; + robot.i2c_buffer.buffer_reg_written = false; break; default: @@ -43,7 +43,7 @@ void update_motors_from_buffer(void) { 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]; - 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]); } } \ No newline at end of file diff --git a/motion controller code/src/include/i2c_buffer.h b/motion controller code/src/include/i2c_buffer.h index 6a74aa7..8bfb859 100644 --- a/motion controller code/src/include/i2c_buffer.h +++ b/motion controller code/src/include/i2c_buffer.h @@ -6,8 +6,8 @@ typedef struct i2c_buffer_t { uint8_t buffer[256]; - uint8_t buffer_address; - bool buffer_address_written; + uint8_t buffer_reg; + bool buffer_reg_written; } i2c_buffer_t; // I2c slave buffer handler for writing and reading data to the buffer diff --git a/motion controller code/src/include/motors.h b/motion controller code/src/include/motors.h index a5e231a..ed9d860 100644 --- a/motion controller code/src/include/motors.h +++ b/motion controller code/src/include/motors.h @@ -17,7 +17,7 @@ typedef struct motor_def_t { uint pwm_pin; uint dir1_pin; uint dir2_pin; - uint8_t buffer_address; + uint8_t buffer_reg; } motor_def_t; extern const motor_def_t MOTORS_DEFS[]; @@ -36,7 +36,7 @@ typedef struct { uint pwm_pin; uint open_pos; uint close_pos; - uint8_t buffer_address; + uint8_t buffer_reg; } servo_motor_def_t; extern const servo_motor_def_t SERVO_MOTORS_DEFS[];