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

Working servos (almost) and cleaned robot.h/.c for both controllers

This commit is contained in:
Ulysse Cura 2026-02-28 10:02:10 +01:00
parent d8d504fdc1
commit c5bcc17529
19 changed files with 292 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
program/main_controller/CMakeLists.txt
View File

@ -18,6 +18,7 @@ add_executable(main_controller
src/main.c
src/robot.c
src/motors.c
src/servos.c
src/motion_control.c
src/i2c/i2c_master.c
src/i2c/gyro.c

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

@ -2,16 +2,23 @@
#define MOTION_CONTROL_H
#include <stdint.h>
#include <stdbool.h>
#define I2C_MOTION_CONTROLLER_ADDRESS 0x09
typedef struct motion_control_data_t {
int16_t target_speed;
float target_angle;
int16_t motor1_speed;
int16_t motor2_speed;
bool led_state;
} motion_control_data_t;
void motion_control_init(void);
void motion_control_update(void);
void motion_control_send(void);
#endif // MOTION_CONTROL_H

14
program/main_controller/src/headers/robot.h
View File

@ -2,16 +2,20 @@
#define ROBOT_H
#include <stdbool.h>
#include "i2c/headers/mcp23017.h"
#include "i2c/headers/gyro.h"
#include "motion_control.h"
#include "i2c/headers/mcp23017.h"
#include "headers/motion_control.h"
#include "headers/servos.h"
#include "wifi/headers/udp_client.h"
typedef struct robot_t {
udp_client_t udp_client;
mcp23017_data_t mcp23017_data;
gyro_data_t gyro_data;
mcp23017_data_t mcp23017_data;
motion_control_data_t motion_control_data;
servos_data_t servos_data;
udp_client_t udp_client;
bool is_running;
float delta_time_ms;
@ -22,7 +26,7 @@ extern robot_t robot;
// Init all robot's components
void robot_init(void);
// Handle inputs and outputs
void robot_handle_inputs_outputs(void);
void robot_update(void);
// Deinit all robot's components
void robot_deinit(void);

22
program/main_controller/src/headers/servos.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef SERVOS_H
#define SERVOS_H
#include <stdint.h>
typedef enum servos_t {
SERVO1,
SERVO2,
SERVO3,
NB_SERVOS
} servos_t;
typedef struct servos_data_t {
uint16_t servos_pos[NB_SERVOS];
} servos_data_t;
void servos_init(void);
void servo_set_angle(servos_t servo, uint16_t pos);
#endif // SERVOS_H

2
program/main_controller/src/main.c
View File

@ -18,7 +18,7 @@ int main(void)
while(robot.is_running)
{
robot_handle_inputs_outputs();
robot_update();
}
robot_deinit();

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

@ -5,21 +5,29 @@
#include <hardware/i2c.h>
#include "i2c/headers/i2c_master.h"
#include "headers/motors.h"
#include "headers/servos.h"
#include "headers/robot.h"
#define ANGLE_PER_S 90.0f
#define GAIN_KP 10
#define MSG_LEN sizeof(uint8_t) * 2
#define MSG_DELAY_MS 5.0f
#define MSG_LEN sizeof(uint8_t) + /* 0x00 */ \
sizeof(uint8_t) + /* LED */ \
sizeof(uint8_t) * NB_MOTORS + /* Motors */ \
sizeof(uint16_t) * NB_SERVOS /* Servos */
#define MSG_DELAY_MS 2.0f
void motion_control_init(void)
{
robot.motion_control_data.led_state = false;
robot.motion_control_data.target_speed = 0;
robot.motion_control_data.target_angle = 0.0f;
robot.motion_control_data.motor1_speed = 0;
robot.motion_control_data.motor2_speed = 0;
}
void motion_control_update(void)
static inline void motors_control_update(void)
{
// 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;
@ -46,8 +54,6 @@ void motion_control_update(void)
//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;
@ -65,19 +71,69 @@ void motion_control_update(void)
correction = 150;
}*/
float motor1_speed = (float)robot.motion_control_data.target_speed - correction;
float motor2_speed = (float)robot.motion_control_data.target_speed + correction;
robot.motion_control_data.motor1_speed = (int16_t)((float)robot.motion_control_data.target_speed - correction);
robot.motion_control_data.motor2_speed = (int16_t)((float)robot.motion_control_data.target_speed + correction);
if(motor1_speed > 255) motor1_speed = 255;
if(motor2_speed > 255) motor2_speed = 255;
if(robot.motion_control_data.motor1_speed > 255) robot.motion_control_data.motor1_speed = 255;
if(robot.motion_control_data.motor2_speed > 255) robot.motion_control_data.motor2_speed = 255;
if(motor1_speed < -255) motor1_speed = -255;
if(motor2_speed < -255) motor2_speed = -255;
if(robot.motion_control_data.motor1_speed < -255) robot.motion_control_data.motor1_speed = -255;
if(robot.motion_control_data.motor2_speed < -255) robot.motion_control_data.motor2_speed = -255;
// Set dir pins
motor_set_dir(MOTOR1, (int16_t)motor1_speed);
motor_set_dir(MOTOR2, (int16_t)motor2_speed);
motor_set_dir(MOTOR1, robot.motion_control_data.motor1_speed);
motor_set_dir(MOTOR2, robot.motion_control_data.motor2_speed);
}
static inline void servos_control_update(void)
{
const uint16_t SERVO1_POSITIONS[5] = {
500,
// MORE TO DO
1950,
};
const uint16_t SERVO2_POSITIONS[2] = {
600,
2850,
};
// XXX DOESN'T WORK ??
const uint16_t SERVO3_POSITIONS[2] = {
600,
2850,
};
/*
// toggle servo 1 pos if button green is pressed
static float servo1_toggle_elapsed_time = 0.0f;
servo1_toggle_elapsed_time += robot.delta_time_ms;
if(robot.udp_client.data.hard.inputs.buttons.button_green && servo1_toggle_elapsed_time >= 200.0f)
{
servo1_toggle_elapsed_time = 0.0f;
servo_set_angle(SERVO1, 90 - robot.servos_data.servos_pos[SERVO1]);
}
*/
//servo_set_angle(SERVO3, 400);
}
void steppers_control_update(void)
{
steppers_set_pos(48);
}
void motion_control_update(void)
{
motors_control_update();
servos_control_update();
steppers_control_update();
}
void motion_control_send(void)
{
static float elapsed_time_ms = 0.0f;
elapsed_time_ms += robot.delta_time_ms;
@ -85,17 +141,20 @@ void motion_control_update(void)
{
elapsed_time_ms = 0;
// Send motors speed via I2C
uint8_t data[5];
uint8_t data[MSG_LEN] = {
0x00,
(uint8_t)robot.motion_control_data.led_state,
(uint8_t)abs(robot.motion_control_data.motor1_speed),
(uint8_t)abs(robot.motion_control_data.motor2_speed),
(uint8_t)(robot.servos_data.servos_pos[SERVO1] >> 8) & 0xFF,
(uint8_t)(robot.servos_data.servos_pos[SERVO1]) & 0xFF,
(uint8_t)(robot.servos_data.servos_pos[SERVO2] >> 8) & 0xFF,
(uint8_t)(robot.servos_data.servos_pos[SERVO2]) & 0xFF,
(uint8_t)(robot.servos_data.servos_pos[SERVO3] >> 8) & 0xFF,
(uint8_t)(robot.servos_data.servos_pos[SERVO3]) & 0xFF
};
data[0] = 0x00; /// registre
data[1] = (uint8_t)abs((int)motor1_speed);
data[2] = (uint8_t)abs((int)motor2_speed);
uint8_t reg = 0x00;
if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data, 3, false) == PICO_ERROR_GENERIC)
if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_MOTION_CONTROLLER_ADDRESS, data, MSG_LEN, false) == PICO_ERROR_GENERIC)
{
puts("Motion controller not reachable");
}

