File content as of revision 9:6c8dcbfc625a:

#include "RWR_Utils.hxx"

// TODO: remove this. added for debugging purposes only
extern Serial serial;

extern DigitalOut espCHPD;
extern DigitalOut espGPIO0;
extern DigitalOut espReset;
extern Serial esp;
extern DigitalOut led;
extern QTR qtr;
extern web_settings_t webSettings;

void ESPReceiveIRQ()
{
  led = !led;
  while(esp.readable())
  {
    char receivedChar = esp.getc();

    #ifdef ENABLE_PASSTHROUGH
      serial.putc(receivedChar);
    #endif

    if (webReceiveBufferPointer < webReceiveBuffer + WEB_RECEIVE_BUFFER_SIZE - 1)
    {
      *webReceiveBufferPointer++ = receivedChar;
    } else
    {
      webReceiveBufferFull = true;
    }
  }

  webReceiveBuffer[WEB_RECEIVE_BUFFER_SIZE - 1] = '\0';
  webReceivedData = true;
}

void USBReceiveIRQ()
{
  while(serial.readable())
  {
    esp.putc(serial.getc());
  }
}

status_t ESPProcessReply()
{
  status_t status = STATUS_PENDING;

  // wait for OK, ERROR or timeout
  if (strstr(webReceiveBuffer, "OK") != NULL)
  {
    status = STATUS_OK;

    goto end;
  }
  if (strstr(webReceiveBuffer, "ERROR") != NULL)
  {
    status = STATUS_ERROR;

    goto end;
  }
  if (ESPCommunicationTimer.read_ms() >= WEB_RECEIVE_TIMEOUT)
  {
    status = STATUS_TIMEOUT;

    goto end;
  }

  end:
  if (status != STATUS_PENDING)
  {
    // Warning: race condition
    esp.attach(NULL);
    webReceiveBufferPointer = webReceiveBuffer;
    memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
    esp.attach(&ESPReceiveIRQ, Serial::RxIrq);
  }

  return status;
}

status_t ESPSendCommandAndProcessReply(const char *command)
{
  status_t status = STATUS_PENDING;

  if (webSendNewCommand)
  {
    ESPCommunicationTimer.reset();
    ESPCommunicationTimer.start();

    #ifdef ESP_DEBUG
      serial.printf("Sending command: %s", command);
    #endif

    esp.printf("%s", command);

    webSendNewCommand = false;
  }

  status = ESPProcessReply();
  if (status < STATUS_PENDING)
  {
    serial.printf("ESP Command %s failed: ", command);
    if (status == STATUS_ERROR)
    {
      serial.printf("ERROR\r\n");
    } else
    if (status == STATUS_TIMEOUT)
    {
      serial.printf("TIMEOUT\r\n");
    }
    serial.printf("Buffer: %s\r\n", webReceiveBuffer);

    webSendNewCommand = true;
  }

  if (status == STATUS_OK)
  {
    webSendNewCommand = true;
  }

  return status;
}

