#include<Wire.

h>
#define MinDegValue 0 // min degree value from accelerometer
#define MaxDegValue 90 // max degree value from accelerometer
#define MinOutVolts 123 // min output voltage 0.6V (4mA on 150R)
#define MaxOutVolts 614 // max output voltage 3V (20mA on 150R)
#define analogPinIn A0
#define analogPinOut A1

const int MPU_addr=0x68;

int16_t axis_X,axis_Y,axis_Z;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;
float outputVolts = 0.60; // should read 511 for 2.5V. Any value 0 to 5V

void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
axis_X=Wire.read()<<8|Wire.read();
axis_Y=Wire.read()<<8|Wire.read();
axis_Z=Wire.read()<<8|Wire.read();
int xAng = map(axis_X,minVal,maxVal,-90,90);
int yAng = map(axis_Y,minVal,maxVal,-90,90);
int zAng = map(axis_Z,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("Angle of inclination in X axis = ");
// Serial.print(x);
// Serial.println((char)176);
// Serial.print("Angle of inclination in Y axis= ");
// Serial.print(y);
// Serial.println((char)176);
Serial.print("Angle of inclination in Z axis= ");
Serial.print(z);
Serial.println((char)176);
Serial.println("-------------------------------------------");

if (z < MinDegValue)
{
z = MinDegValue;
}
else if (z > MaxDegValue)
{
z = MaxDegValue;
}
outputVolts = map(z, MinDegValue, MaxDegValue, MinOutVolts, MaxOutVolts);
outputVolts = 1023 - outputVolts; // PNP

Serial.print("outputVolts= ");
Serial.println(outputVolts);
 changeCap(); // set the capacitor to outputVolts

delay(200);

/* 10 bit analog output. Uses one pin (any analog input pin), 10K and 1uF
capacitor
* Capacitor should be a low leakage type, eg polyester greencap or tantalum
* Circuit is A0 => 10K => 1uF => Gnd
* Settles rapidly in approx 1 millisecond. Call every few seconds to tweak the
value up or down as needed.
* Keep leads short to minimise pickup of stray RF
* Advantage compared to pwm - rapid settling 100,000 times faster than pwmout
with low pass filter, 10 bits resolution instead of 8, and no need for multipole
filter for smoothing
* Disadvantage - not 'set and forget', need to keep calling the changeCap
routine every second or so
* By James Moxham January 2017
*/

void changeCap()
{
int r;
float outV;
int error;
outV = outputVolts; //* 204.6; // 5V * 204.6 = 1023
outV = (int) outV;
// if analog read is the same, then do nothing, if less, then charge the cap
a bit, if more, then discharge
r = averageReading();
Serial.print("Avg = "+String(r));
error = outV - averageReading();
if (error > 10) {
Serial.println("Charge Fast");
chargeFast(outV);
Serial.println(String(analogRead(analogPinIn)));
}
if (error < -10) {
Serial.println("Discharge Fast");
dischargeFast(outV);
Serial.println(String(analogRead(analogPinIn)));
}

if (r != outV) // if equal then do nothing

{
if (r < outV)
{
chargeDischarge(true,0); // tiny charge, can decrease right down to zero
as the delay in the couple lines of code is enough for 1uF/10k. Look for
overshooting
Serial.print("+");
}else{
chargeDischarge(false,0); // tiny discharge
Serial.print("-");
}
}
Serial.println(""); // new line
}
void chargeFast(int v)
{
while (analogRead(analogPinIn) < v) {
chargeDischarge(true,50); // larger steps, should do this in about a
millisecond
}
}

void dischargeFast(int v)
{
while (analogRead(analogPinIn) > v) {
chargeDischarge(false,50);
}
}

int averageReading() // average over about 20 readings. Can try 10 or 50, but 20
seems about right. Not sure why it varies though.
{
int i;
float a = 0;
for (i=0;i<20;i++) {
a = a + analogRead(analogPinIn);
}
a = a / 20;
a = (int) a; // convert to integer
return a;
}

void chargeDischarge(boolean cd,int t) // cd true for charge, false for

discharge
{
pinMode(analogPinOut,OUTPUT); // set analog pin as an output
if (cd == true) {
digitalWrite(analogPinOut,HIGH); // charge
}else{
digitalWrite(analogPinOut,LOW); // discharge
}
delayMicroseconds(t); // can be as low as zero, even the tiny delay in the
code is enough to nudge voltage up or down slightly
pinMode(analogPinOut,INPUT); // set it back to an input
}