Animatronic Tail by Circuito.io

// Global variables and defines
int16_t mpu6050Ax, mpu6050Ay, mpu6050Az;
int16_t mpu6050Gx, mpu6050Gy, mpu6050Gz;

int posx = 0;
int posy = 0;

int joystickX;
int joystickY;

int joystickX_Cal = 0;
int joystickY_Cal = 0;
//use button to toggle between sensors
bool sensor_state = 1;

Servo servoUP;  // create servo object to control a servo
Servo servoLEFT;  // create servo object to control a servo
Servo servoRIGHT;  // create servo object to control a servo

MPU6050 mpu6050;
Joystick joystick(JOYSTICK_PIN_VRX, JOYSTICK_PIN_VRY, JOYSTICK_PIN_SW);


/* This code sets up the essentials for your circuit to work. It runs first every time your circuit is powered with electricity. */
void setup() {
  // Setup Serial which is useful for debugging
  // Use the Serial Monitor to view printed messages
  Serial.begin(9600);
  while (!Serial) ; // wait for serial port to connect. Needed for native USB
  Serial.println("start");

  Wire.begin();
  mpu6050.initialize();

  servoUP.attach(SERVO_1_PIN_SIG);  // attaches the servo on pin 9 to the servo object
  servoLEFT.attach(SERVO_2_PIN_SIG);  // attaches the servo on pin 9 to the servo object
  servoRIGHT.attach(SERVO_3_PIN_SIG);  // attaches the servo on pin 9 to the servo object

  // Read Joystick X,Y axis and press
  joystickX_Cal =  joystick.getX();
  joystickY_Cal =  joystick.getY();
  joystickX = joystickX_Cal;
  joystickY = joystickY_Cal;
}

/* This code is the main logic of your circuit. It defines the interaction between the components you selected. After setup, it runs over and over again, in an eternal loop. */
void loop() {
  int joystickSW =  joystick.getSWonPress();
  if (joystickSW)
  {
    sensor_state = !sensor_state;
    //Serial.println("Switch sesnor");
  }

  if (sensor_state)
  {

    joystickX +=  (joystick.getX() - joystickX_Cal);
    joystickY +=  (joystick.getY() - joystickY_Cal);

    joystickX = constrain(joystickX, 0, 1023);
    joystickY = constrain(joystickY, 0, 1023);
    
 /* Serial.print(joystickX);
    Serial.print("\t");
    Serial.println(joystickY);*/
    
    posx = map(joystickX, 0, 1023, 0, 180);
    posy = map(joystickY, 0, 1023, 0, 180);
    
     /*   Serial.print(posx);
        Serial.print("\t");
        Serial.println(posy);*/
    
    servoLEFT.write(posx);              
    servoRIGHT.write(posx);              
    servoUP.write(posy);
  }
  else
  {
    mpu6050.getMotion6(&mpu6050Ax, &mpu6050Ay, &mpu6050Az, &mpu6050Gx, &mpu6050Gy, &mpu6050Gz);   //read accelerometer and gyroscope raw data in three axes
    posy = constrain(map(mpu6050Gx, -16000, 16000, 0, 180), 0, 180);
    posx = constrain(map(mpu6050Gy, -16000, 16000, 0, 180), 0, 180);

    //Serial.print(mpu6050Gy);
/*
    Serial.print(posy);
    Serial.print("\t");
    Serial.println(posx);*/

    //If leaning forward rise up tail
    if (posx < 80)
      servoUP.write(175);
    //If leaning back lower the tail
    else if (posx > 100)
      servoUP.write(5);

    if (posy > 110)
    {
      servoLEFT.write(175);              // tell servo to go to position in variable 'pos'
      servoRIGHT.write(175);              // tell servo to go to position in variable 'pos'

    }
    else if (posy <= 70)
    {
      servoLEFT.write(5);              // tell servo to go to position in variable 'pos'
      servoRIGHT.write(5);              // tell servo to go to position in variable 'pos'

    }
  }




  delay(100);

}