Added support to multiple reads in arduino controller

Marco-GG · Marco-GG · commit 6ff348ff60d6 · 2016-11-20T19:11:21.000-03:00
The arduino controller can return N readings of a pin (configured using the set_mode function of the protocol).
diff --git a/MLC/arduino/Firmware/Firmware.ino b/MLC/arduino/Firmware/Firmware.ino
@@ -1,5 +1,5 @@
 
-#define DEBUG 0
+#define DEBUG 1
 #if DEBUG
 #define LOG(x,y) Serial.print(x); Serial.println(y);
 #else
@@ -8,19 +8,20 @@
 
 // defines ahorran ROM... aunque nos sobra de eso XD -- Verificar que los const vayan a la rom!
 const uint8_t ANALOG_PRECISION_CMD  = 0x01;
-const uint8_t ADD_INPUT_PORT_CMD  = 0x02;
-const uint8_t ADD_OUTPUT_PORT_CMD = 0x03;
+const uint8_t ADD_INPUT_PIN_CMD     = 0x02;
+const uint8_t ADD_OUTPUT_PIN_CMD    = 0x03;
 const uint8_t SET_PIN_MODE_CMD      = 0x04;
 const uint8_t SET_REPORT_MODE_CMD   = 0x05;
 const uint8_t ACTUATE_CMD           = 0xF0;
+const uint8_t RESET_PINS            = 0xFE;
 
 // Commands executor caller -- one vector position == one command
 int (*executor[255])(const char*);
 
 int not_implemented(const char* data)
 {
   //nothing to do...
-  return 0;
+  return 1;
 }
 
 typedef enum ReportModes {average, bulk, rt};
@@ -32,6 +33,8 @@ uint8_t REPORT_READ_COUNT = 0;
 uint8_t REPORT_READ_DELAY = 0;
 ReportModes REPORT_MODE = average;
 uint8_t INPUT_PORTS[129]; // Port count in first position
+uint8_t ANALOG_PINS_COUNT = 0;
+uint8_t DIGITAL_PINS_COUNT = 0;
 
 /** ---------------------------------------------------------- **/
 
@@ -85,14 +88,41 @@ int set_analog_input(const char* data)
 {
   INPUT_PORTS[0] += 1;
   INPUT_PORTS[INPUT_PORTS[0]] = byte(data[2]);
-  LOG("New analog input: ", byte(data[2]));
+
+  if ( INPUT_PORTS[INPUT_PORTS[0]] >= A0 )
+  {
+    ANALOG_PINS_COUNT++;
+    LOG("New analog input: ", byte(data[2]));
+  } else
+  {
+    DIGITAL_PINS_COUNT++;
+    LOG("New digital input: ", byte(data[2]));
+  }
 
   return 3; // Command of 3 bytes
 }
