Updated gyro and pico files

program/main_controller/CMakeLists.txt

@@ -17,10 +17,10 @@ pico_sdk_init()
 add_executable(main_controller
   src/main.c
   src/robot.c
-  src/i2c/motors.c
+  src/motors.c
-  src/i2c/gyro.c
   src/motion_control.c
   src/i2c/i2c_master.c
+  src/i2c/gyro.c
   src/i2c/mcp23017.c
   src/wifi/wifi_operator.c
   src/wifi/udp_client.c
@@ -45,6 +45,5 @@ pico_add_extra_outputs(main_controller)
 
 add_custom_target(Flash
     DEPENDS main_controller
-
     COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/main_controller.uf2
 )

program/main_controller/src/headers/motion_control.h

@@ -2,21 +2,16 @@
 #define MOTION_CONTROL_H
 
 #include <stdint.h>
-#include "i2c/headers/motors.h"
 
 #define I2C_MOTION_CONTROLLER_ADDRESS 0x09
 
 typedef struct motion_control_data_t {
-    int16_t angle;
+    int16_t target_speed;
-    int8_t x_axis_speed;
+    float target_angle;
-    int8_t y_axis_speed;
 } 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);
+void motion_control_update(void);
-// Update servo motors from motion control data
-void i2c_update_servo_motors(void);
 
 #endif // MOTION_CONTROL_H

program/main_controller/src/headers/motors.h

@@ -0,0 +1,15 @@
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include <stdint.h>
+
+typedef enum motors_t {
+    MOTOR1,
+    MOTOR2,
+
+    NB_MOTORS
+} motors_t;
+
+void motor_set_dir(motors_t motor, int16_t value);
+
+#endif // MOTORS_H

program/main_controller/src/i2c/gyro.c

@@ -5,23 +5,73 @@
 
 #include "headers/gyro.h"
 
+#include <stdio.h>
 #include <stdint.h>
 #include "headers/i2c_master.h"
 #include "headers/robot.h"
 
 // #include <stdio.h>
 
-#define SAMPLE_MIN_ELAPSED_TIME_MS 1.0f
+#define SAMPLE_MIN_ELAPSED_TIME_MS 2.0f
-#define DPS_PER_DIGIT 0.00875f
+#define DPS_PER_DIGIT 1000.0f / 65535.0f * 2.0f // FS_SEL = 2 // Why * 2 ? Idk it works...
 
-void gyro_init(void)
+int gyro_init(void)
 {
     // Reset gyro
     const uint8_t PWR_MGMT_REG = 0x6B;
     const uint8_t PWR_MGMT_CONFIG = 0b10000000; // Reset bit set to 1
-    uint8_t buf[] = {PWR_MGMT_REG, PWR_MGMT_CONFIG};
-    i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 2, false);
 
+    uint8_t buf[2] = {PWR_MGMT_REG, PWR_MGMT_CONFIG};
+    if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 2, false) == PICO_ERROR_GENERIC)
+    {
+        puts("Gyro is not reachable during initialisation");
+        return -1;
+    }
+
+    sleep_ms(10);
+
+    // Remove reset bit
+    buf[1] = 0x00;
+    if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 2, false) == PICO_ERROR_GENERIC)
+    {
+        puts("Gyro is not reachable during inititalisation");
+        return -1;
+    }
+
+    sleep_ms(10);
+
+    // Verify device identification
+    const uint8_t WHO_AM_I_REG = 0x75;
+    buf[0] = WHO_AM_I_REG;
+    if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 1, true) == PICO_ERROR_GENERIC)
+    {
+        puts("Gyro is not reachable during initialisation");
+    }
+    if(i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 1, false) == PICO_ERROR_GENERIC)
+    {
+        puts("Gyro is not reachable during initialisation");
+        return -1;
+    }
+
+    if(buf[0] != 0x68)
+    {
+        puts("Device is not the expected gyro");
+        printf("Have : 0x%x\nExpected : 0x68\n", buf[0]);
+        return -1;
+    }
+
+    // Write gyro config
+    const uint8_t GYRO_CONFIG_REG = 0x1B;
+    const uint8_t GYRO_CONFIG = 0b00010000; // FS_SEL = 2
+    buf[0] = GYRO_CONFIG_REG;
+    buf[1] = GYRO_CONFIG;
+    if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, buf, 2, false) == PICO_ERROR_GENERIC)
+    {
+        puts("Gyro is not reachable during initialisation");
+        return -1;
+    }
+
+    // Reset gyro data
     robot.gyro_data.x_offset = 0.0f;
     robot.gyro_data.y_offset = 0.0f;
     robot.gyro_data.z_offset = 0.0f;