status_t ESPRunStateMachine()
{
  status_t status = STATUS_PENDING;
  int32_t returnValue = 0;
  char *parserPointer = NULL;
  int32_t sentDataSize = 0;
  char *webSendBufferPointer = NULL;
  char closeConnectionCommandBuffer[20];
  bool receivedReady = false;
  bool receiveTimeout = false;

  switch(ESPParserState)
  {
    case RESET_ESP:
      // reset ESP
      espCHPD = 0;
      espReset = 0;
      espGPIO0 = 0;

      wait_ms(1);

      espCHPD = 1;
      espReset = 1;
      espGPIO0 = 1;

      ESPCommunicationTimer.reset();
      ESPCommunicationTimer.start();

      ESPParserState = WAIT_FOR_READY;
    break;

    case WAIT_FOR_READY:
      status = STATUS_PENDING;
      receivedReady = false;
      receiveTimeout = false;

      if (webReceiveBufferPointer - webReceiveBuffer >= (int)sizeof("ready"))
      {
        receivedReady = (strstr((const char*)webReceiveBufferPointer - sizeof("ready"), "ready") != NULL);
      }

      receiveTimeout = (ESPCommunicationTimer.read_ms() >= WEB_RECEIVE_TIMEOUT);

      if (receivedReady)
      {
        status = STATUS_OK;

        ESPParserState = INIT_SEND_AT;
      } else
      if (receiveTimeout)
      {
        serial.printf("ESP reset timeout\r\n");
        status = STATUS_TIMEOUT;

        // disable web interface if max number of attempts reached
        if (espInitAttempts >= WEB_ESP_INIT_ATTEMPTS)
        {
          espCHPD = 0;
          espReset = 0;
          espGPIO0 = 0;

          serial.printf("Failed to initialize ESP. Web interface disabled\r\n");

          ESPParserState = IDLE;
        } else
        // retry to initialize ESP
        {
          espInitAttempts++;

          ESPParserState = RESET_ESP;
        }
      } else
      {
        status = STATUS_PENDING;
      }

      if (status != STATUS_PENDING)
      {
        // Warning: race condition
        esp.attach(NULL);
        webReceiveBufferPointer = webReceiveBuffer;
        memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
        esp.attach(&ESPReceiveIRQ, Serial::RxIrq);
      }
    break;

    case INIT_SEND_AT:
      ESP_SEND_AND_PROCESS_STATUS("AT\r\n", INIT_SEND_CWMODE);
    break;

    case INIT_SEND_CWMODE:
      ESP_SEND_AND_PROCESS_STATUS("AT+CWMODE=2\r\n", INIT_SEND_CWSAP);
    break;

    case INIT_SEND_CWSAP:
      status = ESPSendCommandAndProcessReply("AT+CWSAP=\"" WEB_SSID "\",\"" WEB_PASSWORD "\"," WEB_CHANNEL "," WEB_ECN "," WEB_MAX_CONNECTIONS "," WEB_HIDDEN "\r\n");
      if (status == STATUS_OK)
      {
        ESPParserState = INIT_SEND_CIPAP;
      } else
      if (status < STATUS_PENDING)
      {
        // do not print actual password for security reasons
        serial.printf("ESP command failed: AT+CWSAP=\"" WEB_SSID "\",\" WEB_PASSWORD \"," WEB_CHANNEL "," WEB_ECN "," WEB_MAX_CONNECTIONS "," WEB_HIDDEN "\r\n\r\n");

        ESPParserState = RESET_ESP;
      }
    break;

    case INIT_SEND_CIPAP:
      ESP_SEND_AND_PROCESS_STATUS("AT+CIPAP=\"" WEB_AP_IP "\",\"" WEB_AP_GATEWAY "\",\"" WEB_AP_NETMASK "\"\r\n", INIT_SEND_CIPMUX);
    break;

    case INIT_SEND_CIPMUX:
      ESP_SEND_AND_PROCESS_STATUS("AT+CIPMUX=1\r\n", INIT_SEND_CIPSERVER);
    break;

    case INIT_SEND_CIPSERVER:
      ESP_SEND_AND_PROCESS_STATUS("AT+CIPSERVER=1," WEB_AP_PORT "\r\n", PARSE_IPD);
    break;

    case PARSE_IPD:
      // wait for IPD
      webReceiveRequestsData = strstr(webReceiveBuffer, "+IPD");
      if ((webReceiveRequestsData != NULL) && (webReceiveRequestsData - webReceiveBuffer) >= WEB_IPD_HEADER_MIN_SIZE)
      {
        ESPCommunicationTimer.reset();
        ESPCommunicationTimer.start();

        serial.printf("Received +IPD\r\n");
      }

      // parse connection ID and HTTP payload size
      returnValue = sscanf(webReceiveRequestsData, "+IPD,%d,%d:", &webConnectionId, &webPayloadSize);
      if ((returnValue < 2) || (returnValue == EOF))
      {
        webConnectionId = -1;
        status = STATUS_ERROR;

        goto end;
      }
      if (webPayloadSize <= 0)
      {
        webConnectionId = -1;
        status = STATUS_ERROR;

        serial.printf("Invalid web payload size %d\r\n", webPayloadSize);

        goto end;
      }

      // check if payload fits in buffer (overflow avoided in receive IRQ)
      if (webReceiveRequestsData + webPayloadSize >= webReceiveBuffer + WEB_RECEIVE_BUFFER_SIZE)
      {
        webConnectionId = -1;
        status = STATUS_ERROR;

        serial.printf("Web payload size %d exceeds buffer size\r\n", webPayloadSize);

        goto end;
      }
      serial.printf("Connection ID: %i\r\n", webConnectionId);
      serial.printf("Payload size: %i\r\n", webPayloadSize);

      // locate start of payload in buffer
      webReceiveRequestsData = strstr(webReceiveRequestsData, ":");
      if (webReceiveRequestsData == NULL)
      {
        serial.printf("Invalid +IPD message\r\n");

        ESPParserState = PARSE_IPD;

        webConnectionId = -1;
        status = STATUS_ERROR;

        // Warning: race condition
        esp.attach(NULL);
        webReceiveBufferPointer = webReceiveBuffer;
        memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
        esp.attach(&ESPReceiveIRQ, Serial::RxIrq);
      }

      // skip ':'
      webReceiveRequestsData++;

      // check if HTTP request is complete
      if (webReceiveBufferPointer >= webReceiveRequestsData + webPayloadSize - 1)
      {
        ESPParserState = PARSE_REQUEST_BODY;
      }

      // check timeout
      if (ESPCommunicationTimer.read_ms() >= WEB_RECEIVE_TIMEOUT)
      {
        serial.printf("+IPD message receive timeout\r\n");

        ESPParserState = RESET_ESP;
      }

    break;

    case PARSE_REQUEST_BODY:
      parserPointer = strstr(webReceiveRequestsData, "GET");
      if (parserPointer != NULL)
      {
        serial.printf("Received GET request\r\n");

        // Warning: race condition
        esp.attach(NULL);
        webReceiveBufferPointer = webReceiveBuffer;
        memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
        esp.attach(&ESPReceiveIRQ, Serial::RxIrq);

        ESPParserState = GENERATE_WEB_PAGE;
      }

      parserPointer = strstr(webReceiveRequestsData, "POST");
      if (parserPointer == NULL)
      {
        goto PARSE_REQUEST_BODY_END;
      }

      serial.printf("Received POST request\r\n");
      serial.printf("Buffer: %s\r\n", parserPointer);

      parserPointer = strstr(parserPointer, "motors=");
      if (parserPointer == NULL)
      {
        serial.printf("Invalid POST request\r\n");

        goto PARSE_REQUEST_BODY_END;
      }

      returnValue = sscanf(parserPointer,
        "motors=%d&kp=%d&ki=%d&kd=%dend",
        &webSettings.motorsEnabled,
        &webSettings.Kp,
        &webSettings.Ki,
        &webSettings.Kd);

      if ((returnValue < 1) || (returnValue == EOF))
      {
        serial.printf("Invalid POST request parameters\r\n");

        // overwrite settings with hardcoded defaults
        webSettings.motorsEnabled = WEB_MOTORS_ENABLED_DEFAULT;
        webSettings.Kp = (int32_t) (PID_DEFAULT_KP * 1000);
        webSettings.Ki = (int32_t) (PID_DEFAULT_KI * 1000);
        webSettings.Kd = (int32_t) (PID_DEFAULT_KD * 1000);
      }

      serial.printf("New web settings:\r\n");
      serial.printf("Enable motors: %d\r\n", webSettings.motorsEnabled);
      serial.printf("Kp: %d\r\n", webSettings.Kp);
      serial.printf("Ki: %d\r\n", webSettings.Ki);
      serial.printf("Kd: %d\r\n", webSettings.Kd);

      // Warning: race condition
      esp.attach(NULL);
      webReceiveBufferPointer = webReceiveBuffer;
      memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
      esp.attach(&ESPReceiveIRQ, Serial::RxIrq);

      ESPParserState = GENERATE_WEB_PAGE;
      status = STATUS_OK;

      PARSE_REQUEST_BODY_END:
    break;

    case GENERATE_WEB_PAGE:
      // update web status
      webStatus.digitizedSensorValues = qtr.readSensorsAsDigital();

      qtr.readSensorsAsAnalog(webStatus.analogSensorValues);
      for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
      {
         webStatus.analogSensorValuesAverage += webStatus.analogSensorValues[i];
      }
      webStatus.analogSensorValuesAverage /= SENSORS_BAR_SIZE;

      // WEB page data
      memset(webSendBuffer, 0, WEB_SEND_BUFFER_SIZE);

      webSendBufferPointer = webSendBuffer;
      webSendBufferPointer += sprintf(webSendBufferPointer, "<!DOCTYPE html>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<html><head><title>" WEB_SSID " Web Interface</title>");
      // avoid automatic favicon requests
      webSendBufferPointer += sprintf(webSendBufferPointer, "<link rel=\"icon\" href=\"data:,\"></head>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<body>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<div style=\"text-align:center; background-color:#F4F4F4; color:#00AEDB;\"><h1>" WEB_SSID " Web Interface</h1>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<h2>Controls</h2>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<form method=\"POST\">");

      if (webSettings.motorsEnabled)
      {
        webSendBufferPointer += sprintf(webSendBufferPointer, "<p><input type=\"radio\" name=\"motors\" value=\"1\" checked>  Enable motors");
        webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"radio\" name=\"motors\" value=\"0\" >  Disable motors");
      } else
      {
        webSendBufferPointer += sprintf(webSendBufferPointer, "<p><input type=\"radio\" name=\"motors\" value=\"1\" >  Enable motors");
        webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"radio\" name=\"motors\" value=\"0\" checked>  Disable motors");
      }

      webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"number\" name=\"kp\" value=\"%d\" >  PID Kp", webSettings.Kp);
      webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"number\" name=\"ki\" value=\"%d\" >  PID Ki", webSettings.Ki);
      webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"number\" name=\"kd\" value=\"%d\" >  PID Kd", webSettings.Kd);
      // hidden input used as end of payload marker
      webSendBufferPointer += sprintf(webSendBufferPointer, "<br><input type=\"hidden\" name=\"end\" value=\"0\" >");
      webSendBufferPointer += sprintf(webSendBufferPointer, "</strong><p><input type=\"submit\" value=\"send-refresh\" style=\"background: #3498db;");
      webSendBufferPointer += sprintf(webSendBufferPointer, "background-image:-webkit-linear-gradient(top, #3498db, #2980b9);");
      webSendBufferPointer += sprintf(webSendBufferPointer, "background-image:linear-gradient(to bottom, #3498db, #2980b9);");
      webSendBufferPointer += sprintf(webSendBufferPointer, "-webkit-border-radius:12;border-radius: 12px;font-family: Arial;color:#ffffff;font-size:20px;padding:");
      webSendBufferPointer += sprintf(webSendBufferPointer, "10px 20px 10px 20px; border:solid #103c57 3px;text-decoration: none;");
      webSendBufferPointer += sprintf(webSendBufferPointer, "background: #3cb0fd;");
      webSendBufferPointer += sprintf(webSendBufferPointer, "background-image:-webkit-linear-gradient(top,#3cb0fd,#1a5f8a);");
      webSendBufferPointer += sprintf(webSendBufferPointer, "background-image:linear-gradient(to bottom,#3cb0fd,#1a5f8a);");
      webSendBufferPointer += sprintf(webSendBufferPointer, "text-decoration:none;\"></form></span>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<h2>Status</h2>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "Digitized sensor values: ");
      for (uint32_t mask = (1 << (SENSORS_BAR_SIZE - 1)); mask > 0; mask >>= 1)
      {
        webSendBufferPointer += sprintf(webSendBufferPointer, "%lu", ((webStatus.digitizedSensorValues & mask) != 0));
      }
      webSendBufferPointer += sprintf(webSendBufferPointer, "<br>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "Analog sensor values:<br>");
      for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
      {
        webSendBufferPointer += sprintf(webSendBufferPointer, "Sensor %lu analog value: %lu<br>", i, webStatus.analogSensorValues[i]);
      }
      webSendBufferPointer += sprintf(webSendBufferPointer, "Analog sensor values average: %lu<br>", webStatus.analogSensorValuesAverage);
      webSendBufferPointer += sprintf(webSendBufferPointer, "<p/><h2>How to use:</h2><ul>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "<li>Click 'Send-Refresh' to send controls to robot and update status</li>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "</ul>");
      webSendBufferPointer += sprintf(webSendBufferPointer, "</body></html>");

      // get total page buffer length
      sentDataSize = webSendBufferPointer - webSendBuffer;

      // send IPD link channel and buffer character length.
      esp.printf("AT+CIPSEND=%d,%d\r\n", webConnectionId, sentDataSize);

      ESPCommunicationTimer.reset();
      ESPCommunicationTimer.start();

      ESPParserState = WAIT_FOR_CIPSEND_READY;
    break;

    case WAIT_FOR_CIPSEND_READY:
      status =  STATUS_PENDING;
      // wait for ESP to be ready to receive HTTP reponse data by
      // waiting for '>' after CIPSEND command or timeout
      receivedReady = (strstr(webReceiveBuffer, ">") != NULL);
      receiveTimeout = (ESPCommunicationTimer.read_ms() >= WEB_RECEIVE_TIMEOUT);

      if (receivedReady)
      {
        serial.printf("CIPSEND ready\r\n");

        status =  STATUS_OK;
        ESPParserState = SEND_PAGE;
      }
      if (receiveTimeout)
      {
        serial.printf("CIPSEND timeout\r\n");

        status =  STATUS_TIMEOUT;
        ESPParserState = CLOSE_CONNECTION;
      }
    break;

    case SEND_PAGE:
      esp.attach(NULL);
      webReceiveBufferPointer = webReceiveBuffer;
      memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));

      // send HTTP response
      esp.printf("%s", webSendBuffer);

      ESPCommunicationTimer.reset();
      ESPCommunicationTimer.start();

      esp.attach(&ESPReceiveIRQ, Serial::RxIrq);

      ESPParserState = WAIT_FOR_SEND_OK;
    break;

    case WAIT_FOR_SEND_OK:
      status = STATUS_PENDING;

      receivedReady = (strstr(webReceiveBuffer, "SEND OK") != NULL);
      receiveTimeout = (ESPCommunicationTimer.read_ms() >= WEB_RECEIVE_TIMEOUT);

      if (receivedReady)
      {
        serial.printf("Received SEND OK\r\n");

        status = STATUS_OK;
      }
      if (receiveTimeout)
      {
        serial.printf("SEND OK timeout\r\n");

        status = STATUS_TIMEOUT;
      }

      if (status != STATUS_PENDING)
      {
        ESPParserState = CLOSE_CONNECTION;
      }

      return status;
    break;

    case CLOSE_CONNECTION:
      // close connection to complete HTTP response
      memset(closeConnectionCommandBuffer, '\0', sizeof(closeConnectionCommandBuffer));
      sprintf(closeConnectionCommandBuffer, "AT+CIPCLOSE=%d\r\n", webConnectionId);

      ESP_SEND_AND_PROCESS_STATUS(closeConnectionCommandBuffer, PARSE_IPD);

      if (status != STATUS_PENDING)
      {
        // Warning: race condition
        esp.attach(NULL);
        webReceiveBufferPointer = webReceiveBuffer;
        memset(webReceiveBuffer, '\0', sizeof(webReceiveBuffer));
        esp.attach(&ESPReceiveIRQ, Serial::RxIrq);
      }
    break;

    default:
    break;
  }

  end:
  return status;
}

