Horizontal platform

1#include<Wire.h>
2#include <Servo.h>
3#include <math.h>
4const int  MPU=0x68;
5int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,servox,servoy,norm;
6double pitch,roll;
7Servo  myservox;
8Servo myservoy;
9
10void setup(){
11pinMode(7,OUTPUT);   // indicatie  horizontaal;
12pinMode(8,OUTPUT);   // indicatie pitch +;
13pinMode(9,OUTPUT);   // indicatie pitch -;
14pinMode(10,OUTPUT);   // indicatie roll +;
15pinMode(11,OUTPUT);   // indicatie roll -;
16myservox.attach(12);
17myservoy.attach(13);
18norm=2;
19servox=90;    //middenpositie
20servoy=90;    //tussen 0 en 179
21myservox.write(servox);    //start op de middenpositie
22myservoy.write(servoy);    //start op de middenpositie
23Wire.begin();
24Wire.beginTransmission(MPU);
25Wire.write(0x6B);
26Wire.write(0);
27Wire.endTransmission(true);
28//Serial.begin(9600);
29}
30void  loop(){
31Wire.beginTransmission(MPU);
32Wire.write(0x3B);
33Wire.endTransmission(false);
34Wire.requestFrom(MPU,14,true);
35
36int  AcXoff,AcYoff,AcZoff,GyXoff,GyYoff,GyZoff;
37int temp,toff;
38double t,tx,tf;
39
40//Acceleration  data correction
41AcXoff = -950;
42AcYoff = -300;
43AcZoff = 0;
44
45//Temperature  correction
46toff = -1600;
47
48//Gyro correction
49GyXoff = 480;
50GyYoff  = 170;
51GyZoff = 210;
52
53//read accel data
54AcX=(Wire.read()<<8|Wire.read())  + AcXoff;
55AcY=(Wire.read()<<8|Wire.read()) + AcYoff;
56AcZ=(Wire.read()<<8|Wire.read())  + AcYoff;
57
58//read temperature data
59temp=(Wire.read()<<8|Wire.read()) +  toff;
60tx=temp;
61t = tx/340 + 36.53;
62tf = (t * 9/5) + 32;
63
64//read gyro  data
65GyX=(Wire.read()<<8|Wire.read()) + GyXoff;
66GyY=(Wire.read()<<8|Wire.read())  + GyYoff;
67GyZ=(Wire.read()<<8|Wire.read()) + GyZoff;
68
69//get pitch/roll
70getAngle(AcX,AcY,AcZ);
71if  (pitch<norm && pitch>-norm && roll<norm && roll>-norm)   // binnen de norm horizontaal
72{
73  digitalWrite(7, HIGH);
74}
75else
76{
77  digitalWrite(7, LOW);
78}
79if  (pitch>norm)  // helt te veel voorover
80{
81  if (servox>0)
82  {
83   servox=servox-1;
84   myservox.write(servox);
85  }
86  digitalWrite(8, HIGH);
87}
88else
89{
90  digitalWrite(8, LOW);
91}
92
93if (pitch<-norm)  // helt te veel achterover
94{
95  if (servox<179)
96  {
97   servox=servox+1;
98   myservox.write(servox);
99  }
100  digitalWrite(9, HIGH);
101}
102else
103{
104  digitalWrite(9, LOW);
105}
106if  (roll>norm)    // helt te veel naar links
107{
108  if (servoy<179)
109  {
110   servoy=servoy+1;
111   myservoy.write(servoy);
112  }
113  digitalWrite(10, HIGH);
114}
115else
116{
117  digitalWrite(10, LOW);
118}
119if (roll<-norm)     // helt te veel naar rechts
120{
121  if (servoy>0)
122  {
123   servoy=servoy-1;
124   myservoy.write(servoy);
125  }
126
127  digitalWrite(11, HIGH);
128}
129else
130{
131  digitalWrite(11, LOW);
132}
133
134//send  the data out the serial port
135//Serial.print("Angle: ");
136//Serial.print("Pitch  = "); Serial.print(pitch);
137//Serial.print(" | Roll = "); Serial.print(roll);
138
139//Serial.print("  Temp: ");
140//Serial.print("Temp(F) = "); Serial.print(tf);
141//Serial.print("  | Temp(C) = "); Serial.print(t);
142
143//Serial.print("  Accel.: ");
144//Serial.print("X  = "); Serial.print(AcX);
145//Serial.print(" | Y = "); Serial.print(AcY);
146//Serial.print("  | Z = "); Serial.print(AcZ);
147
148//Serial.print("  Gyroscope: ");
149//Serial.print("X  = "); Serial.print(GyX);
150//Serial.print(" | Y = "); Serial.print(GyY);
151//Serial.print("  | Z = "); Serial.println(GyZ);
152//Serial.println(" ");
153delay(20);
154}
155
156//convert  the accel data to pitch/roll
157void getAngle(int Vx,int Vy,int Vz) {
158double  x = Vx;
159double y = Vy;
160double z = Vz;
161
162pitch = atan(x/sqrt((y*y) + (z*z)));
163roll  = atan(y/sqrt((x*x) + (z*z)));
164//convert radians into degrees
165pitch = pitch  * (180.0/3.14);
166roll = roll * (180.0/3.14) ;
167
168}