main controller code/.vscode/settings.json

@@ -2,7 +2,6 @@
     "files.associations": {
         "binary_info.h": "c",
         "i2c.h": "c",
-        "stdlib.h": "c",
+        "stdlib.h": "c"
-        "stdint.h": "c"
     }
 }

main controller code/CMakeLists.txt

@@ -12,13 +12,6 @@ pico_sdk_init()
 
 add_executable(motion_controller
   src/main.c
-  src/robot.c
-  src/motors.c
-  src/gyro.c
-  src/motion_control.c
-  src/i2c_master.c
-  src/udp_client.c
-  src/udp_buffer.c
 )
 
 target_link_libraries(motion_controller

main controller code/Readme.md

@@ -12,7 +12,7 @@ The robot’s I2C communication works as follows:
  * Send the target register address (to read from or write to).
  * Read or write the register data. Multiple registers can be read/written sequentially, with the address auto-incrementing after each operation.
 
-This code is designed to be the master in the i2c communication.
+This code is designed to be the master in the i2c communication;
 
 |Adress |R/W|Description                    |Encoding           |
 |-------|:-:|-------------------------------|:-----------------:|
@@ -52,12 +52,3 @@ 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.
-
-
-Pinout description
------------------------------------------------
-
-|Pin |Description                       |GPIO Type  |
-|----|----------------------------------|-----------|
-| 4  | I2C Bus SDA                      | I2C       |
-| 5  | I2C Bus SCL                      | I2C       |

main controller code/src/gyro.c

@@ -1 +0,0 @@
-#include "include/gyro.h"

main controller code/src/i2c_master.c

@@ -1,17 +0,0 @@
-#include "include/i2c_master.h"
-
-inline void i2c_master_send(uint8_t address, const uint8_t *src, size_t len)
-{
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, src, len, false, I2C_MASTER_MAX_TIMEOUT);
-}
-
-inline void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len)
-{
-    i2c_read_blocking_until(I2C_MASTER_INSTANCE, address, dst, len, false, I2C_MASTER_MAX_TIMEOUT);
-}
-
-inline void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len)
-{
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, data, len, true, I2C_MASTER_MAX_TIMEOUT);
-    i2c_master_receive(address, data, len);
-}

main controller code/src/include/gyro.h

@@ -1,6 +0,0 @@
-#ifndef GYRO_H
-#define GYRO_H
-
-#define I2C_GYRO_ADDRESS 0x6b
-
-#endif // GYRO_H

main controller code/src/include/i2c_master.h

@@ -1,18 +0,0 @@
-#ifndef I2C_MASTER_H
-#define I2C_MASTER_H
-
-#include <hardware/i2c.h>
-
-#define I2C_MASTER_SDA_PIN 4
-#define I2C_MASTER_SCL_PIN 5
-#define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_MAX_TIMEOUT 10000
-
-// Send [src] of [len] to [address] and close communication
-void i2c_master_send(uint8_t address, const uint8_t *src, size_t len);
-// Receive [dst] of [len] from [address] and close communication
-void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len);
-// Send [data] of [len] to [address], receive [data] of [len] from [address] and close communication
-void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len);
-
-#endif // I2C_MASTER_H

main controller code/src/include/motors.h

@@ -1,10 +1,6 @@
 #ifndef MOTORS_H
 #define MOTORS_H
 
-#include <pico/types.h>
-
-#define I2C_MOTION_CONTROLLER_ADDRESS 0x09
-
 // Motors
 typedef enum motors_enum_t {
     MOTOR1,
@@ -43,9 +39,17 @@ typedef struct {
 
 extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
 
-// Send [motor] to [value] through i2c to motion controller
+// Init all motors defined in the MOTORS_DEF array
-void i2c_set_motor_speed(motors_enum_t motor, int8_t value);
+void init_motors(void);
-// Get [motor] speed from motion controller through i2c
+// Init all servo motors defined in the SERVO_MOTORS_DEF array
-int8_t i2c_get_motor_speed(motors_enum_t motor);
+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 (for this config)
+void motor_set(motors_enum_t motor, int 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);
 
 #endif // MOTORS_H

main controller code/src/include/robot.h

@@ -1,8 +1,6 @@
 #ifndef ROBOT_H
 #define ROBOT_H
 
-#include <stdbool.h>
-
 typedef struct robot_t {
     bool is_running;
     double delta_time_ms;

main controller code/src/include/udp_buffer.h

@@ -1,4 +0,0 @@
-#ifndef UDP_BUFFER_H
-#define UDP_BUFFER_H
-
-#endif // UDP_BUFFER_H

main controller code/src/include/udp_client.h

@@ -1,4 +0,0 @@
-#ifndef UDP_CLIENT_H
-#define UDP_CLIENT_H
-
-#endif // UDP_CLIENT_H

main controller code/src/motors.c

@@ -1,7 +1,7 @@
-#include "include/motors.h"
+#include <pico/stdlib.h>
+#include <hardware/pwm.h>
 
-#include <stdlib.h>
+#include "include/motors.h"
-#include "include/i2c_master.h"
 
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
@@ -17,24 +17,103 @@ const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
     {15, 0, 25000, 0x07},
 };
 
-void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
+// Init all motors defined in the MOTORS_DEF array
+void init_motors(void)
+{
+    for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
+    {
+        const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
+
+        // Init PWM
+        uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
+
+        gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
+        pwm_set_wrap(slice_num, 128);
+        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);
+    }
+}
+
+// Init all servo motors defined in the SERVO_MOTORS_DEF array
+void init_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];
+
+        //  Init PWM   //
+        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];
 