Motor::Motor(const PinName &_in1Pin, const PinName &_in2Pin, const bool _brake):
  in1PWM(_in1Pin),
  in2PWM(_in2Pin),
  brake(_brake)
{
  in1PWM.period(PWM_PERIOD);
  in1PWM.write(brake * 1.0f);
  in2PWM.period(PWM_PERIOD);
  in2PWM.write(brake * 1.0f);
}

void Motor::setSpeed(const float speed)
{
  if (speed >= 0.0f)
  {
    in2PWM.write(brake * 1.0f);
    in1PWM.write(speed);
  } else
  if (speed < 0.0f)
  {
    in1PWM.write(brake * 1.0f);
    in2PWM.write(-speed);
  }
}

void Motor::setBrake(const bool _brake)
{
  in1PWM.write(_brake * 1.0f);
  in2PWM.write(_brake * 1.0f);
}

QTR::QTR(DigitalOut &_ctl, AnalogIn (&_sensors)[SENSORS_BAR_SIZE], const uint32_t dimPulses):
  ctl(_ctl),
  sensors(_sensors),
  lastSensorState(0)
{
  dim(dimPulses);
  ctl = 1;
}

void QTR::dim(const uint32_t pulses)
{
  // reset QTR dim
  ctl = 0;
  wait_ms(1.5f);
  ctl = 1;
  wait_us(1);

  // apply pulses on CTL to dim sensors
  for (uint32_t i = 0; i < pulses; i++)
  {
    ctl = 0;
    wait_us(1);
    ctl = 1;
    wait_us(1);
  }
}

