#include

#include

const int MPU$\_$addr$=0$x$68$; $//$ I$2$C address of the MPU$-6050$

int$16\_$t AcX,AcY,AcZ,Tmp$,$GyX$,$GyY$,$GyZ;

float delta$\_$t $=0\mathrm{.}005$;

float pitchAcc,rollAcc, pitch, roll, pitched;

float P$\_$CompCoeff$=0\mathrm{.}98$;

Servo myservo$1,$ myservo$2$;

void setup$()\{$

Wire.begin$()$;

Wire.beginTransmission$($MPU$\_$addr$)$;

Wire.write$(0$x$6$B$)$; $//$ PWR$\_$MGMT$\_1$ register

Wire.write$(0)$; $//$ set to zero $($wakes up the MPU$-6050)$

Wire.endTransmission$($true$)$;

Serial.begin$(115200)$;

myservo$1.$attach$(10)$;

myservo$2.$attach$(11)$;

Serial.begin$(9600)$;

$\}$

void loop$()\{$

Wire.beginTransmission$($MPU$\_$addr$)$;

Wire.write$(0$x$3$B$)$; $//$ starting with register $0$x$3$B $($ACCEL$\_$XOUT$\_$H$)$

Wire.endTransmission$($false$)$;

Wire.requestFrom$($MPU$\_$addr,$14,$true$)$; $//$ request a total of $14$ registers

AcX$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$3$B $($ACCEL$\_$XOUT$\_$H$)$ & $0$x$3$C$($ACCEL$\_$XOUT$\_$L$)$

AcY$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$3$D $($ACCEL$\_$YOUT$\_$H$)$ & $0$x$3$E$($ACCEL$\_$YOUT$\_$L$)$

AcZ$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$3$F $($ACCEL$\_$ZOUT$\_$H$)$ & $0$x$40($ACCEL$\_$ZOUT$\_$L$)$

Tmp$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$41($TEMP$\_$OUT$\_$H$)$ & $0$x$42($TEMP$\_$OUT$\_$L$)$

GyX$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$43($GYRO$\_$XOUT$\_$H$)$ & $0$x$44($GYRO$\_$XOUT$\_$L$)$

GyY$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$45($GYRO$\_$YOUT$\_$H$)$ & $0$x$46($GYRO$\_$YOUT$\_$L$)$

GyZ$=$Wire.read$()<<8|$Wire$.$read$()$; $//0$x$47($GYRO$\_$ZOUT$\_$H$)$ & $0$x$48($GYRO$\_$ZOUT$\_$L$)$

$//$ Serial.print$($AcX$)$;

$//$ Serial.print$("\backslash$t$")$;

$//$ Serial.print$($AcY$)$;

$//$ Serial.print$("\backslash$t$")$;

$//$ Serial.print$($AcZ$)$;

$//$ Serial.print$("\backslash$t$")$;

$//$ Serial.print$($GyX$)$;

$//$ Serial.print$("\backslash$t$")$;

$//$ Serial.print$($GyY$)$;

$//$ Serial.print$("\backslash$t$")$;

$//$ Serial.println$($GyZ$)$;

$//$Complementary filter

long squaresum$\_$P$=(($long$)$GyY$*$GyY$+($long$)$AcY$*$AcY$)$;

long squaresum$\_$R$=(($long$)$GyX$*$GyX$+($long$)$AcX$*$AcX$)$;

pitch$+=((-$AcY$/40\mathrm{.}8$f$)*($delta$\_$t$))$;

roll$+=((-$AcX$/45\mathrm{.}8$f$)*($delta$\_$t$))$; $//32\mathrm{.}8$

pitchAcc$=$ atan$(($AcY$/$sqrt$($squaresum$\_$P$))*$RAD$\_$TO$\_$DEG$)$;

rollAcc $=$atan$(($AcX$/$sqrt$($squaresum$\_$R$))*$RAD$\_$TO$\_$DEG$)$;

pitch $=($P$\_$CompCoeff$*$pitch $+(1\mathrm{.}0$f$-$P$\_$CompCoeff$)*$pitchAcc$)$;$//$pitch

P$\_$CompCoeff$*$pitch $+(1\mathrm{.}0$f$-$P$\_$CompCoeff$)*$pitchAcc;

roll $=($P$\_$CompCoeff$*$roll $+(1\mathrm{.}0$f$-$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

myservo$1.$write$(($roll $+120))$;

myservo$2.$write$($pitched $+340)$;

Serial.print$($roll$+120)$;

Serial.print$("")$;

Serial.println$($pitched$+340)$;

$\}$

i have this code for selfstabalizing spoon, but the problem is that our servo$2$ which is related to pitching is not responding, also the gyroscope values is constant at $180,$ but it must changes the angle as the object is shaking $,$ so i need anyone to provide a correct code with same connection