16
program/main_controller/src/motors.c
View File

@ -3,10 +3,10 @@
#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
#define MCP23017_MOTOR1_DIR1_GPIO 12
#define MCP23017_MOTOR1_DIR2_GPIO 13
#define MCP23017_MOTOR2_DIR1_GPIO 14
#define MCP23017_MOTOR2_DIR2_GPIO 15
void motor_set_dir(motors_t motor, int16_t value)
{
@ -16,13 +16,13 @@ void motor_set_dir(motors_t motor, int16_t value)
switch(motor)
{
case MOTOR1:
gpio_dir1_pin = MOTOR1_DIR1_MCP23017_GPIO;
gpio_dir2_pin = MOTOR1_DIR2_MCP23017_GPIO;
gpio_dir1_pin = MCP23017_MOTOR1_DIR1_GPIO;
gpio_dir2_pin = MCP23017_MOTOR1_DIR2_GPIO;
break;
case MOTOR2:
gpio_dir1_pin = MOTOR2_DIR1_MCP23017_GPIO;
gpio_dir2_pin = MOTOR2_DIR2_MCP23017_GPIO;
gpio_dir1_pin = MCP23017_MOTOR2_DIR1_GPIO;
gpio_dir2_pin = MCP23017_MOTOR2_DIR2_GPIO;
break;
default:

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

@ -20,6 +20,7 @@ void robot_init(void)
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
// I2C and devices
i2c_master_init();
mcp23017_init();
@ -31,31 +32,15 @@ void robot_init(void)
motion_control_init();
if(wifi_operator_init()){
while(1){
printf("No Wifi\n");
// WiFi
if(wifi_operator_init())
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");
if(udp_client_init())
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++)
// Initialisation ended, loop forever if inititalisation failed
for(uint i = 0, led_state = false; robot.is_running ? (i < 5) : true; i++)
{
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
@ -63,7 +48,6 @@ void robot_init(void)
led_state = !led_state;
}
printf(">robot_running:%d\n", robot.is_running);
}
static inline void update_time(void)
@ -76,21 +60,18 @@ static inline void update_time(void)
static float elapsed_time_ms = 0.0f;
elapsed_time_ms += robot.delta_time_ms;
static bool led_state = false;
if(elapsed_time_ms >= 1000.0f)
{
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, robot.motion_control_data.led_state);
elapsed_time_ms = 0.0f;
led_state = !led_state;
robot.motion_control_data.led_state = !robot.motion_control_data.led_state;
}
}
void robot_handle_inputs_outputs(void)
void robot_update(void)
{
static bool led_state=false;
static uint32_t temps_ms_old =0, timer_200_ms=0;
uint32_t temps_ms;
update_time();
cyw43_arch_poll();
@ -99,17 +80,11 @@ void robot_handle_inputs_outputs(void)
motion_control_update();
motion_control_send();
mcp23017_update();
temps_ms = to_ms_since_boot(get_absolute_time());
timer_200_ms += temps_ms - temps_ms_old;
temps_ms_old = temps_ms;
if(timer_200_ms > 200){
timer_200_ms = 0;
led_state= !led_state;
uint8_t data[] = {0x02, led_state};
int ret = i2c_write_blocking(I2C_MASTER_INSTANCE, 0x09, data, 2, false);
}
tight_loop_contents();
}
void robot_deinit(void)