uint32_t QTR::readSensorsAsDigital()
{
  uint32_t currentSensorState = lastSensorState;

  // for each sensor
  for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
  {
    uint32_t mask = (1U << i);

    // if last sensor state was 'true'
    if (lastSensorState & mask)
    {
      // if current analog value is below low threshold
      if ((uint32_t)sensors[i].read_u16() < QTR_THRESHOLD_LOW)
      {
        // reset bit
        currentSensorState &= ~mask;
      }
    } else // if last sensor state was 'false'
    {
      // if current analog value is above high threshold
      if ((uint32_t)sensors[i].read_u16() > QTR_THRESHOLD_HIGH)
      {
        // set bit
        currentSensorState |= mask;
      }
    }
  }

  lastSensorState = currentSensorState;
  return currentSensorState;
}

void QTR::readSensorsAsAnalog(uint32_t *buffer)
{
  for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
  {
    buffer[i] = (uint32_t)sensors[i].read_u16();
  }
}

float QTR::readSensorsAsFloat()
{
  uint32_t sensorValue = readSensorsAsDigital();
  float returnValue = digitizedSensorsToFloat(sensorValue);

  // return line position as a float between -1.0 (left) and 1.0 (right)
  return returnValue;
}

float QTR::digitizedSensorsToFloat(uint32_t digitizedSensors)
{
  float returnValue = 0.0f;

  // count leading zeros
  uint32_t clz = __CLZ(digitizedSensors);
  // reverse bits
  uint32_t ctz = __RBIT(digitizedSensors);
  // count trailing zeros
  ctz = __CLZ(ctz);

  // convert CLZ and CTZ to intermediate line position
  int intermediateResult = (32 - (int32_t)clz - SENSORS_BAR_SIZE + (int32_t)ctz);
  // normalize line position and multiply by 1000 to allow integer comparison
  intermediateResult = intermediateResult * 1000 / (SENSORS_BAR_SIZE - 1);

  // replace out of range readings with 0
  if ((intermediateResult < -1000) || (intermediateResult > 1000))
  {
    returnValue = 0.0f;
  } else
  {
    returnValue = intermediateResult / 1000.0f;
  }

  // return line position as a float between -1.0 (left) and 1.0 (right)
  return returnValue;
}