Camera Mount Control by Smartphone

/*
*/
#include <Servo.h>


Servo azimuthServo;
Servo elevationServo;
const int azimuthPin = 4; // pin to ajimuth motor
const int elevationPin = 5; // pin to elevation motor

int ax, ay, az;
int gx, gy, gz;

void setup() {
  // set motor pin to output mode
  pinMode(azimuthPin, OUTPUT);
  pinMode(elevationPin, OUTPUT);

  azimuthServo.attach(azimuthPin);
  elevationServo.attach(elevationPin);

  azimuthServo.write(90);
  elevationServo.write(0);


}

// Holds the last data we read from scratch characteristic 1
ScratchData lastScratch;

bool compareScratch(ScratchData *scratch1, ScratchData *scratch2) {
  // If they contain different numbers of bytes, they can't be equal,
  // so return false
  if (scratch1->length != scratch2->length) {
    return false;
  }

  // Compare each byte in order and return false if two bytes don't match
  for (int i = 0; i < scratch1->length; i++) {
    if (scratch1->data[i] != scratch2->data[i]) {
      return false;
    }
  }
}

void loop() {
  // Read data from scratch characteristic 1
  ScratchData thisScratch = Bean.readScratchData(1);

  // Compare the data we just read with the data we saw last time
  // Compare the data we just read with the data we saw last time
  bool matched = compareScratch(&thisScratch, &lastScratch);

  // Save the data we just read for our next comparison
  lastScratch = thisScratch;

  if (!matched) {
    int azimuthAngle = lastScratch.data[0] + (lastScratch.data[1] << 8);
    int tiltAngle = lastScratch.data[2] + (lastScratch.data[3] << 8);
    if (azimuthAngle >= 0 && azimuthAngle <=180 ) { 
      azimuthServo.write(azimuthAngle);
    }
    if(tiltAngle >= 0 && tiltAngle <= 90) {
      elevationServo.write(tiltAngle);
    }
    ax = Bean.getAccelerationX();
    ay = Bean.getAccelerationY();
    az = Bean.getAccelerationZ();
    uint8_t accelValue[6];
    accelValue[0] = ax & 0xFF;
    accelValue[1] = ax >> 8;
    accelValue[2] = ay & 0xFF;
    accelValue[3] = ay >> 8;
    accelValue[4] = az & 0xFF;
    accelValue[5] = az >> 8; 
    Bean.setScratchData(2, accelValue, 6); 
  } else {
    // Otherwise, just sleep for one second and check again
    Bean.sleep(1000);
  }

}