14
program/main_controller/src/servos.c Normal file
View File

@ -0,0 +1,14 @@
#include "headers/servos.h"
#include "headers/robot.h"
void servos_init(void)
{
for(size_t servo = SERVO1; servo < NB_SERVOS; servo++)
robot.servos_data.servos_pos[servo] = 0;
}
inline void servo_set_angle(servos_t servo, uint16_t pos)
{
robot.servos_data.servos_pos[servo] = pos;
}

1
program/motion_controller/CMakeLists.txt
View File

@ -13,6 +13,7 @@ add_executable(motion_controller
src/robot.c
src/i2c/i2c_slave.c
src/motors.c
src/servos.c
)
target_include_directories(motion_controller PRIVATE

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

@ -4,20 +4,18 @@
#include <pico/stdlib.h>
// Motors
typedef enum motors_enum_t {
typedef enum motors_t {
MOTOR1,
MOTOR2,
NB_MOTORS
} motors_enum_t;
} motors_t;
#define MOTOR1_PWM_PIN 9
#define MOTOR2_PWM_PIN 8
#define I2C_BUFFER_MOTOR1_SPEED 1
#define I2C_BUFFER_MOTOR2_SPEED 2
// Init all motors defined in the MOTORS_DEF array
void motors_init(void);
// Set [motor] in motor_enum_t at [value] between -127 and 128
void motor_set_speed(motors_enum_t motor, uint8_t value);
// Update motors from the data in the i2c buffer
void motors_update(void);

4
program/motion_controller/src/headers/robot.h
View File

@ -7,7 +7,7 @@ typedef struct robot_t {
i2c_buffer_t i2c_buffer;
bool is_running;
double delta_time_ms;
float delta_time_ms;
} robot_t;
extern robot_t robot;
@ -15,7 +15,7 @@ extern robot_t robot;
// Init all robot's components
void robot_init(void);
// Handle inputs and outputs
void robot_handle_inputs_outputs(void);
void robot_update(void);
// Deinit all robot's components
void robot_deinit(void);

23
program/motion_controller/src/headers/servos.h Normal file
View File

@ -0,0 +1,23 @@
#ifndef SERVOS_H
#define SERVOS_H
typedef enum servos_t {
SERVO1,
SERVO2,
SERVO3,
NB_SERVOS
} servos_t;
#define I2C_BUFFER_SERVO1_ANGLE_H 3
#define I2C_BUFFER_SERVO1_ANGLE_L 4
#define I2C_BUFFER_SERVO2_ANGLE_H 5
#define I2C_BUFFER_SERVO2_ANGLE_L 6
#define I2C_BUFFER_SERVO3_ANGLE_H 7
#define I2C_BUFFER_SERVO3_ANGLE_L 8
void servos_init(void);
void servos_update(void);
#endif // SERVOS_H

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

@ -25,7 +25,4 @@ 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);
#endif // I2C_SLAVE_H

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