-    uint8_t x = *(uint8_t *)&value;
+    // Set PWM to zero
-    uint8_t data[2] = {motor_def->buffer_address, x};
+    pwm_set_gpio_level(motor_def->pwm_pin, 0);
 
-    i2c_master_send(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
+    // Set dir pins to false
+    gpio_put(motor_def->dir1_pin, false);
+    gpio_put(motor_def->dir2_pin, false);
 }
 
-int8_t i2c_get_motor_speed(motors_enum_t motor)
+// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
+void motor_set(motors_enum_t motor, int value)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
-    uint8_t data[1] = {motor_def->buffer_address};
+    if(value < 0)
+    {
+        gpio_put(motor_def->dir1_pin, true);
+        gpio_put(motor_def->dir2_pin, false);
 
-    i2c_master_send_receive(I2C_MOTION_CONTROLLER_ADDRESS, data, 1);
+        value = -value;
-
+    }
-    int8_t value = *(int8_t *)data;
+    else if(value > 0)
-    return value;
+    {
+        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);
 }

main controller code/src/robot.c

@@ -1,18 +1,26 @@
-#include "include/robot.h"
-
 #include <pico/stdlib.h>
+#include "include/motors.h"
+#include "include/i2c_slave.h"
+
+#include "include/robot.h"
 
 void robot_init(void)
 {
     stdio_init_all();
 
+    init_motors();
+    init_servo_motors();
+    i2c_slave_init();
+
     robot.is_running = true;
 }
 
 void robot_handle_inputs_outputs(void)
 {
+    update_motors_from_buffer();
 }
 
 void robot_deinit(void)
 {
+    i2c_slave_deinit();
 }

main controller code/src/udp_buffer.c

@@ -1 +0,0 @@
-#include "include/udp_buffer.h"

main controller code/src/udp_client.c

@@ -1 +0,0 @@
-#include "include/udp_client.h"

motion controller code/.vscode/settings.json

@@ -7,7 +7,6 @@
         "stdlib.h": "c",
         "robot.h": "c",
         "stdio.h": "c",
-        "i2c_buffer.h": "c",
+        "i2c_buffer.h": "c"
-        "motors.h": "c"
     }
 }

motion controller code/Readme.md

@@ -52,28 +52,3 @@ To control a servo motor you need to write data to its adress of the form :
 >```
 
 Value is 0 or 1 for the open pos or the close pos.
-
-
-Pinout description
------------------------------------------------
-
-|Pin |Description                       |GPIO Type  |
-|----|----------------------------------|-----------|
-| 0  | Motor1 Speed                     | PWM       |
-| 1  | Motor2 Speed                     | PWM       |
-| 2  | Motor3 Speed                     | PWM       |
-| 3  | Motor4 Speed                     | PWM       |
-| 4  | Motor1 Dir1                      | OUTPUT    |
-| 5  | Motor1 Dir2                      | OUTPUT    |
-| 6  | Motor2 Dir1                      | OUTPUT    |
-| 7  | Motor2 Dir2                      | OUTPUT    |
-| 8  | Motor3 Dir1                      | OUTPUT    |
-| 9  | Motor3 Dir2                      | OUTPUT    |
-| 10 | Motor4 Dir1                      | OUTPUT    |
-| 11 | Motor5 Dir2                      | OUTPUT    |
-| 12 | Servo1 Angle                     | PWM       |
-| 13 | Servo2 Angle                     | PWM       |
-| 14 | Servo3 Angle                     | PWM       |
-| 15 | Servo4 Angle                     | PWM       |
-| 21 | I2C Bus SDA                      | I2C       |
-| 20 | I2C Bus SCL                      | I2C       |

motion controller code/src/i2c_buffer.c

@@ -1,8 +1,8 @@
-#include "include/i2c_buffer.h"
-
 #include "include/robot.h"
 #include "include/motors.h"
 
+#include "include/i2c_buffer.h"
+
 void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event)
 {
     switch(event)

motion controller code/src/i2c_slave.c

@@ -4,12 +4,12 @@
  * SPDX-License-Identifier: MIT
  */
 
-#include "include/i2c_slave.h"
-
 #include <pico/stdlib.h>
 #include <hardware/irq.h>
 #include "include/i2c_buffer.h"
 
+#include "include/i2c_slave.h"
+
 static bool transfer_in_progress;
 
 static inline void finish_transfer(void)

motion controller code/src/include/motors.h

@@ -1,8 +1,6 @@
 #ifndef MOTORS_H
 #define MOTORS_H
 
-#include <pico/stdlib.h>
-
 // Motors
 typedef enum motors_enum_t {
     MOTOR1,
@@ -47,8 +45,8 @@ void init_motors(void);
 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
+// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config)
-void motor_set(motors_enum_t motor, int8_t value);
+void motor_set(motors_enum_t motor, int 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

motion controller code/src/motors.c

@@ -1,8 +1,8 @@
-#include "include/motors.h"
-
 #include <pico/stdlib.h>
 #include <hardware/pwm.h>
 
+#include "include/motors.h"
+
 const motor_def_t MOTORS_DEFS[] = {
     {0,  4,  5, 0x00},
     {1,  6,  7, 0x01},
@@ -75,7 +75,7 @@ void motor_zero(motors_enum_t motor)
 }
 
 // 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)
+void motor_set(motors_enum_t motor, int value)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 

motion controller code/src/robot.c

@@ -1,9 +1,9 @@
-#include "include/robot.h"
-
 #include <pico/stdlib.h>
 #include "include/motors.h"
 #include "include/i2c_slave.h"
 
+#include "include/robot.h"
+
 void robot_init(void)
 {
     stdio_init_all();