rs485 for maple..

#include <itoa.h>

#define pinRTS 27

/* Phone number ordering:
 0 = master
 32 = relay board
 99 = unset number

 0 = master
 z = relay board (32)
 x = unset number (99)
 */

/* AT language set structure

 +++A T | COMMAND / PARAMETER | VALUE | #

 */

/* Examples:
 Dialing 1:  +++ATD11### //Call 1
 Dialing 99: +++ATD9999### // Call 11
 Hangup:     +++ATHH###
 Test if we are talking: +++ATZZ#
 Set phone number to 02: +++ATSETPHONE22#
 or: +++ATSETPHONE0022#
 Set phone number to 25: +++ATSETPHONE2255#
 Get values; like current phone number: +++ATGETPHONE
 */

byte phoneNO;
boolean connected;
byte conState; /* 0 = listening; 1 = receivingCommand; 2 = executing; 3 = filetransfer */
byte chCue[4];
char receivedCom[32];
boolean recError;

void setup() {
  pinMode(pinRTS, OUTPUT);
  digitalWrite(pinRTS, LOW);
  Serial1.begin(115200);

  phoneNO = 99; //  Initial phoneNo is 99
  receivedCom[0] = 0;
  conState = 0;
  connected = false;
  recError = false;
  chCue = {
    0, 0, 0, 0  };

  while ( (USART1_BASE -> SR & USART_SR_TC) == 0);
  delay(1200); // Settle for awhile
  Serial1.flush();
  outputMSG("Ready\r\n");
  SerialUSB.println("Ready\r\n");
}

void loop() {

  if ( Serial1.available() > 0 ) {
    addChCue();
    chCue[3] = Serial1.read();
    /*
     SerialUSB.print("Something in buffer.. '");
     SerialUSB.write(chCue[3]);
     SerialUSB.println(" '");
     */
    if (( conState == 1 ) && ( chCue[3] == '#' )) // Command recording ends
      conState = 2;

    if ( conState == 1 ) { // Record command
      receivedCom[0]++;
      if (( chCue[3] > 96 ) && ( chCue[3] < 123 ))
        receivedCom[receivedCom[0]] = chCue[3] - 32;
      else
        receivedCom[receivedCom[0]] = chCue[3];

      if ( receivedCom[0] > 28 ) { // Command too long, we have error somewhere
        receivedCom[0]--;
        recError = true;
        if ( !connected )
          conState = 0;
        SerialUSB.println("Error: Too long command.");
      }

    }

    if ( conState == 2 ) { // Command recording ended - answering time
      SerialUSB.println("Command recording ended..");
      parseCommand();
      conState = 0;
      recError = false;
      receivedCom[0] = 0;
    }

    if (( chCue[0] && chCue[1] && chCue[2] == '+' ) && (( chCue[3] == 'A' ) || ( chCue[3] == 'a' ))) {
      conState = 1;
      receivedCom[0] = 1;
      receivedCom[1] = 'A';
      recError = false;
      SerialUSB.println("Starting to record a command..");
    }

  }

}

void addChCue() {
  chCue[0] = chCue[1];
  chCue[1] = chCue[2];
  chCue[2] = chCue[3];
}

byte getCommandValue(byte commandLen) {
  if (( receivedCom[0] > commandLen + 4 ) || ( receivedCom[0] < commandLen + 2 )) {
    recError = true;
    return 255; // Garbage on command or command has no value
  }

  if ( receivedCom[commandLen + 1] != receivedCom[commandLen + 2] ) {
    recError = true;
    return 254; // Mismatch
  }

  if ( ( receivedCom[0] > commandLen + 2 ) && ( receivedCom[commandLen + 3] != receivedCom[commandLen + 4] )) {
    recError = true;
    return 254; // Mismatch
  }

  if ( receivedCom[0] > commandLen + 2 )
    return ((( receivedCom[commandLen + 1] - 48 ) * 10 ) + (receivedCom[commandLen + 3] - 48));

  return (receivedCom[commandLen + 1] - 48);

}