+/**
+   RESET: 0xFF
+*/
+int reset(const char* data)
+{
+  for ( int i = 1; i <= byte(INPUT_PORTS[0]); i++)
+  {
+    pinMode(INPUT_PORTS[i], OUTPUT);
+  }
+
+  INPUT_PORTS[0] = 0;
+  ANALOG_PINS_COUNT = 0;
+  DIGITAL_PINS_COUNT = 0;
+
+  LOG("System reset executed", "");
+
+  return 1;
+}
 
 /**
- * ANALOG_OUTPUT: 0x03 0x01 [PORT]
- */
+   ANALOG_OUTPUT: 0x03 0x01 [PORT]
+*/
 int set_analog_output(const char* data)
 {
   // No se si vale la pena guardar registro de pines de salida...
@@ -101,13 +131,16 @@ int set_analog_output(const char* data)
 }
 
 /**
- * ACTUATE: 0xF0 [DATA_LEN] [PIN_A] [VALUE_PIN_A] ... [PIN_N] [VALUE_PIN_N]
- */
+   ACTUATE: 0xF0 [DATA_LEN] [PIN_A] [VALUE_PIN_A] ... [PIN_N] [VALUE_PIN_N]
+*/
 int actuate(const char* data)
 {
   int offset = 0;
   int byte_count = byte(data[1]); // Un byte puede llegar a limitar la cantidad de salidas... creo
 
+  uint8_t digital_input_buffer[DIGITAL_PINS_COUNT][REPORT_READ_COUNT / 8 + 2]; // 255 lectures of 1 bit for every digital pin -- 1 extra byte for the port address
+  uint8_t analog_input_buffer[ANALOG_PINS_COUNT][(2 * REPORT_READ_COUNT) + 2]; // 255 lectures of 2 bytes for every analog pin -- 1 extra byte for the port address
+
   LOG("Actutating over payload of size: ", byte_count);
 
   // ACTUATION ZONE
@@ -138,42 +171,104 @@ int actuate(const char* data)
 
   char response[130];
   response[0] = '\xF1';
-  byte len = 1;  // Inicia en 1 para evitar pisar el id de comando
-  for (int i = 1; i <= byte(INPUT_PORTS[0]); i++)
+  uint16_t len = 0;  // Inicia en 1 para evitar pisar el id de comando
+
+  // Resets of digital buffers (required due the buffering strategy for digital pins)
+  memset(digital_input_buffer, 0, DIGITAL_PINS_COUNT * (REPORT_READ_COUNT / 8 + 2));
+
+  for (int i = 0; i < DIGITAL_PINS_COUNT; i++)
   {
-    response[len + 1] = INPUT_PORTS[i];
-    if ( INPUT_PORTS[i] >= A0 )
+    for (int j = 0; j < REPORT_READ_COUNT / 8 + 1; j++)
     {
-      int data = analogRead(INPUT_PORTS[i]-A0);
-      response[len + 2] = byte((data & 0xFF00) >> 8); // Se guarda el msb en el buffer
-      response[len + 3] = byte(data & 0xFF);        // Se guarda el lsb en el buffer
+      Serial.print(digital_input_buffer[i][j], HEX);
+    }
+    Serial.println("");
+  }
 
-      len += 3; // Cada lectura de un recurso analógico ocupa dos bytes. FIXME: se puede optimizar con bajas resoluciones
-      LOG("Analog pin read: ", INPUT_PORTS[i]);
-    } else
+  // Tracks count of digital and analog ports
+  LOG("Number of lectures to report: ", REPORT_READ_COUNT);
+  for (int lecture = 0; lecture <= REPORT_READ_COUNT; lecture++)
+  {
+    uint8_t current_digital = 0;
+    uint8_t current_analog = 0;
+    for (int i = 1; i <= byte(INPUT_PORTS[0]); i++)
     {
-      int data = digitalRead(INPUT_PORTS[i]);
-      response[len + 2] = byte(data);
-      len += 2;
-      LOG("Digital pin read: ", INPUT_PORTS[i]);
-    }
-  };
+      //response[len + 1] = INPUT_PORTS[i];
+      if ( INPUT_PORTS[i] >= A0 )
+      {
+        LOG("Reading analog port A", INPUT_PORTS[i] - A0);
+        int data = analogRead(INPUT_PORTS[i] - A0);
+        //response[len + 2] = byte((data & 0xFF00) >> 8); // Se guarda el msb en el buffer
+        //response[len + 3] = byte(data & 0xFF);        // Se guarda el lsb en el buffer
+        analog_input_buffer[current_analog][0] = INPUT_PORTS[i];
+        analog_input_buffer[current_analog][lecture + 1] = byte((data & 0xFF00) >> 8);
+        analog_input_buffer[current_analog][lecture + 2] = byte(data & 0xFF);
+
+        current_analog++;
+
+        //len += 3; // Cada lectura de un recurso analógico ocupa dos bytes. FIXME: se puede optimizar con bajas resoluciones
+        LOG("=====================================", "");
+        LOG("Analog pin read: ", INPUT_PORTS[i]);
+        LOG("Analog read value: ", data);
+      } else
+      {
+        int data = digitalRead(INPUT_PORTS[i]);
+        //response[len + 2] = byte(data);
+        digital_input_buffer[current_digital][0] = INPUT_PORTS[i];
+        digital_input_buffer[current_digital][(lecture / 8) + 1] += (byte(data) & 0x01) << (lecture % 8); // Just keep first bit
+        current_digital++;
+        //len += 2;
+        LOG("=====================================", "");
+        LOG("Digital pin read: ", INPUT_PORTS[i]);
+        LOG("Raw read: ", data);
+        LOG("Lecture: ", lecture);
+        LOG("Filtered read: ", (byte(data) & 0x01));
+        LOG("Left padding: ", (lecture % 8));
+        LOG("Added: ",  (byte(data) & 0x01) << (lecture % 8));
+        LOG("Digital read value: ", digital_input_buffer[current_digital - 1][(lecture / 8) + 1]);
+      }
+    };
+  }
 