@@ -30,20 +80,27 @@ void gyro_init(void)
     robot.gyro_data.y_angle = 0.0f;
     robot.gyro_data.z_angle = 0.0f;
 
-    sleep_ms(100);
+    return 0;
 }
 
-static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t *y, int16_t *z)
+static inline void __attribute__((always_inline)) gyro_read_raw(int16_t *x, int16_t *y, int16_t *z)
 {
     const uint8_t GYRO_XOUT_H_REG = 0x43;
 
     uint8_t data[6];
-    i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, &GYRO_XOUT_H_REG, 1, true);
+    if(i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, &GYRO_XOUT_H_REG, 1, true) == PICO_ERROR_GENERIC)
-    i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, data, 6, false);
+    {
+        puts("Gyro is not reachable");
+    }
 
-    *x = (int16_t)((data[1] << 8) | data[0]);
+    if(i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, data, 6, false) == PICO_ERROR_GENERIC)
-    *y = (int16_t)((data[3] << 8) | data[2]);
+    {
-    *z = (int16_t)((data[5] << 8) | data[4]);
+        puts("Gyro is not reachable");
+    }
+
+    *x = (int16_t)((data[0] << 8) | data[1]);
+    *y = (int16_t)((data[2] << 8) | data[3]);
+    *z = (int16_t)((data[4] << 8) | data[5]);
 }
 
 void gyro_calibrate(void)
