Arduino networking code

4DrobotServer/4DrobotServer.ino

+/*
+ * Server for controling 4D robot for moving samples in/out of the beam at CLEAR.
+ * Access via telnet-like interface at port 23.
+ * 
+ * Example for accessing over text interface:
+ *  nc 192.168.1.31 23
+ * Netcat (nc) is the best client: it is a clean TCP client,
+ * which makes no attempts to negotiate anything.
+ * 
+ * Kyrre Ness Sjobak, 24 April 2021
+ * University of Oslo / CERN
+ * 
+ * Inspired by PositionGaugeServer, also in use at CLEAR.
+ */
+
+#include <Arduino.h>
+
+#include <SPI.h>
+#include <Ethernet.h>
+
+#include <OneWire.h>
+#include <DallasTemperature.h>
+
+#include <Servo.h>
+
+//This file holds all the configurations
+// to be modified for deployment
+#include "4DrobotServer_config.h"
+
+// ***** GLOBAL VARIABLES ***********************
+
+EthernetServer telnet_socket(telnet_port);
+EthernetClient telnet_connection[telnet_numConnections];
+bool           telnet_connectionInUse[telnet_numConnections];
+
+//Buffers & buffCount (number of elements = idx of first free element)
+char           telnet_buff[telnet_numConnections][telnet_bufflen];
+size_t         telnet_buffCount[telnet_numConnections];
+
+char           serial_buff[telnet_bufflen];
+size_t         serial_buffCount;
+
+char           output_buff[output_bufflen];
+size_t         output_buffCount;
+
+//Temperature sensor access & buffers
+OneWire            temp_oneWire(temp_oneWire_pin);
+DallasTemperature  temp_sensors(&temp_oneWire);
+float              temp_data[temp_numSensors]; //[degC]
+unsigned long      temp_update_prev = 0;       //[ms]
+
+//Grabber servo
+Servo grabber_servo;
+bool  grabber_go   = false;
+int   grabber_pos  = grabber_closed; //Assumed initial position
+int   grabber_goto = grabber_closed;
+
+// ***** CODE ***********************************
+
+void setup() {
+  //Serial setup, for debugging
+  Serial.begin(9600);
+  
+  // wait for serial port to connect. Needed for native USB port only
+  int serialCounter=0;
+  while (!Serial) {
+    //However only wait for 1 second before giving up,
+    // so that it can work also when not connected to USB
+    delay(100);
+    serialCounter++;
+    if (serialCounter > 10) break;
+  }
+  Serial.println();
+  Serial.println("4DrobotServer initializing...");
+
+
+  //Network setup
+  Serial.println("Starting networking...");
+  #ifdef USE_DHCP
+  Ethernet.begin(mac);
+  #else
+  Ethernet.begin(mac, ip, myDns, gateway, subnet);
+  #endif
+
+  // Check for Ethernet hardware present
+  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
+    Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
+    while (true) {
+      // Do nothing, no point running without Ethernet hardware
+      // TODO: Not actually true, we could also run with just serial...
+      delay(1);
+    }
+  }
+  while(true)
+    if (Ethernet.linkStatus() == LinkOFF) {
+      Serial.println("Ethernet cable is not connected");
+    }
+    else {
+      Serial.println("Ethernet cable connected / status unknown");
+      break;
+  }
+  byte buffMAC[6];
+  Serial.print("MAC address:     ");
+  Ethernet.MACAddress(buffMAC);
+  for (byte octet=0; octet<6;octet++) {
+    Serial.print(buffMAC[octet],HEX);
+    if(octet < 5) {
+      Serial.print(":");
+    }
+  }
+  Serial.println();
+  
+  Serial.print("IP address:      "); Serial.println(Ethernet.localIP());
+  Serial.print("DNS address:     "); Serial.println(Ethernet.dnsServerIP());
+  Serial.print("Gateway address: "); Serial.println(Ethernet.gatewayIP());
+  Serial.print("Subnet:          "); Serial.println(Ethernet.subnetMask());
+
+  //Server setup
+  telnet_socket.begin();
+  for (uint8_t i=0; i<telnet_numConnections; i++) {
+    memset(telnet_buff[i], '\0', sizeof(telnet_buff[i]));
+    telnet_buffCount[i] = 0;
+    telnet_connectionInUse[i] = false;
+  }
+  memset(serial_buff, '\0', sizeof(serial_buff));
+  serial_buffCount = 0;
+  
+  memset(output_buff, '\0', sizeof(output_buff));
+  output_buffCount = 0;
+
+  //Initialize temperature sensors to something nonsensical
+  for (size_t i=0; i<temp_numSensors; i++) {
+    temp_data[i] = -500.0f; //(it's float not Farenheit)
+  }
+
+  //Serial is ready for input!
+  Serial.println("$");
+}
+
+
+
+
+
+void loop() {
+  //Run all the "programs" in order
+  
+  //delay(1000);
+
+  temperatures_update();
+  
+  telnet_server();
+
+  servo_control();
+
+  // ** Housekeeping   **
+  #ifdef USE_DHCP
+  DHCPhousekeeping();
+  #endif
+}
+
+
+
+
+
+void temperatures_update() {
+  //Program for refreshing the temperatures
+
+  //Poll temperature sensors no faster than the given interval
+  //If too short, some sensors may heat up and give inaccurate results
+  unsigned long thisUpdateTime = millis();
+  if (not (thisUpdateTime - temp_update_prev >= temp_update_interval)) {
+    //Not yet
+    return;
+  }
+  temp_update_prev = thisUpdateTime;
+
+  //Refresh temp_data
+  #ifndef DUMMY_TEMP
+  temp_sensors.requestTemperatures();
+  for (size_t i=0; i<temp_numSensors; i++) {
+    temp_data[i] = temp_sensors.getTempCByIndex(i);
+  }
+  #else
+  for (size_t i=0; i< temp_numSensors; i++) {
+    temp_data[i] = random(-1000L,15000L)/100.0f;
+  }
+  #endif
+}
+
+
+
+
+
+void servo_control() {
+  //Program for controlling the servomotor for the grabber
+  if (grabber_go) {
+    for(int i=grabber_pos; i != grabber_goto; (grabber_pos < grabber_goto) ? i++ : i--) {
+      #ifndef DUMMY_SERVO
+      grabber_servo.write(i);
+      #else
+      Serial.print("Grabber to: ");
+      Serial.println(i);
+      #endif
+      delay(grabber_stepWait);
+    }
+    grabber_pos = grabber_goto;
+    grabber_go  = false;
+
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER GO FINISHED\n"));
+  }
+}
+
+
+
+
+
+void telnet_server() {
+  // Program for running the telnet- and serial communication
+  
+  //1. Check for new connections
+  while (true) {
+    //Loop in case there are multiple new connections
+    EthernetClient newConnection = telnet_socket.accept();
+    if (newConnection) {
+      //Find a new connection slot
+      for (size_t i=0; i<telnet_numConnections; i++) {
+        if (!telnet_connectionInUse[i]) {
+          telnet_connection[i] = newConnection;
+          telnet_connectionInUse[i] = true;
+          
+          bufferWrite(output_buff, sizeof(output_buff), output_buffCount, "< INFO CONN MAKE: ");
+
+          IPAddress IP = telnet_connection[i].remoteIP();
+          char IP_buff[16];
+          snprintf(IP_buff, sizeof(IP_buff), "%d.%d.%d.%d", IP[0],IP[1],IP[2],IP[3]);
+          bufferWrite(output_buff, sizeof(output_buff), output_buffCount, IP_buff);
+
+          snprintf(IP_buff, sizeof(IP_buff), " ON #%-2d\n", i);
+          bufferWrite(output_buff, sizeof(output_buff), output_buffCount, IP_buff);
+          
+          telnet_connection[i].println("$");
+          
+          break;
+        }
+      }
+    }
+    else {
+      break;
+    }
+  }
+
+  //2. Check for incoming data (telnet & serial) and buffer it
+  for (size_t i = 0; i < telnet_numConnections; i++) {
+    while (telnet_connectionInUse[i]        &&
+           telnet_connection[i].connected() &&
+           telnet_connection[i].available()  > 0   ) {
+      bufferWrite(telnet_buff[i], sizeof(telnet_buff[i]), telnet_buffCount[i], telnet_connection[i].read());
+    }
+  }
+  while (Serial.available() > 0) {
+    bufferWrite(serial_buff, sizeof(serial_buff), serial_buffCount, Serial.read());
+  }
+
+  //3. Feed the clients & serial with the content of the output buffer
+  if(output_buffCount) {
+    for (size_t i = 0; i < telnet_numConnections; i++) { 
+      if (telnet_connectionInUse[i] &&
+          telnet_connection[i].connected() ) {
+        telnet_connection[i].print(output_buff);
+      }
+    }
+    Serial.print(output_buff);
+
+    memset(output_buff, '\0', sizeof(output_buff));
+    output_buffCount = 0;
+  }
+
+  //4. Check for telnet disconnects & handle
+  for (size_t i = 0; i < telnet_numConnections; i++) {
+    if ( telnet_connectionInUse[i] &&
+        !telnet_connection[i].connected() ) {
+
+      telnet_connectionInUse[i] = false;
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, "< INFO CONN BREAK: ");
+
+      IPAddress IP = telnet_connection[i].remoteIP();
+      char IP_buff[16];
+      snprintf(IP_buff, sizeof(IP_buff), "%d.%d.%d.%d", IP[0],IP[1],IP[2],IP[3]);
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, IP_buff);
+
+      snprintf(IP_buff, sizeof(IP_buff), " ON #%-2d\n", i);
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, IP_buff);
+
+      telnet_connection[i].stop();
+
+      memset(telnet_buff[i],'\0',sizeof(telnet_buff[i]));
+      telnet_buffCount[i]=0;
+    }
+  }
+
+  //5. Parse input buffers & notify that we are ready for more
+  for (size_t i = 0; i < telnet_numConnections; i++) {
+    input_parser(telnet_buff[i], sizeof(telnet_buff[i]), telnet_buffCount[i], &(telnet_connection[i]));
+  }
+  input_parser(serial_buff, sizeof(serial_buff), serial_buffCount, &(Serial));
+  
+}
+void input_parser(char* in, const size_t buffSize, size_t& buffCount, Stream* originatingStream) {
+  // Attempt to parse the commands in the buffer.
+  // It will only attempt to parse commands that end with a '\n', '\r', or a combination thereof
+  
+  //1. Sanity-check the buffer size
+  // (Note: Last entry is always a \0, buffSize cannot point to it)
+  if (buffCount == buffSize-1) {
+    originatingStream->println("@ INFO BUFFER FULL");
+    originatingStream->println("@ INFO BUFFER RESET");
+
+    memset(in,'\0', buffSize);
+    buffCount=0;
+
+    // Ready for more
+    originatingStream->println("$");
+    
+    return;
+  }
+  
+  //2. Look for whole lines & parse each of them in order
+  char line_buff[buffSize];
+  memset(line_buff,'\0', sizeof(line_buff));
+  size_t line_next = 0; //Index of the first valid character
+
+  bool lastchar_was_newline = true;
+  bool gotCommand           = false; //Print a `$` when done, after having run commands
+  
+  for (size_t i = 0; i<buffCount; i++) {
+    
+    if(in[i] == '\0') {
+      //Sanity check: There should be no NULL before the end of the buffer `in`      
+      originatingStream->println("@ INFO BUFFER NULL");
+      originatingStream->println("@ INFO BUFFER RESET");
+    
+      memset(line_buff,'\0', sizeof(line_buff));
+      memset(in,'\0', buffSize);
+      line_next = 0;
+      buffCount = 0;
+
+      // Ready for more
+      originatingStream->println("$");
+
+      return;
+    }
+    else if (lastchar_was_newline) {
+      //Skip repeated newlines, and place line_next correctly
+      line_next = i;
+      if(in[i] != '\n' and in[i] != '\r') {
+        //Double newline or newline at start
+        lastchar_was_newline = false;
+      }
+    }
+    else if (in[i] == '\n' or in[i] == '\r') {
+      //Newline not following start or another newline
+      memcpy(line_buff, in+line_next, i-line_next);
+      line_buff[i-line_next]='\0';
+      parse_line(line_buff);
+
+      lastchar_was_newline = true;
+      //Note: line_next is valid before lastchar_was_newline is false
+      line_next = i;
+      
+      memset(line_buff,'\0', sizeof(line_buff));
+      gotCommand = true;
+    }
+    //Else: Just keep counting chars
+  }
+
+  //3. Prepare buffers for next round 
+  if (lastchar_was_newline && buffCount > 0) {
+    // All data was parsed; reset buffer and continue
+    if(not in[line_next+1] == '\0') {
+      //Internal error!
+      originatingStream->println("@ INFO BUFFER UNTERMINATED");
+      originatingStream->println("@ INFO BUFFER RESET");
+      //TODO:
+      // This may indicate a serious bug (array overrun)
+      // consider resetting the CPU!
+    }  
+
+    //All treated, let's reset
+    memset(line_buff,'\0', sizeof(line_buff));
+    memset(in,'\0', buffSize);
+    buffCount = 0;
+    line_next = 0;
+
+    // Ready for more
+    originatingStream->println("$");
+  }
+  else if (not lastchar_was_newline && line_next>0) {
+    // Shift remaining content of `in` to start of `in`
+    size_t j = 0;
+    for(size_t i = line_next; i < buffCount; i++) {
+      in[j] = in[i];
+      j += 1;
+    }
+    
+    memset(in+j,'\0', buffSize-j);
+    buffCount = j;
+  }
+  
+}
+void parse_line(char* line_buff) {
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, "> ");
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, line_buff);
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, '\n');
+
+  size_t len = strlen(line_buff);
+
+  if      (len >= 4 and strncmp(line_buff, "HELP", 4)==0) {
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< USAGE:\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'HELP'            : Get help\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'TEMP'            : Get temperature(s)\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'GRABBER STATUS'  : Get grabber status\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'GRABBER POS int' : Goto the given position (integer)\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'GRABBER CLOSE'   : Close the grabber\n"));
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("<  'GRABBER OPEN'    : Open the grabber\n"));
+  }  
+  else if (len >= 4 and strncmp(line_buff, "TEMP", 4)==0) {
+    for (size_t i=0; i<temp_numSensors; i++) {
+      char buff[23];
+      float f = temp_data[i];
+      int tmp_int = (int) (f*100);
+      int tmp_dec = abs(tmp_int-tmp_int/100*100);
+      tmp_int     = tmp_int/100;
+      snprintf(buff, sizeof(buff), "< TEMP #%03d %+04d.%02d C\n", i, tmp_int,tmp_dec);
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
+    }
+  }
+  else if (len >= 7 and strncmp(line_buff, "GRABBER", 7)==0) {
+    //Note: Not writing out the sub-options to save RAM
+    if (len >= 14 and strncmp(line_buff+7, " STATUS", 7)==0)   { //'GRABBER STATUS'
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< POS = "));
+      char buff[5];
+      snprintf(buff,sizeof(buff), "%3d\n", grabber_pos);
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
+    }
+    else if (len >= 13 and strncmp(line_buff+7, " CLOSE", 6)==0)  { //'GRABBER CLOSE'
+      if(grabber_go) {
+        bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR GRABBER ALREADY SET"));
+      }
+      grabber_goto = grabber_closed;
+      grabber_go   = true;
+    }
+    else if (len >= 12 and strncmp(line_buff+7, " OPEN", 5)==0)  { //'GRABBER OPEN'
+      if(grabber_go) {
+        bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR GRABBER ALREADY SET"));
+      }
+      grabber_goto = grabber_open;
+      grabber_go   = true;
+    }
+    else {
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR GRABBER COMMAND NOT RECOGNIZED\n"));
+    }
+  }
+  else {
+    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< ERROR COMMAND NOT RECOGNIZED\n"));
+  }
+}
+
+
+
+
+
+int bufferWrite(char* buff, const size_t buffSize, size_t& buffCount, const char* in) {
+  /* Add a zero-terminated string to a buffer,
+   * checking for buffer overruns and
+   * (SIDE EFFECT!) incrementing the buffCount.
+   * If the buffer will overrun, truncate the input string.
+   * Return number of chars written, or return <0 on error.
+   * If buffer overrun, chars may still be written -> positive return.
+   */
+
+  size_t len = strlen(in);
+  
+  int ret    = snprintf(buff+buffCount, buffSize-buffCount, in);
+  buffCount += ret;
+  return ret;
+}
+int bufferWrite(char* buff, const size_t buffSize, size_t& buffCount, const char in) {
+  /* Add a single char to a buffer,
+   * checking that there is space for it and
+   * (SIDE EFFECT!) incrementing the buffCount.
+   * Returns the number of chars written (0 or 1).
+   */
+  if (buffCount == buffSize-1) {
+    //Pointing at terminating \0,
+    // no more space!
+    return 0;
+  }
+  else {
+    buff[buffCount] = in;
+    buffCount++;
+    return 1;
+  }
+}
+int bufferWrite(char* buff, const size_t buffSize, size_t& buffCount, const __FlashStringHelper *ifsh) {
+  //Copy flash contents into buffer, otherwise functionally identical to the const char* version.
+  // Used to save RAM from big hardcoded strings.
+  //
+  // Based on ArduinoCore-avr/cores/arduino/Print.cpp->Print::Print(const __FlashStringHelper *ifsh)
+  // If it doesn't work (future arch), try removing the 'F()' around the strings and comment out this function.
+
+  PGM_P p = reinterpret_cast<PGM_P>(ifsh);
+  size_t n = 0;
+  while (1) {
+    char c = pgm_read_byte(p++);
+    if (c == 0) break;
+    if (bufferWrite(buff, buffSize, buffCount, c)) n++;
+    else break;
+  }
+  return n;
+}
+
+
+
+
+
+#ifdef USE_DHCP
+void DHCPhousekeeping() {
+  switch (Ethernet.maintain()) {
+    case 1:
+      //renewed fail
+      Serial.println("Error: renewed fail");
+      break;
+
+    case 2:
+      //renewed success
+      Serial.println("Renewed success");
+      //print your local IP address:
+      Serial.print("My IP address: ");
+      Serial.println(Ethernet.localIP());
+      break;
+
+    case 3:
+      //rebind fail
+      Serial.println("Error: rebind fail");
+      break;
+
+    case 4:
+      //rebind success
+      Serial.println("Rebind success");
+      //print your local IP address:
+      Serial.print("My IP address: ");
+      Serial.println(Ethernet.localIP());
+      break;
+
+    default:
+      //nothing happened
+      break;
+  }
+}
+#endif