Add first I2C comms

src/motor-esp32/i2c_comms.cpp

@@ -0,0 +1,63 @@
+#include "Arduino.h"
+#include "Wire.h"
+#include "io.h"
+#include "motor_control.h"
+#include "roller_speed.h"
+
+#include "../common/shared_structs.h"
+
+#define MOTOR_ESP32_SLAVE_I2C_ADDR 0x55
+#define I2C_FREQUENCY 100000
+
+
+
+i2c_status_packet_t i2c_reply;
+
+void on_I2C_request() {
+    // Hopefully because most of our data is pulled in with queues, rather than larger locks,
+    // assembling the reply won't take too long.
+    
+    i2c_reply.roller_1_speed = get_roller_speed(0);
+    i2c_reply.roller_2_speed = get_roller_speed(1);
+    i2c_reply.roller_3_speed = get_roller_speed(2);
+    i2c_reply.roller_1_revs = get_roller_revs(0);
+    i2c_reply.roller_2_revs = get_roller_revs(1);
+    i2c_reply.roller_3_revs = get_roller_revs(2);
+    i2c_reply.motor_1_duty = get_motor_duty(0);
+    i2c_reply.motor_2_duty = get_motor_duty(1);
+    i2c_reply.motor_3_duty = get_motor_duty(2);
+    i2c_reply.motor_1_settings = get_motor_settings(0);
+    i2c_reply.motor_2_settings = get_motor_settings(1);
+    i2c_reply.motor_3_settings = get_motor_settings(2);
+    i2c_reply.motor_1_params = get_motor_parameters(0);
+    i2c_reply.motor_2_params = get_motor_parameters(1);
+    i2c_reply.motor_3_params = get_motor_parameters(2);
+
+    Wire.write((byte *)&i2c_reply, sizeof i2c_reply);
+}
+
+void on_I2C_receive(int len){
+
+}
+
+void i2c_comms_setup(void) {
+    // Set here and wait for requests from the i2C master (the other ESP32).
+    /* Data to go back:
+    - roller speeds (1,2,3)
+    - roller revs (1,2,3)
+    - motor desired speeds
+    - motor duty cycles
+    - motor IS voltages
+    - system voltage
+    - motor parameters
+    - motor settings
+
+    Data to come in:
+    - motor settings (enabled, desired speed) 1, 2, 3
+    - motor control parameters (kp, ki, kd, min duty)
+    */
+
+    Wire.onRequest(on_I2C_request);
+    Wire.onReceive(on_I2C_receive);
+    Wire.begin((uint8_t)MOTOR_ESP32_SLAVE_I2C_ADDR, GPIO_I2C_SDA, GPIO_I2C_SCL, (uint32_t)I2C_FREQUENCY);
+}

src/motor-esp32/i2c_comms.h

@@ -0,0 +1,7 @@
+#ifndef i2c_comms_h
+#define i2c_comms_h
+
+
+void i2c_comms_setup(void);
+
+#endif

src/motor-esp32/io.h

@@ -16,7 +16,7 @@
 #define GPIO_ROLLER3_IS    GPIO_NUM_34
 #define GPIO_SYS_VOLTAGE   GPIO_NUM_35
 
-#define GPIO_SDA GPIO_NUM_21
-#define GPIO_SCL GPIO_NUM_22
+#define GPIO_I2C_SDA GPIO_NUM_21
+#define GPIO_I2C_SCL GPIO_NUM_22
 
 #endif

src/motor-esp32/main.cpp

@@ -2,6 +2,7 @@
 
 #include "motor_control.h"
 #include "roller_speed.h"
+#include "i2c_comms.h"
 
 void setup() {
     Serial.setTimeout(1);
@@ -9,11 +10,17 @@ void setup() {
 
     roller_speed_setup();
     motor_control_setup();
+    i2c_comms_setup();
 }
 
+motor_settings_t m_set;
+
 void loop() {
     delay(500);
-    Serial.print("Rsp:");
+    Serial.print("Dsr:");
+    m_set = get_motor_settings(0);
+    Serial.print(m_set.desired_speed);
+    Serial.print(" Rsp:");
     Serial.print(get_roller_speed(0));
     Serial.print(",");
     Serial.print(get_roller_speed(1));
@@ -33,9 +40,8 @@ void loop() {
     // Serial.print(", ");
     // Serial.println(get_roller_speed(0));
 
-    motor_settings_t m_set;
     m_set.enabled = true;
-    m_set.desired_speed = ((millis() % 5000) / 5000.0) * 0.65;
+    m_set.desired_speed = ((millis() % 10000) / 10000.0) * 0.65;
     set_motor_settings(0,m_set);
     m_set.desired_speed = 0.3;
     set_motor_settings(1,m_set);

src/primary-esp32/io.h

@@ -0,0 +1,9 @@
+#ifndef io_h
+#define io_h
+
+// Many of the ESP-IDF functions require pins from the gpio_num_t enum
+
+#define GPIO_I2C_SDA GPIO_NUM_21
+#define GPIO_I2C_SCL GPIO_NUM_22
+
+#endif

src/primary-esp32/main.cpp

@@ -1,13 +1,40 @@
 #include <Arduino.h>
+#include "Wire.h"
+#include "io.h"
+#include "../common/shared_structs.h"
+
+#define MOTOR_ESP32_SLAVE_I2C_ADDR 0x55
+#define I2C_FREQUENCY 100000
 
 #define MOTOR_SPEED_SENSE 5
 
 void setup() {
+  Wire.begin(GPIO_I2C_SDA, GPIO_I2C_SCL, (uint32_t)I2C_FREQUENCY);
+
   Serial.setTimeout(1);
   Serial.begin(57600);
 }
 
+uint8_t bytecount;
+i2c_status_packet_t i2c_reply;
+
 void loop() {
-  delay(1000);
-  Serial.println("PrimaryESP32");
+  delay(100);
+  bytecount = Wire.requestFrom(MOTOR_ESP32_SLAVE_I2C_ADDR, sizeof i2c_reply);
+  if(bytecount == sizeof i2c_reply){
+    Wire.readBytes((byte*)&i2c_reply, bytecount);
+
+    Serial.print("Dsr:");
+    Serial.print(i2c_reply.motor_1_settings.desired_speed);
+    Serial.print(" Rsp:");
+    Serial.print(i2c_reply.roller_1_speed);
+    Serial.print("   Err:");
+    Serial.print(i2c_reply.motor_1_settings.desired_speed-i2c_reply.roller_1_speed);
+    Serial.print(" Mdty:");
+    Serial.println(i2c_reply.motor_1_duty);
+
+  } else {
+    Serial.println(bytecount);
+  }
+  
 }