+  // Every analog output will be in the buffer as
+  if (ANALOG_PINS_COUNT > 0)
+  {
+    len += ANALOG_PINS_COUNT + ( (REPORT_READ_COUNT + 1) * 2);
+  }
+
+  if (DIGITAL_PINS_COUNT > 0)
+  {
+    len +=  DIGITAL_PINS_COUNT + (((REPORT_READ_COUNT + 1) / 8 ) + 1);
+  }
 
   LOG("Reporting actuate results ", len - 1);
-  response[1] = len - 1;
-  SerialUSB.write(response, len + 1); // 2 bytes extras por el id de comando y la longitud y -1 por el padding
+  response[1] = len;
+  SerialUSB.write(response, 2); // 2 bytes extras por el id de comando y la longitud y -1 por el padding
+
+  //response[len + 1] = INPUT_PORTS[i];
+  if ( ANALOG_PINS_COUNT > 0)
+  {
+    SerialUSB.write(analog_input_buffer[0], ANALOG_PINS_COUNT + ((REPORT_READ_COUNT + 1) * 2 * ANALOG_PINS_COUNT));
+    //SerialUSB.write(INPUT_PORTS[i]);
+    //SerialUSB.write(analog_input_buffer[INPUT_PORTS[i] - A0], (REPORT_READ_COUNT + 1) * 2);
+    LOG("Reported an analog port with a read count len of: ", (REPORT_READ_COUNT + 1) * 2 + 1);
+  }
+  
+  if ( DIGITAL_PINS_COUNT > 0) {
+    SerialUSB.write(digital_input_buffer[0], DIGITAL_PINS_COUNT + (((REPORT_READ_COUNT + 1) / 8 ) + 1) * DIGITAL_PINS_COUNT );
+    LOG("Reported an digital port with a read count len of: ",  DIGITAL_PINS_COUNT + (((REPORT_READ_COUNT + 1) / 8 ) + 1) * DIGITAL_PINS_COUNT );
+  }
 
