//This code was used in the bachelor’s thesis, "The Stabilizing Spoon".
//The code was modified in the later section to enable one of the
//servo motors to rotate further than what was possible with the
//original code.

// ============= INCLUDING LIBRARIES AND DECLARING VARIABLES =============
#include<Wire.h>
#include <Servo.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float delta_t = 0.005;
float pitchAcc,rollAcc, pitch, roll, pitched;
float P_CompCoeff= 0.98;
// ============= INITIAL SETUP ===========================================
Servo myservo1, myservo2;
void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(115200);
myservo1.attach(10);
myservo2.attach(11);
}

// ============= MAIN LOOP ===============================================
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C
//(ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E
//(ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40
//(ACCEL_ZOUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44
//(GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46
//(GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48
//(GYRO_ZOUT_L)
//Complementary filter
long squaresum_P=((long)GyY*GyY+(long)AcY*AcY);
long squaresum_R=((long)GyX*GyX+(long)AcX*AcX);
pitch+=((-AcY/40.8f)*(delta_t));
roll+=((-AcX/45.8f)*(delta_t)); //32.8
pitchAcc= atan((AcY/sqrt(squaresum_P))*RAD_TO_DEG);
rollAcc =atan((AcX/sqrt(squaresum_R))*RAD_TO_DEG);
pitch =(P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc);//pitch=P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
roll =(P_CompCoeff*roll + (1.0f-P_CompCoeff)*rollAcc);
/*
if-statements to make the roll command go to where it is meant to go,
i.e clockwise/counterclockwise rotation
*/
if (pitch < -158)
{
pitched = abs(pitch + 158);
pitched = pitched - 158;
}
else if (pitch > -156)
{
pitched = abs(156 + pitch);
pitched = -156 - pitched;
}
//locked movement for upward direction of pitch
if (pitched < -240)
{
pitched = -240;
}
//Servo commands, roll/pitch + nr, where nr is compensation for mountingto start horizontally
myservo1.write((roll + 120));
myservo2.write(pitched + 340);
}