diff --git a/program/main_controller/CMakeLists.txt b/program/main_controller/CMakeLists.txt index 06f6f56..dba9b09 100644 --- a/program/main_controller/CMakeLists.txt +++ b/program/main_controller/CMakeLists.txt @@ -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 diff --git a/program/main_controller/src/headers/motion_control.h b/program/main_controller/src/headers/motion_control.h index a8f49ac..05dafa6 100644 --- a/program/main_controller/src/headers/motion_control.h +++ b/program/main_controller/src/headers/motion_control.h @@ -2,21 +2,16 @@ #define MOTION_CONTROL_H #include -#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 \ No newline at end of file diff --git a/program/main_controller/src/headers/motors.h b/program/main_controller/src/headers/motors.h new file mode 100644 index 0000000..c0d6ac8 --- /dev/null +++ b/program/main_controller/src/headers/motors.h @@ -0,0 +1,15 @@ +#ifndef MOTORS_H +#define MOTORS_H + +#include + +typedef enum motors_t { + MOTOR1, + MOTOR2, + + NB_MOTORS +} motors_t; + +void motor_set_dir(motors_t motor, int16_t value); + +#endif // MOTORS_H \ No newline at end of file diff --git a/program/main_controller/src/i2c/headers/motors.h b/program/main_controller/src/i2c/headers/motors.h deleted file mode 100644 index be81a88..0000000 --- a/program/main_controller/src/i2c/headers/motors.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef MOTORS_H -#define MOTORS_H - -#include - -// 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 diff --git a/program/main_controller/src/i2c/motors.c b/program/main_controller/src/i2c/motors.c deleted file mode 100644 index 028bffd..0000000 --- a/program/main_controller/src/i2c/motors.c +++ /dev/null @@ -1,35 +0,0 @@ -#include "headers/motors.h" - -#include -#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); -} diff --git a/program/main_controller/src/motion_control.c b/program/main_controller/src/motion_control.c index 0f4ed8e..0255c89 100644 --- a/program/main_controller/src/motion_control.c +++ b/program/main_controller/src/motion_control.c @@ -2,66 +2,100 @@ #include #include -#include "i2c/headers/motors.h" +#include +#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, ®, 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"); + } + } } diff --git a/program/main_controller/src/motors.c b/program/main_controller/src/motors.c new file mode 100644 index 0000000..4e8853d --- /dev/null +++ b/program/main_controller/src/motors.c @@ -0,0 +1,47 @@ +#include "headers/motors.h" + +#include +#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); + } +} diff --git a/program/main_controller/src/robot.c b/program/main_controller/src/robot.c index 3154348..707d378 100644 --- a/program/main_controller/src/robot.c +++ b/program/main_controller/src/robot.c @@ -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(); diff --git a/program/main_controller/src/wifi/headers/udp_client.h b/program/main_controller/src/wifi/headers/udp_client.h index 91b2f1c..5bf81e3 100644 --- a/program/main_controller/src/wifi/headers/udp_client.h +++ b/program/main_controller/src/wifi/headers/udp_client.h @@ -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 diff --git a/program/main_controller/src/wifi/udp_client.c b/program/main_controller/src/wifi/udp_client.c index f52fbf1..b1bee29 100644 --- a/program/main_controller/src/wifi/udp_client.c +++ b/program/main_controller/src/wifi/udp_client.c @@ -1,11 +1,15 @@ #include "headers/udp_client.h" #include +#include #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) diff --git a/program/motion_controller/CMakeLists.txt b/program/motion_controller/CMakeLists.txt index 0d971ff..265a052 100644 --- a/program/motion_controller/CMakeLists.txt +++ b/program/motion_controller/CMakeLists.txt @@ -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 diff --git a/program/motion_controller/src/headers/motors.h b/program/motion_controller/src/headers/motors.h index 069da65..550887a 100644 --- a/program/motion_controller/src/headers/motors.h +++ b/program/motion_controller/src/headers/motors.h @@ -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 diff --git a/program/motion_controller/src/headers/servo_motors.h b/program/motion_controller/src/headers/servo_motors.h deleted file mode 100644 index e69de29..0000000 diff --git a/program/motion_controller/src/i2c/headers/i2c_slave.h b/program/motion_controller/src/i2c/headers/i2c_slave.h index 017d4bd..f4ad4b8 100644 --- a/program/motion_controller/src/i2c/headers/i2c_slave.h +++ b/program/motion_controller/src/i2c/headers/i2c_slave.h @@ -1,9 +1,3 @@ -/* - * Copyright (c) 2021 Valentin Milea - * - * 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; diff --git a/program/motion_controller/src/i2c/i2c_slave.c b/program/motion_controller/src/i2c/i2c_slave.c index 4dfcb8c..9ff620e 100644 --- a/program/motion_controller/src/i2c/i2c_slave.c +++ b/program/motion_controller/src/i2c/i2c_slave.c @@ -1,9 +1,3 @@ -/* - * Copyright (c) 2021 Valentin Milea - * - * SPDX-License-Identifier: MIT - */ - #include "headers/i2c_slave.h" #include @@ -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; diff --git a/program/motion_controller/src/motors.c b/program/motion_controller/src/motors.c index 8c591d9..b9edfdf 100644 --- a/program/motion_controller/src/motors.c +++ b/program/motion_controller/src/motors.c @@ -4,137 +4,50 @@ #include #include -const motor_def_t MOTORS_DEFS[] = { - {0, 4, 5, 0x00}, - {1, 6, 7, 0x01}, - {2, 9, 8, 0x02}, - {3, 11, 10, 0x03}, -}; +#include -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); } diff --git a/program/motion_controller/src/robot.c b/program/motion_controller/src/robot.c index 37205bd..6781bf0 100644 --- a/program/motion_controller/src/robot.c +++ b/program/motion_controller/src/robot.c @@ -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)