void parseCommand() {

  if (( receivedCom[0] == 0 ) || ( conState == 0 )) // Extra '#' to make sure command end mark is sent..
    return;

  if (( receivedCom[0] >= 5 ) && ( receivedCom[0] < 8 ) && ( commandBegins("ATD")) && ( !recError ) && ( getCommandValue(3) == phoneNO )) {
    connected = true;
    outputMSG("OK#\r\n");
    return;
  }

  if ( !connected ) // We answer to the next commands only if we are connected
      return;

  if ( recError ) {
    outputMSG("ERR#\r\n");
    return;
  }

  SerialUSB.println(receivedCom);

  if (( receivedCom[0] >= 5 ) && ( receivedCom[0] < 8 ) && ( commandBegins("ATD")) && ( getCommandValue(3) != phoneNO )) {
    connected = false; // master called somewhere else and forgot to hangup.. Let's try to be smart..
    return;
  }

  if (( receivedCom[0] == 4 ) && ( commandBegins("ATZZ"))) { // Always return OK to ATZZ, suites to test if we are connected
    outputMSG("OK#\r\n");
    return;
  }

  if (( receivedCom[0] == 4 ) && ( commandBegins("ATHH"))) { // Hangup
    SerialUSB.println("Hangup..");

    outputMSG("OK#\r\n");
    connected = false;
    return;
  }

  if (( receivedCom[0] > 7 ) && ( commandBegins("ATSET"))) {
    setCommand();
    return;
  }

  if (( receivedCom[0] > 7 ) && ( commandBegins("ATGET"))) {
    getCommand();
    return;
  }

  outputMSG("UNK#\r\n"); // Unknown command

}

void setCommand() {

  SerialUSB.println("Setting values..");

  if (( receivedCom[0] >= 12 ) && ( receivedCom[0] < 15 ) && ( commandBegins("ATSETPHONE"))) { // Set new phone number
    byte newNo = getCommandValue(10);

    if (( newNo < 32 ) && ( newNo > 0 )) { // Rule out relay board and master
      phoneNO = newNo;
      outputMSG("OK#\r\n");
      return;
    }

    outputMSG("ERR#\r\n");
    return;
  }

  outputMSG("UNK#\r\n"); // Unknown command

}

void getCommand() {

  SerialUSB.println("Getting values..");

    if (( receivedCom[0] == 10 ) && ( commandBegins("ATGETPHONE"))) { // Get phone no
    outputVAL(phoneNO);
    connected = false;
    return;
  }

  outputMSG("UNK#\r\n"); // Unknown command

}

void outputVAL(byte VAL) {
  char buffer[4];
  itoa(VAL, buffer, 10);

  digitalWrite(pinRTS,HIGH); // Set rs485 control pin high to enable writing to rs485
  Serial1.print("VAL:");
  Serial1.print(buffer[0]); Serial1.print(buffer[0]);
  if ( buffer[1] != 0 ) {
    Serial1.print(buffer[1]); Serial1.print(buffer[1]);
  }
  Serial1.print("#\r\n");

  while ( (USART1_BASE -> SR & USART_SR_TC) == 0);
  digitalWrite(pinRTS,LOW);
}


void outputMSG(char *MSG) {
  byte c = 0;

  digitalWrite(pinRTS,HIGH); // Set rs485 control pin high to enable writing to rs485
  while ( MSG[c] != 0 ) {
    Serial1.write(MSG[c]);
    c++;
  }

  while ( (USART1_BASE -> SR & USART_SR_TC) == 0);
  digitalWrite(pinRTS,LOW);
}

boolean commandBegins(char *comparison)
{

  byte c = 0;
    byte b1 = receivedCom[0];
  while ( comparison[c] != 0 )
  {
    if (( receivedCom[0] < c ) || ( receivedCom[c+1] != comparison[c] ))
      return false;
    c++;
  }

  return true;
}