@@ -55,13 +112,13 @@ void gyro_calibrate(void)
 
     for(uint i = 0; i < NB_SAMPLES; i++)
     {
-        gyro_read(&x, &y, &z);
+        gyro_read_raw(&x, &y, &z);
 
         x_sum += x;
         y_sum += y;
         z_sum += z;
 
-        sleep_us(SAMPLE_MIN_ELAPSED_TIME_MS * 1000);
+        sleep_ms(SAMPLE_MIN_ELAPSED_TIME_MS);
 
         //printf(">cal_x:%d\n", x);
         //printf(">cal_y:%d\n", y);
@@ -80,7 +137,7 @@ void gyro_calibrate(void)
 static inline void __attribute__((always_inline)) gyro_get_dps(float *x_dps, float *y_dps, float *z_dps)
 {
     int16_t x, y, z;
-    gyro_read(&x, &y, &z);
+    gyro_read_raw(&x, &y, &z);
 
     *x_dps = x * DPS_PER_DIGIT;
     *y_dps = y * DPS_PER_DIGIT;

program/main_controller/src/i2c/headers/gyro.h

@@ -1,7 +1,7 @@
 #ifndef GYRO_H
 #define GYRO_H
 
-#define I2C_GYRO_ADDRESS 0x6B
+#define I2C_GYRO_ADDRESS 0x68
 
 typedef struct gyro_data_t {
     float x_offset, y_offset, z_offset;
@@ -9,7 +9,7 @@ typedef struct gyro_data_t {
 } gyro_data_t;
 
 // Check if gyro has correctly initialised and configure it for simple use
-void gyro_init(void);
+int gyro_init(void);
 // Calibrate gyro
 void gyro_calibrate(void);
 // Update gyro data

program/main_controller/src/i2c/headers/i2c_master.h

@@ -6,7 +6,7 @@
 #define I2C_MASTER_SDA_PIN 16
 #define I2C_MASTER_SCL_PIN 17
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_BAUD_RATE 200 * 1000
+#define I2C_MASTER_BAUD_RATE 300 * 1000
 
 // Init master i2c
 void i2c_master_init(void);

program/main_controller/src/i2c/headers/motors.h

@@ -1,49 +0,0 @@
-#ifndef MOTORS_H
-#define MOTORS_H
-
-#include <pico/types.h>
-
-// 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

program/main_controller/src/i2c/motors.c

@@ -1,35 +0,0 @@
-#include "headers/motors.h"
-
-#include <stdlib.h>
-#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);
-}

program/main_controller/src/motion_control.c

@@ -2,66 +2,100 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "i2c/headers/motors.h"
+#include <hardware/i2c.h>
+#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.target_speed = 0;
-    robot.motion_control_data.x_axis_speed = 0;
+    robot.motion_control_data.target_angle = 0.0f;
-    robot.motion_control_data.y_axis_speed = 0;
 }
 
-void i2c_update_motion_control(void)
+void motion_control_update(void)
 {
-    // Motion control work as follow :
+    // First we update motion control data from received data
-    //  - Motors are rotated on-board at 45*.
+    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;
-    //  - we calculate the error of the targeted angle and the actual angle
+    robot.motion_control_data.target_speed =  robot.udp_client.data.hard.inputs.joystick_x;
-    //  - 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
 
-    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;
+        if(fabsf(angle_offset) < 45.0f)
-    float target_angle_radian = target_angle / 180.0f * M_PI;
+        {
+            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;
+    // Wrap angle
-    if(error > 180) error -= 360;
+    while(robot.motion_control_data.target_angle >  180.0f) robot.motion_control_data.target_angle -= 360.0f;
-    if(error < -180) error += 360;
+    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 +
+    float motor1_speed = (float)robot.motion_control_data.target_speed - correction;
-                                    sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed;
+    float motor2_speed = (float)robot.motion_control_data.target_speed + correction;
-    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;
 
-    int motor1_speed = target_x_axis_speed + (int)correction;
+    if(motor1_speed > 255) motor1_speed = 255;
-    int motor2_speed = target_x_axis_speed - (int)correction;
+    if(motor2_speed > 255) motor2_speed = 255;
-    int motor3_speed = target_y_axis_speed + (int)correction;
-    int motor4_speed = target_y_axis_speed - (int)correction;
 
-    if(motor1_speed > 127) motor1_speed = 127;
+    if(motor1_speed < -255) motor1_speed = -255;
-    if(motor2_speed > 127) motor2_speed = 127;
+    if(motor2_speed < -255) motor2_speed = -255;
-    if(motor3_speed > 127) motor3_speed = 127;
-    if(motor4_speed > 127) motor4_speed = 127;
 
-    if(motor1_speed < -128) motor1_speed = -128;
+    // Set dir pins
-    if(motor2_speed < -128) motor2_speed = -128;
+    motor_set_dir(MOTOR1, (int16_t)motor1_speed);
-    if(motor3_speed < -128) motor3_speed = -128;
+    motor_set_dir(MOTOR2, (int16_t)motor2_speed);
-    if(motor4_speed < -128) motor4_speed = -128;
 
-    i2c_set_motor(MOTOR1, motor1_speed);
+    static float elapsed_time_ms = 0.0f;
-    i2c_set_motor(MOTOR2, motor2_speed);
+    elapsed_time_ms += robot.delta_time_ms;
-    i2c_set_motor(MOTOR3, motor3_speed);
-    i2c_set_motor(MOTOR4, motor4_speed);
-}
 
-void i2c_update_servo_motors(void)
+    if(elapsed_time_ms >= MSG_DELAY_MS)
     {
-    //for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
+        // Send motors speed via I2C
-    //    i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]);
+        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, &reg, 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");
+        }
+    }
 }

program/main_controller/src/motors.c

@@ -0,0 +1,47 @@
+#include "headers/motors.h"
+
+#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
+
+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);
+    }
+}

program/main_controller/src/robot.c

@@ -19,15 +19,16 @@ void robot_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
 
-    //i2c_master_init();
+    i2c_master_init();
 
-    //mcp23017_init();
+    mcp23017_init();
 
-    //gyro_init();
+    if(gyro_init())
+        robot.is_running = false;
 
-    //gyro_calibrate();
+    gyro_calibrate();
 
-    //motion_control_init();
+    motion_control_init();
 
     if(wifi_operator_init())
         robot.is_running = false;
@@ -74,9 +75,7 @@ void robot_handle_inputs_outputs(void)
 
     gyro_update();
 
-    //update_motion_control();
+    motion_control_update();
-
-    //i2c_update_servo_motors();
 
     mcp23017_update();
 

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

program/main_controller/src/wifi/udp_client.c

@@ -1,11 +1,15 @@
 #include "headers/udp_client.h"
 
 #include <stdio.h>
+#include <string.h>
 #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 = message_callback;
-    robot.udp_client.message_callback = default_message_callback;
+    //robot.udp_client.message_callback = default_message_callback;
 
     robot.udp_client.pcb = udp_new();
     if(robot.udp_client.pcb == NULL)

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

program/motion_controller/pico_sdk_import.cmake

@@ -3,6 +3,28 @@
 # This can be dropped into an external project to help locate this SDK
 # It should be include()ed prior to project()
 
+# Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd.
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
+# following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
+# disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
+# derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
 if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
     set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
     message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
@@ -18,9 +40,20 @@ if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_P
     message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
 endif ()
 
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
+    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
+    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
+endif ()
+
+if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
+  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
+  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
+endif()
+
 set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
 set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
 set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
 
 if (NOT PICO_SDK_PATH)
     if (PICO_SDK_FETCH_FROM_GIT)
@@ -32,11 +65,37 @@ if (NOT PICO_SDK_PATH)
         FetchContent_Declare(
                 pico_sdk
                 GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                GIT_TAG master
+                GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
         )
+
         if (NOT pico_sdk)
             message("Downloading Raspberry Pi Pico SDK")
-            FetchContent_Populate(pico_sdk)
+            # GIT_SUBMODULES_RECURSE was added in 3.17
+            if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+                FetchContent_Populate(
+                        pico_sdk
+                        QUIET
+                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+                        GIT_SUBMODULES_RECURSE FALSE
+
+                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
+                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
+                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
+                )
+            else ()
+                FetchContent_Populate(
+                        pico_sdk
+                        QUIET
+                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+
+                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
+                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
+                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
+                )
+            endif ()
+
             set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
         endif ()
         set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})

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 {
+#define MOTOR1_PWM_PIN 9
-    uint pwm_pin;
+#define MOTOR2_PWM_PIN 8
-    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[];
 
 // Init all motors defined in the MOTORS_DEF array
-void init_motors(void);
+void motors_init(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);
 // Set [motor] in motor_enum_t at [value] between -127 and 128
-void motor_set(motors_enum_t motor, int8_t value);
+void motor_set_speed(motors_enum_t motor, uint8_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);
-
 // Update motors from the data in the i2c buffer
-void update_motors_from_buffer(void);
+void motors_update(void);
-// Update servo motors from the data in the i2c buffer
-void update_servo_motors_from_buffer(void);
 
 #endif // MOTORS_H

program/motion_controller/src/i2c/headers/i2c_slave.h

@@ -1,9 +1,3 @@
-/*
- * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
- *
- * 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;

program/motion_controller/src/i2c/i2c_slave.c

@@ -1,9 +1,3 @@
-/*
- * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
- *
- * SPDX-License-Identifier: MIT
- */
-
 #include "headers/i2c_slave.h"
 
 #include <hardware/gpio.h>
@@ -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;
 

program/motion_controller/src/motors.c

@@ -4,137 +4,50 @@
 #include <hardware/pwm.h>
 #include <headers/robot.h>
 
-const motor_def_t MOTORS_DEFS[] = {
+#include <stdio.h>
-    {0,  4,  5, 0x00},
-    {1,  6,  7, 0x01},
-    {2,  9,  8, 0x02},
-    {3, 11, 10, 0x03},
-};
 
-const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
+static inline uint get_motor_pwm_pin(motors_enum_t motor)
-    {12, 0, 25000, 0x04},
+{
-    {13, 0, 25000, 0x05},
+    switch(motor)
-    {14, 0, 25000, 0x06},
+    {
-    {15, 0, 25000, 0x07},
+      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);
+        const uint SLICE_NUM = pwm_gpio_to_slice_num(MOTOR_PWM_PIN);
-        pwm_set_wrap(SLICE_NUM, 128);
+
+        gpio_set_function(MOTOR_PWM_PIN, GPIO_FUNC_PWM);
+        pwm_set_wrap(SLICE_NUM, 256);
         pwm_set_enabled(SLICE_NUM, true);
 
-        // Init dir pins
+        motor_set_speed(motor, 0);
-        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);
     }
 }
 
-// Init all servo motors defined in the SERVO_MOTORS_DEF array
+// Set [motor] in motor_enum_t at [value] between 0 and 255
-void init_servo_motors(void)
+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++)
+    uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
+    pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
+}
+
+void motors_update(void)
 {
-        const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
+    motor_set_speed(MOTOR1, robot.i2c_buffer.buffer.hard.motor1_speed);
-
+    motor_set_speed(MOTOR2, robot.i2c_buffer.buffer.hard.motor2_speed);
-        // 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);
-    }
-}
-
-// Set [motor] to 0
-void motor_zero(motors_enum_t motor)
-{
-    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]);
-    }
 }

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();
+    motors_init();
-    //init_servo_motors();
+
     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)