Servo motor won't work right

Here is my code. I may have put in wrong integers or something, but I have it set to read the altimeter and move when it reaches 2 meters (this is to test to make sure everything works. It will actually be deployed at 270 meters). It moves only a small amount, and it only does that the moment we plug it in then never again. If any of y’all could give me some tips it would be much appreciated!

#include <Wire.h>
#include <MS5611.h>
#include “I2Cdev.h”
#include “MPU6050.h”
#include <SPI.h>
#include <SD.h>
#include <Servo.h>

Servo myServo;
int state = 0;
unsigned long time;
const int chipSelect = 4;
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include “Wire.h”
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // ← use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;

MS5611 ms5611;
double referencePressure;
#define LED_PIN 13
bool blinkState = false;

void setup()
// join I2C bus (I2Cdev library doesn’t do this automatically)
Fastwire::setup(400, true);


// Initialize MS5611 sensor
Serial.println(“Initialize MS5611 Sensor”);

while (!ms5611.begin())
Serial.println(“Could not find a valid MS5611 sensor, check wiring!”);

// Get reference pressure for relative altitude
referencePressure = ms5611.readPressure();

// Check settings

// initialize device
// Serial.println(“Initializing I2C devices…”);

// verify connection
// Serial.println(“Testing device connections…”);
// Serial.println(accelgyro.testConnection() ? “MPU6050 connection successful” : “MPU6050 connection failed”);

// Serial.print(“Initializing SD card…”);
if (!SD.begin(chipSelect)) {// see if the card is present and can be initialized:
// Serial.println(“Card failed, or not present”);
// don’t do anything more:
// Serial.println(“card initialized.”);


void checkSettings()
// Serial.print("Oversampling: ");
// Serial.println(ms5611.getOversampling());

void loop()

// read raw accel measurements from device
accelgyro.getAcceleration(&ax, &ay, &az);

// Read raw pressure and temp values
uint32_t rawTemp = ms5611.readRawTemperature();
uint32_t rawPressure = ms5611.readRawPressure();

// Read true temperature & Pressure
double realTemperature = ms5611.readTemperature();
long realPressure = ms5611.readPressure();

// Calculate altitude
float absoluteAltitude = ms5611.getAltitude(realPressure);
float relativeAltitude = ms5611.getAltitude(realPressure, referencePressure);

//change variables to strings
String comma = String (’,’);
String absalt = String (ms5611.getAltitude(realPressure)); //absoloute altitude in meters.
String temp = String ( ms5611.readTemperature()); //real temp in degrees, may over read initially, will take time for sensor to stabilize.
String realpressure = String (ms5611.readPressure()); // pressure in pascals
String accelXraw = String (ax); // raw accel in X. Divide by 2048 to get a “G” reading.
String timer = String (millis()); //puts a millis time stamp on each string.
//make a big string containing above strings
String Baro_data = String (absalt + comma + temp + comma + realpressure + comma + accelXraw + comma + timer) ;

Serial.println (Baro_data);
File dataFile =“datalog.txt”, FILE_WRITE);
// if the file is available, write to it:
if (dataFile) {
dataFile.println(Baro_data);//put Baro_data on the SD card

if (millis() > 10000) //change this number to change alarm delay (1s = 1000ms)
{ tone (8, 1000); // change the second number to alter the tone of the peizo alarm

else {


Please use code tags when posting code (</> icon). We need more info about your project to be able to help much. Schematics, description?

Where is ‘gy’ ever set to any value?


The easier you make it to read and copy the code the more likely it is that you will get help

Please follow the advice given in the link below when posting code

//change variables to strings

Why ?

  if (gy >= 2)

Where in the code does the value of gy change ?

This is to launch a rocket and read altitude, speed, etc, and deploy a parachute.

But where is the value of gy ever changed ?

Idk why I put that there. I realize now that I should have it set to the altitude. The changed version would be


That looks wrong too. Although I don’t know the library, should it be

if(ms5611.getAltitude(realPressure) >= 2)

Another point. Although you can test the return value of a function it is often best to read the value into a variable and test that. Doing this helps with debugging when you need to know the value