take off orienter.ino

/*This arduino controls orientation...it is always on standby and is only activated by the master arduino
   immedietly the rocket fires.the function of this is to make sure the
   rocket is always on the right way up for takeoff.
   THIS SYSTEM IS ON THE I2C BUS HENCE IS A SLAVE
 * *Has a unique adress to identify it from all other devices on the bus
however this system is not a must since its function is too regulate it to 
vertical flight...this is only for best precision...
more attention is to be given to the landing system.......


*/
#include <Wire.h>
#include <Servo.h>//include servo library
Servo a1, b1, c1, d1; //define four servo objects
//-----SETUP ACCELEROMETER---------------
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

int minVal = 265;
int maxVal = 402;

double x;
double y;
double z;
//------------------------------------------------

void setup() {
  //----------------------------------------------------------
  // initialize the serial port:
  Serial.begin(9600);
  //-----------SETUP ACCELEROMETER--------------
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  //------------SETUP SERVOS---------------------

  a1.attach(3);//attach servos to pwm pins
  b1.attach(5);
  c1.attach(6);
  d1.attach(9);
  
  a1.write(90);//take servo position to the middle(90)
  b1.write(90);
  c1.write(90);
  d1.write(90);
}

void loop() {


  //------------DATA from mpu----------------
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  int xAng = map(AcX, minVal, maxVal, -90, 90);
  int yAng = map(AcY, minVal, maxVal, -90, 90);
  int zAng = map(AcZ, minVal, maxVal, -90, 90);

  x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
  y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
  z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);

  /* Serial.print("AngleX= ");
    Serial.println(x);

    Serial.print("AngleY= ");
    Serial.println(y);

    Serial.print("AngleZ= ");
    Serial.println(z);
    Serial.println("-----------------------------------------");
  */

  int xpos1 = map(x, 0, -90, 0, 90); //directly map change in x to servo b1 and d1
  int xpos2 = map(x, 0, 90, 90, 180); //directly map change in x to servo b1 and d1

  int ypos1 = map(y, 0, -90, 0, 90); //directly map change in x to servo b1 and d1
  int ypos2 = map(y, 0, 90, 90, 180); //directly map change in x to servo b1 and d1

  c1.write(xpos2);//slighly reduce surface area to correct orient
  d1.write(xpos1);//slighly reduce surface area to correct orient

  b1.write(ypos2);//slighly reduce surface area to correct orient
  a1.write(ypos1);//slighly reduce surface area to correct orient




}