@ -9,38 +9,35 @@
#include <hardware/gpio.h>
#include <hardware/i2c.h>
#include "pico/i2c_slave.h"
#include <stdio.h>
extern i2c_buffer_t i2c_buffer;
#include "headers/robot.h"
void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
{
switch(event)
{
case I2C_SLAVE_RECEIVE: // master has written some data
if(!i2c_buffer.buffer_reg_written)
if(!robot.i2c_buffer.buffer_reg_written)
{
// writes always start with the memory address
i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
i2c_buffer.buffer_reg_written = true;
robot.i2c_buffer.buffer_reg = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
robot.i2c_buffer.buffer_reg_written = true;
}
else
{
// save into memory
i2c_buffer.buffer[i2c_buffer.buffer_reg] = i2c_read_byte_raw(I2C_SLAVE_INSTANCE);
i2c_buffer.buffer_reg++;
robot.i2c_buffer.buffer[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, i2c_buffer.buffer[i2c_buffer.buffer_reg]);
i2c_buffer.buffer_reg++;
i2c_write_byte_raw(I2C_SLAVE_INSTANCE, robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]);
robot.i2c_buffer.buffer_reg++;
break;
case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
i2c_buffer.buffer_reg_written = false;
robot.i2c_buffer.buffer_reg_written = false;
break;
default:
@ -70,11 +67,3 @@ void deinit_i2c_slave(void)
// New SDK method to reset i2c slave
i2c_slave_deinit(I2C_SLAVE_INSTANCE);
}
uint8_t get_vitesse_moteur_1(void){
return i2c_buffer.buffer[0];
}
uint8_t get_vitesse_moteur_2(void){
return i2c_buffer.buffer[1];
}

32
program/motion_controller/src/main.c
View File

@ -8,38 +8,20 @@
* Ce Pico est un esclave piloté par le Pico Principal. *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include <pico/stdlib.h>
#include <stdio.h>
#include "i2c/headers/i2c_slave.h"
#include "headers/motors.h"
#include "headers/robot.h"
i2c_buffer_t i2c_buffer;
robot_t robot;
int main(void)
{
stdio_init_all();
robot_init();
sleep_ms(1000);
puts("STDIO INIT ALL DONE");
init_i2c_slave();
puts("SLAVE INIT DONE");
gpio_init(PICO_DEFAULT_LED_PIN);
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
motors_init();
while(true)
while(robot.is_running)
{
gpio_put(PICO_DEFAULT_LED_PIN, i2c_buffer.buffer[2]);
motors_update();
sleep_ms(10);
robot_update();
}
robot_deinit();
return 0;
}

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

@ -1,14 +1,13 @@
#include "headers/motors.h"
#include "i2c/headers/i2c_slave.h"
#include <pico/stdlib.h>
#include <hardware/pwm.h>
#include <headers/robot.h>
#include "headers/robot.h"
#define MOTOR1_PWM_PIN 9
#define MOTOR2_PWM_PIN 8
#include <stdio.h>
static inline uint get_motor_pwm_pin(motors_enum_t motor)
static inline uint get_motor_pwm_pin(motors_t motor)
{
switch(motor)
{
@ -23,10 +22,16 @@ static inline uint get_motor_pwm_pin(motors_enum_t motor)
}
}
static inline void motor_set_speed(motors_t motor, uint8_t value)
{
uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
}
// Init all motors defined in the MOTORS_DEF array
void motors_init(void)
{
for(motors_enum_t motor = MOTOR1; motor < NB_MOTORS; motor++)
for(motors_t motor = MOTOR1; motor < NB_MOTORS; motor++)
{
// Init PWM
const uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
@ -44,15 +49,8 @@ void motors_init(void)
}
}
// Set [motor] in motor_enum_t at [value] between 0 and 255
void motor_set_speed(motors_enum_t motor, uint8_t value)
{
uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
}
void motors_update(void)
{
motor_set_speed(MOTOR1, get_vitesse_moteur_1());
motor_set_speed(MOTOR2, get_vitesse_moteur_2());
motor_set_speed(MOTOR1, robot.i2c_buffer.buffer[I2C_BUFFER_MOTOR1_SPEED]);
motor_set_speed(MOTOR2, robot.i2c_buffer.buffer[I2C_BUFFER_MOTOR2_SPEED]);
}

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

@ -3,6 +3,7 @@
#include <pico/stdlib.h>
#include <time.h>
#include "headers/motors.h"
#include "headers/servos.h"
#include "i2c/headers/i2c_slave.h"
#include <stdio.h>
@ -17,12 +18,14 @@ void robot_init(void)
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
gpio_put(PICO_DEFAULT_LED_PIN, true);
//motors_init();
motors_init();
servos_init();
init_i2c_slave();
// Initialisation ended
for(uint i = 0, led_state = false; i < 5; i++)
// Initialisation ended, loop forever if initialisation failed
for(uint i = 0, led_state = false; robot.is_running ? (i < 5) : true; i++)
{
gpio_put(PICO_DEFAULT_LED_PIN, led_state);
@ -34,31 +37,23 @@ void robot_init(void)
static inline void update_time(void)
{
static bool led_state = false;
static double last_time = 0.0;
double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC;
static float last_time = 0.0f;
float start_time = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
robot.delta_time_ms = start_time - last_time;
last_time = start_time;
static double elapsed_time = 0.0;
elapsed_time += robot.delta_time_ms;
gpio_put(PICO_DEFAULT_LED_PIN, robot.i2c_buffer.buffer[0]);
}
void robot_handle_inputs_outputs(void)
void robot_update(void)
{
static bool led_state=false;
update_time();
motors_update();
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);
servos_update();
//tight_loop_contents();
tight_loop_contents();
}
void robot_deinit(void)

58
program/motion_controller/src/servos.c Normal file
View File

@ -0,0 +1,58 @@
#include "headers/servos.h"
#include <hardware/pwm.h>
#include <pico/stdlib.h>
#include "headers/robot.h"
#define SERVO1_PWM_PIN 7
#define SERVO2_PWM_PIN 6
#define SERVO3_PWM_PIN 0
static inline uint get_servo_pwm_pin(servos_t servo)
{
switch(servo)
{
case SERVO1:
return SERVO1_PWM_PIN;
case SERVO2:
return SERVO2_PWM_PIN;
case SERVO3:
return SERVO3_PWM_PIN;
default:
return -1;
}
}
static inline void servo_set_speed(servos_t servo, uint16_t pos)
{
const uint SERVO_PWM_PIN = get_servo_pwm_pin(servo);
pwm_set_gpio_level(SERVO_PWM_PIN, pos);
}
void servos_init(void)
{
for(servos_t servo = SERVO1; servo < NB_SERVOS; servo++)
{
// Init PWM
const uint SERVO_PWM_PIN = get_servo_pwm_pin(servo);
const uint SLICE_NUM = pwm_gpio_to_slice_num(SERVO_PWM_PIN);
gpio_set_function(SERVO_PWM_PIN, GPIO_FUNC_PWM);
pwm_set_clkdiv(SLICE_NUM, 100);
pwm_set_wrap(SLICE_NUM, 25000);
pwm_set_enabled(SLICE_NUM, true);
servo_set_speed(servo, 0);
}
}
void servos_update(void)
{
servo_set_speed(SERVO1, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO1_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO1_ANGLE_L]);
servo_set_speed(SERVO2, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO2_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO2_ANGLE_L]);
servo_set_speed(SERVO3, robot.i2c_buffer.buffer[I2C_BUFFER_SERVO3_ANGLE_H] << 8 | robot.i2c_buffer.buffer[I2C_BUFFER_SERVO3_ANGLE_L]);
}