Serial communications sketch

Serial_Communications.pde

/*
  Serial Communications
  
  Reports and receives servo positions via serial
  */

#include <Servo.h>
#include <AFMotor.h>
#include <aJSON.h>

#define NEWLINE 10 // byte code for newline
#define CARRIAGE_RETURN 13 // byte code for carriage return
#define EXCLAMATION_MARK 33 // byte code for exclamation mark
#define BUFFER_SIZE 128 // 127 characters + termination ("\o")
#define READ_VAL_STRING "r" // string, when received, which triggers an output of current motor values
#define radiansToDegrees(x) x * PI / 180

Servo servo1;
Servo servo2;

typedef struct Motor {
  int speed;
  int direction;
  AF_DCMotor motor;
};

void motor_setSpeed(struct Motor *m, int speed)
{
  m->speed = speed;
  m->motor.setSpeed(speed);
  return;
}

void motor_setDirection(struct Motor *m, int direction)
{
  if (direction >= 1 && direction <= 4)
  {
    m->direction = direction;
    m->motor.run(direction);
  }
  return;
}
  

Motor motor1 = {0, FORWARD, AF_DCMotor(2, MOTOR12_64KHZ)};
Motor motor2 = {0, FORWARD, AF_DCMotor(2, MOTOR12_64KHZ)};

char incomingByte;
char buffer[BUFFER_SIZE];
char *p = buffer; // pointer to buffer


void setup() 
{
  servo1.attach(8);
  servo2.attach(9);
  Serial.begin(9600);
  establishContact();
}

void loop()
{
  
  // read from the serial port
  if (Serial.available() > 0) {
    incomingByte = Serial.read();

    if ( incomingByte == EXCLAMATION_MARK || incomingByte == NEWLINE || incomingByte == CARRIAGE_RETURN) {
      *p = '\0'; // terminate the string with the zero byte
      
      
      if (!strcmp(buffer, READ_VAL_STRING)) 
      { // output current motor values
        Serial.print(prepare_json());
      } else { // process commands to motors
        aJsonObject *root = aJson.parse(buffer);
        process_json(root);
        free((void *)root);
      }
      
      p = buffer; // reset pointer to buffer position
      return; // exit the loop
    }
    
    // store byte in buffer and increment pointer 
    *p++ = incomingByte;
  }
}

// send `A` until receiving a response
void establishContact()
{
  digitalWrite (13, HIGH);
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);
    delay(300);
  }
  digitalWrite (13, LOW);
}

// process incoming json values, changing motors as needed
// TODO: DRY
void process_json(aJsonObject *root)
{
      aJsonObject *s1 = aJson.getObjectItem(root, "s1");
      aJsonObject *s2 = aJson.getObjectItem(root, "s2");
      
      aJsonObject *m1 = aJson.getObjectItem(root, "m1");
      aJsonObject *m1_speed = aJson.getObjectItem(m1, "speed");
      aJsonObject *m1_direction = aJson.getObjectItem(m1, "direction");
      
      aJsonObject *m2 = aJson.getObjectItem(root, "m2");
      aJsonObject *m2_speed = aJson.getObjectItem(m2, "speed");
      aJsonObject *m2_direction = aJson.getObjectItem(m2, "direction");
      
      Serial.print(s1->valueint);
      servo1.write(s1->valueint);
      
      Serial.print(m1->valueint);
      motor_setSpeed(&motor1, m1_speed->valueint);
      motor_setDirection(&motor1, m1_direction->valueint);
      
      motor_setSpeed(&motor2, m2_speed->valueint);
      motor_setDirection(&motor2, m2_direction->valueint);
}

// prepare json string to be sent to Lab View controller
char *prepare_json()
{
  aJsonObject *root, *m1, *m2;
  root = aJson.createObject();
  
  aJson.addNumberToObject(root, "s1", servo1.read());
  aJson.addNumberToObject(root, "s2", servo2.read());
  
  aJson.addItemToObject(root, "m1", m1 = aJson.createObject());
  aJson.addNumberToObject(m1, "speed", motor1.speed);
  aJson.addNumberToObject(m1, "direction", motor1.direction);
    
  aJson.addItemToObject(root, "m2", m2 = aJson.createObject());
  aJson.addNumberToObject(m2, "speed", motor2.speed);
  aJson.addNumberToObject(m2, "direction", motor2.direction);
  
  return aJson.print(root);
}

// set the first element of the buffer to 0
void clearBuffer()
{
  *buffer = '\0';
}