diff --git a/program/main_controller/src/motion_control.c b/program/main_controller/src/motion_control.c index 0255c89..dac2197 100644 --- a/program/main_controller/src/motion_control.c +++ b/program/main_controller/src/motion_control.c @@ -86,6 +86,12 @@ void motion_control_update(void) data.hard.motor1_speed = (uint8_t)abs((int)motor1_speed); data.hard.motor2_speed = (uint8_t)abs((int)motor2_speed); + data.hard.motor1_speed = (uint8_t)abs((int)robot.udp_client.data.hard.inputs.joystick_x); + data.hard.motor2_speed = (uint8_t)abs((int)robot.udp_client.data.hard.inputs.joystick_x); + + motor_set_dir(MOTOR1, (int16_t)robot.udp_client.data.hard.inputs.joystick_x); + motor_set_dir(MOTOR2, (int16_t)robot.udp_client.data.hard.inputs.joystick_x); + uint8_t reg = 0x00; if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, ®, 1, true) == PICO_ERROR_GENERIC) diff --git a/program/main_controller/src/robot.c b/program/main_controller/src/robot.c index 707d378..8d0f927 100644 --- a/program/main_controller/src/robot.c +++ b/program/main_controller/src/robot.c @@ -30,12 +30,29 @@ void robot_init(void) motion_control_init(); - if(wifi_operator_init()) - robot.is_running = false; - - if(udp_client_init()) - robot.is_running = false; + if(wifi_operator_init()){ + while(1){ + printf("No Wifi\n"); + robot.is_running = false; + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true); + sleep_ms(50); + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false); + sleep_ms(50); + } + } + + if(udp_client_init()){ + while(1){ + printf("No UDP\n"); + robot.is_running = false; + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true); + sleep_ms(50); + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false); + sleep_ms(50); + } + } + // Initialisation ended for(uint8_t i = 0, led_state = true; i < 5; i++) { @@ -45,6 +62,7 @@ void robot_init(void) led_state = !led_state; } + printf(">robot_running:%d\n", robot.is_running); } static inline void update_time(void) @@ -69,6 +87,7 @@ static inline void update_time(void) void robot_handle_inputs_outputs(void) { + static bool led_state=false; update_time(); cyw43_arch_poll(); @@ -79,7 +98,10 @@ void robot_handle_inputs_outputs(void) mcp23017_update(); - tight_loop_contents(); + sleep_ms(200); + led_state= !led_state; + uint8_t data[] = {0x02, led_state}; + int ret = i2c_write_blocking(I2C_MASTER_INSTANCE, 0x09, data, 2, false); } void robot_deinit(void) diff --git a/program/motion_controller/CMakeLists.txt b/program/motion_controller/CMakeLists.txt index 265a052..79e5865 100644 --- a/program/motion_controller/CMakeLists.txt +++ b/program/motion_controller/CMakeLists.txt @@ -27,7 +27,7 @@ target_link_libraries(motion_controller ) pico_enable_stdio_usb(motion_controller 1) -pico_enable_stdio_uart(motion_controller 1) +pico_enable_stdio_uart(motion_controller 0) pico_add_extra_outputs(motion_controller) diff --git a/program/motion_controller/src/i2c/headers/i2c_slave.h b/program/motion_controller/src/i2c/headers/i2c_slave.h index f4ad4b8..20808a4 100644 --- a/program/motion_controller/src/i2c/headers/i2c_slave.h +++ b/program/motion_controller/src/i2c/headers/i2c_slave.h @@ -27,4 +27,9 @@ void init_i2c_slave(void); // Deinit i2c slave void deinit_i2c_slave(void); +uint8_t get_vitesse_moteur_1(void); +uint8_t get_vitesse_moteur_2(void); + +extern int nb_messages; + #endif // I2C_SLAVE_H \ No newline at end of file diff --git a/program/motion_controller/src/i2c/i2c_slave.c b/program/motion_controller/src/i2c/i2c_slave.c index 9ff620e..f810ed8 100644 --- a/program/motion_controller/src/i2c/i2c_slave.c +++ b/program/motion_controller/src/i2c/i2c_slave.c @@ -4,37 +4,40 @@ #include #include #include - -void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +static struct { - switch(event) - { - case I2C_SLAVE_RECEIVE: // master has written some data - if(!robot.i2c_buffer.buffer_reg_written) - { + uint8_t mem[256]; + uint8_t mem_address; + bool mem_address_written; +} context; + +int nb_messages = 0; + +// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls / +// printing to stdio may interfere with interrupt handling. +static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) { + switch (event) { + case I2C_SLAVE_RECEIVE: // master has written some data + if (!context.mem_address_written) { // writes always start with the memory address - robot.i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE); - robot.i2c_buffer.buffer_reg_written = true; - } - else - { + context.mem_address = i2c_read_byte_raw(i2c); + context.mem_address_written = true; + } else { // save into memory - robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE); - robot.i2c_buffer.buffer_reg++; + context.mem[context.mem_address] = i2c_read_byte_raw(i2c); + context.mem_address++; } break; - - case I2C_SLAVE_REQUEST: // master is requesting data + case I2C_SLAVE_REQUEST: // master is requesting data // load from memory - i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer.raw[robot.i2c_buffer.buffer_reg]); - robot.i2c_buffer.buffer_reg++; + i2c_write_byte_raw(i2c, context.mem[context.mem_address]); + context.mem_address++; break; - - case I2C_SLAVE_FINISH: // master has signalled Stop / Restart - robot.i2c_buffer.buffer_reg_written = false; + case I2C_SLAVE_FINISH: // master has signalled Stop / Restart + context.mem_address_written = false; + nb_messages++; break; - - default: + default: break; } } @@ -49,7 +52,7 @@ void init_i2c_slave(void) i2c_init(I2C_SLAVE_INSTANCE, 0); // New SDK method to init i2c slave - i2c_slave_init(I2C_SLAVE_INSTANCE, I2C_SLAVE_ADDRESS, &i2c_slave_buffer_handler); + i2c_slave_init(I2C_SLAVE_INSTANCE, I2C_SLAVE_ADDRESS, i2c_slave_handler); } void deinit_i2c_slave(void) @@ -60,4 +63,12 @@ void deinit_i2c_slave(void) // New SDK method to reset i2c slave i2c_slave_deinit(I2C_SLAVE_INSTANCE); -} \ No newline at end of file +} + +uint8_t get_vitesse_moteur_1(void){ + return context.mem[0]; +} + +uint8_t get_vitesse_moteur_2(void){ + return context.mem[1]; +} diff --git a/program/motion_controller/src/motors.c b/program/motion_controller/src/motors.c index b9edfdf..3fc4958 100644 --- a/program/motion_controller/src/motors.c +++ b/program/motion_controller/src/motors.c @@ -1,9 +1,11 @@ #include "headers/motors.h" +#include "i2c/headers/i2c_slave.h" #include #include #include + #include static inline uint get_motor_pwm_pin(motors_enum_t motor) @@ -32,7 +34,10 @@ void motors_init(void) const uint SLICE_NUM = pwm_gpio_to_slice_num(MOTOR_PWM_PIN); gpio_set_function(MOTOR_PWM_PIN, GPIO_FUNC_PWM); + + pwm_set_clkdiv(SLICE_NUM, 255); pwm_set_wrap(SLICE_NUM, 256); + pwm_set_enabled(SLICE_NUM, true); motor_set_speed(motor, 0); @@ -48,6 +53,6 @@ void motor_set_speed(motors_enum_t motor, uint8_t value) void motors_update(void) { - motor_set_speed(MOTOR1, robot.i2c_buffer.buffer.hard.motor1_speed); - motor_set_speed(MOTOR2, robot.i2c_buffer.buffer.hard.motor2_speed); + motor_set_speed(MOTOR1, get_vitesse_moteur_1()); + motor_set_speed(MOTOR2, get_vitesse_moteur_2()); } diff --git a/program/motion_controller/src/robot.c b/program/motion_controller/src/robot.c index 6781bf0..f7d2079 100644 --- a/program/motion_controller/src/robot.c +++ b/program/motion_controller/src/robot.c @@ -43,9 +43,9 @@ static inline void update_time(void) static double elapsed_time = 0.0; elapsed_time += robot.delta_time_ms; - if(elapsed_time >= 1000.0) + if(nb_messages >= 10) { - elapsed_time = 0.0; + nb_messages = 0; gpio_put(PICO_DEFAULT_LED_PIN, led_state); @@ -55,18 +55,22 @@ static inline void update_time(void) void robot_handle_inputs_outputs(void) { + static bool led_state=false; update_time(); 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); + printf(">motor1_speed:%d\n", get_vitesse_moteur_1()); + printf(">motor2_speed:%d\n", get_vitesse_moteur_2()); + sleep_ms(200); + led_state= !led_state; + uint8_t data[] = {0x02, led_state}; + int ret = i2c_write_blocking(i2c0, 0x09, data, 2, false); - tight_loop_contents(); + //tight_loop_contents(); } void robot_deinit(void) { deinit_i2c_slave(); -} \ No newline at end of file +}