Controlling pan/tilt mechanism with GY-87 IMU module

1
2#include <Servo.h>
3#include "I2Cdev.h"
4#include "MPU6050.h"
5#include   "HMC5883L.h"
6#include "Wire.h"
7Servo servobase;
8Servo servoelbow;
9int   servoPos = 90; //
10int compassCorrection = 0; // set to 0 (not used)
11static   const char LED = 6;
12static const float ACCEL_SENS = 16384.0; // Accel Sensitivity   with default +/- 2g scale
13static const float GYRO_SENS  = 131.0;   // Gyro Sensitivity   with default +/- 250 deg/s scale
14HMC5883L mag;
15int16_t mx, my, mz;
16MPU6050   accelgyro ;
17int16_t ax, ay, az ;
18int16_t gx, gy, gz ;
19void setup()
20{
21boolean   state = HIGH;
22unsigned int count = 0;
23Serial.begin(9600);
24  while (!Serial   && (count < 30) )
25  {
26    delay(200); 
27    digitalWrite(LED, state);
28     state = !state;
29    count++;
30  }
31  digitalWrite(LED, HIGH);
32  Wire.begin();
33   accelgyro.initialize();
34  mag.initialize();
35}
36void loop()
37{
38  servobase.attach(9);
39servoelbow.attach(6);
40   static unsigned long ms = 0;
41  static boolean state = HIGH;
42if (millis()   - ms > 100)
43  {
44 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
45 mag.getHeading(&mx,   &my, &mz);
46  float heading = atan2(my, mx);
47  if(heading < 0) heading += 2   * M_PI;
48  float headingDegrees = heading * 180/M_PI; 
49  int headingDegreesInteger   = headingDegrees;
50if ((headingDegreesInteger - compassCorrection) >= 0)
51  {
52     headingDegreesInteger = headingDegreesInteger  - compassCorrection;
53  }
54   else
55  {
56    headingDegreesInteger = headingDegreesInteger - compassCorrection   + 360;
57  }
58  if (headingDegreesInteger > 90 && headingDegreesInteger < 270)
59   {
60    if (headingDegreesInteger <= 180)
61    {
62      servoPos = 180;
63     }
64    else
65    {
66      servoPos = 10;
67    }
68  }
69  else
70   {
71    if (headingDegreesInteger <= 90)
72    {
73      servoPos = map(headingDegreesInteger,   0, 90, 85, 10);
74    }
75    else
76    {
77      servoPos = map(headingDegreesInteger,360,   270, 85, 180); //(360,270,85,180)
78    }
79  }
80  servobase.write(servoPos);
81   Serial.println (servoPos);
82  delay(10);
83accelgyro.getMotion6 (&ax, &ay,   &az, &gx, &gy, &gz);
84ay = map (ay, -17000, 17000, 0, 180) ;
85Serial.println   (180-ay);
86servoelbow.write (180-ay); 
87delay(10);
88    ms = millis();
89     digitalWrite(LED, state);
90    state = !state;
91  }
92}

1
2#include <Servo.h>
3#include "I2Cdev.h"
4#include "MPU6050.h"
5#include  "HMC5883L.h"
6#include "Wire.h"
7Servo servobase;
8Servo servoelbow;
9int  servoPos = 90; //
10int compassCorrection = 0; // set to 0 (not used)
11static  const char LED = 6;
12static const float ACCEL_SENS = 16384.0; // Accel Sensitivity  with default +/- 2g scale
13static const float GYRO_SENS  = 131.0;   // Gyro Sensitivity  with default +/- 250 deg/s scale
14HMC5883L mag;
15int16_t mx, my, mz;
16MPU6050  accelgyro ;
17int16_t ax, ay, az ;
18int16_t gx, gy, gz ;
19void setup()
20{
21boolean  state = HIGH;
22unsigned int count = 0;
23Serial.begin(9600);
24  while (!Serial  && (count < 30) )
25  {
26    delay(200); 
27    digitalWrite(LED, state);
28    state = !state;
29    count++;
30  }
31  digitalWrite(LED, HIGH);
32  Wire.begin();
33  accelgyro.initialize();
34  mag.initialize();
35}
36void loop()
37{
38  servobase.attach(9);
39servoelbow.attach(6);
40  static unsigned long ms = 0;
41  static boolean state = HIGH;
42if (millis()  - ms > 100)
43  {
44 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
45 mag.getHeading(&mx,  &my, &mz);
46  float heading = atan2(my, mx);
47  if(heading < 0) heading += 2  * M_PI;
48  float headingDegrees = heading * 180/M_PI; 
49  int headingDegreesInteger  = headingDegrees;
50if ((headingDegreesInteger - compassCorrection) >= 0)
51  {
52    headingDegreesInteger = headingDegreesInteger  - compassCorrection;
53  }
54  else
55  {
56    headingDegreesInteger = headingDegreesInteger - compassCorrection  + 360;
57  }
58  if (headingDegreesInteger > 90 && headingDegreesInteger < 270)
59  {
60    if (headingDegreesInteger <= 180)
61    {
62      servoPos = 180;
63    }
64    else
65    {
66      servoPos = 10;
67    }
68  }
69  else
70  {
71    if (headingDegreesInteger <= 90)
72    {
73      servoPos = map(headingDegreesInteger,  0, 90, 85, 10);
74    }
75    else
76    {
77      servoPos = map(headingDegreesInteger,360,  270, 85, 180); //(360,270,85,180)
78    }
79  }
80  servobase.write(servoPos);
81  Serial.println (servoPos);
82  delay(10);
83accelgyro.getMotion6 (&ax, &ay,  &az, &gx, &gy, &gz);
84ay = map (ay, -17000, 17000, 0, 180) ;
85Serial.println  (180-ay);
86servoelbow.write (180-ay); 
87delay(10);
88    ms = millis();
89    digitalWrite(LED, state);
90    state = !state;
91  }
92}