-  return offset + 2;
+  return len + 2;
 }
 
 void setup() {
   SerialUSB.begin(115200);
-  
-  #if DEBUG
+
+#if DEBUG
   Serial.begin(115200);
-  #endif
-  
+#endif
+
   for (int i = 0; i < 255; i++)
   {
     executor[i] = &not_implemented;
@@ -183,36 +278,53 @@ void setup() {
 
   /**  Commands Callbacks  **/
   executor[ANALOG_PRECISION_CMD]  = &set_analog_precision;
-  executor[ADD_INPUT_PORT_CMD]  = &set_analog_input;
-  executor[ADD_OUTPUT_PORT_CMD] = &set_analog_output;
+  executor[ADD_INPUT_PIN_CMD]     = &set_analog_input;
+  executor[ADD_OUTPUT_PIN_CMD]    = &set_analog_output;
   executor[SET_PIN_MODE_CMD]      = &set_pin_mode;
   executor[SET_REPORT_MODE_CMD]   = &set_report_mode;
   executor[ACTUATE_CMD]           = &actuate;
+  executor[RESET_PINS]            = &reset;
+
+//  executor[SET_PIN_MODE_CMD]("\x04\x02\x3E\x00");
+//  executor[ANALOG_PRECISION_CMD]("\x01\x01\x0C");
+//  executor[SET_REPORT_MODE_CMD]("\x05\x03\x00\x09\x00");
+//  executor[ADD_INPUT_PIN_CMD]("\x02\x01\x3E");
+//  executor[SET_PIN_MODE_CMD]("\x04\x02\x3E\x00");
+//  //  executor[ADD_INPUT_PIN_CMD]("\x02\x01\x3F");
+//  //  executor[SET_PIN_MODE_CMD]("\x04\x02\x3F\x00");
+//  executor[ADD_INPUT_PIN_CMD]("\x02\x01\x28");
+//  executor[SET_PIN_MODE_CMD]("\x04\x02\x28\x00");
+//  executor[ADD_INPUT_PIN_CMD]("\x02\x01\x29");
+//  executor[SET_PIN_MODE_CMD]("\x04\x02\x29\x00");
+//  executor[ADD_INPUT_PIN_CMD]("\x02\x01\x2A");
+//  executor[SET_PIN_MODE_CMD]("\x04\x02\x2A\x00");
 
 }
 
 void loop() {
-  //executor[ACTUATE_CMD]("\xF0\x01\x28\x01");
+  //executor[ACTUATE_CMD]("\xF0\x01\x2B\x01");
 
   if (SerialUSB.available() > 0)
   {
     LOG("USB serial data available ", SerialUSB.available());
     char input[64]; // Esto se reserva en el stack, tal vez hacerlo global consume menos recursos...
-    
+
     byte b_read = 0;
     byte b_pos = 0;
-    
+
     while (SerialUSB.available() > 0)
     {
       b_read += SerialUSB.readBytes(input, SerialUSB.available());
     }
-    
+
     // Loop to process all commands received in the buffer
-    while(b_pos < b_read)
+    while (b_pos < b_read)
     {
       LOG("Executing command: ", int(input[b_pos]));
       b_pos += executor[input[b_pos]](&input[b_pos]); // Does the callback for the command
       LOG("b_pos ", b_pos);
     }
   }
+
+  delay(4000);
 }
diff --git a/MLC/arduino/protocol.py b/MLC/arduino/protocol.py
@@ -6,6 +6,7 @@
                     "REPORT_MODE"     : '\x05\x03%s%s%s',
                     "ACK"             : '\xFF\x00',
                     "ACTUATE"         : '\xF0',
+                    "RESET"           : '\xFE',
                     "ACTUATE_REPORT"  : '\xF1' }
 
 class ArduinoInterface:
@@ -19,6 +20,9 @@ def __init__(self, connection, board):
         self._anlg_outputs = []
         self._digital_outputs = []
         self._anlg_precition = 10 #Default Arduino analog precision
+        self._report_mode = "AVERAGE"
+        self._read_count = 1 #Default number of inputs read
+        self._read_delay = 0
         self._board = board
 
     def set_precition(self, bits):
@@ -33,9 +37,13 @@ def __set_pin_mode(self, port, mode):
         
         self._connection.send(_PROTOCOL_CMDS["PIN_MODE"] % (chr(port), chr(self.PIN_MOD[mode])))
 
-    def set_report_mode(self, mode, read_count=1, read_delay=0):
+    def set_report_mode(self, mode, read_count=0, read_delay=0):
         if mode not in self.REPORT_MOD.keys():
             raise Exception("Report mode error. Unknown value: %s" % mode)
+
+        self._report_mode = mode
+        self._read_count = read_count + 1
+        self._read_delay = read_delay
         
         self._connection.send(_PROTOCOL_CMDS["REPORT_MODE"] % (chr(self.REPORT_MOD[mode]), chr(read_count), chr(read_delay)))
   
@@ -76,6 +84,9 @@ def __validate_pin(self, pin):
         if pin not in self._board["DIGITAL_PINS"] and pin not in self._board["ANALOG_PINS"]:
             raise Exception("Invalid pin %s for board %s" % (pin, self._board["NAME"]))
         return
+  
+    def reset(self):
+        self._connection.send(_PROTOCOL_CMDS["RESET"])
 
     def actuate(self, data):
         """
@@ -120,7 +131,8 @@ def actuate(self, data):
                 pos = pos + 3
             else:
                 if ord(data[pos]) in self._digital_inputs:
-                    results.append((data[pos], bool(ord(data[pos+1]))))
+                    for i in range(0, self._read_count):
+                        results.append((data[pos], bool(ord(data[pos+1]))))
                     pos = pos + 2
                 else:
                     raise Exception("Unknown port in response. Restart Arduino board, your software and pray")
diff --git a/tests/pocs/test_connection.py b/tests/pocs/test_connection.py
@@ -8,13 +8,16 @@
 def actuate(terminal):
     connection = SerialConnection(port=terminal)
     arduinoDue = ArduinoInterface(connection, boards.Due)
-    
+
+    arduinoDue.reset()
+    arduinoDue.set_report_mode("BULK")    
     arduinoDue.add_output(40)
     arduinoDue.add_input(64)
+    arduinoDue.add_input(30)
 
     output = arduinoDue.actuate([(40,1)])
     for i in output:
-        print "Pin %d output: %s" % (ord(i[0]), i[1])
+        print "Pin %d input: %s" % (ord(i[0]), i[1])
 
 
 if __name__ == "__main__":
