[Programming] main_code 5.0 Beta

/* Requires the SD library from the Adafruit website * SD card attached MOSI - pin 11, MISO - pin 12, CLK - pin 13, CS - pin 10*/
#include <SD.h>
#include <Servo.h>
#include "Wire.h"
#include "Adafruit_BMP085.h"
Adafruit_BMP085 bmp;
Servo myservo;  // create servo object to control a servo
const int chipSelect = 10; //??
File dataFile;
//global varibles
long initialPressure=0;
int counter = 0; //counter for flush
int flushRate = 100; //the rate of flush
//long topP;
//int floors;
//long PPerFloor= (topP-initialPressure)/floors;
//------------------------------------------------------------------------------------------- <setup>
void setup(){ 
 
  //>>>>>>>>>>>>>>>> servo setup
  myservo.attach(9); // servo-pin: pin9
 
  //>>>>>>>>>>>>>>>> pressure sensor setup
   bmp.begin();
  
  //>>>>>>>>>>>>>>>> SD card setup
 
  //----------------------------------  [initialize SD card]
  Serial.begin(57600);  // initialize serial communications: 57600
  while (!Serial) { // wait for serial port to connect. (Needed for Leonardo only) 
    ;
  }
  Serial.println("Initializing SD card..."); // (make sure that the default chip select pin is set to output, even if you don't use it)
  pinMode(SS, OUTPUT);   
  if (!SD.begin(chipSelect)) {  // see if the card is present and can be initialized:
    Serial.println("Card failed, or not present");      
    while (1) ; // if card failed, don't do anything more:
  } 
  Serial.println("card initialized sucessfully!");
 
  //-----------------------------------  [search for the last file and create a new file]
  char filename[] = "LOGGER00.TXT";
  for (uint8_t i = 0; i < 100; i++) {  
    filename[6] = i/10 + '0';  
    filename[7] = i%10 + '0';   
    if (! SD.exists(filename)) { // open a new file if it doesn't exist        
      dataFile = SD.open(filename, FILE_WRITE);    
      break;    
    } 
  }  
  if (! dataFile) {  // check if a new file is found
    Serial.println("couldnt create file"); 
  }
 
  //------------------------------------------------------ [the new file is found]
  delay(500);
  Serial.print("Logging to: "); 
  Serial.println(filename);
 
  //>>>>>>>>>>>>>>>>>> pressure sensor setup
  //------------------------------------------------------- [Calibrate pressure]
  Serial.print("Calibrate Pressure: ");
  for (int j = 0 ; j < 10; j++)
  {
    initialPressure += long(bmp.readPressure());
    delay(50);
  }
  initialPressure = initialPressure / 10;  //Average the initial pressure
  Serial.println(initialPressure);
  Serial.println();
  delay(5000);
}


//--------------------------------------------------------------------------------------------  <loop>
String accelRead()//read data from accelerometer
{
  String data = "";
  int a0 = analogRead(0);
  int a1 = analogRead(1);
 
  Serial.print(a0);
  Serial.print(",");
  Serial.print(a1);
 
  data += String(a0);
  data += ",";
  data += String(a1);
 
  return data;
}
String pressureRead()//read data from pressure
{
  String data = "";
  int t=bmp.readTemperature();
  long p=bmp.readPressure();
 
  Serial.println();
  Serial.print("T = ");
  Serial.print(t);
  Serial.println(" *C");
  Serial.print("P = ");
  Serial.print(p);
  Serial.println(" Pa");
  Serial.println();
 
  data += String(t);
  data += ",";
  data += String(p);
 
  return data;
}
int SDwrite(File inputFile, String data, int i) //write data into SD card and flush in certain rate
{
  inputFile.print(data);
  if (i>flushRate)//flush data into SD card. flush rate: 100
  {     
    dataFile.flush(); 
    i = -1; 
  }
  return i+1;
}
int SDwriteNL(File inputFile, int i)
{
  inputFile.println();
  if (i>flushRate)
  {     
    dataFile.flush(); 
    i = -1; 
  }
  return i+1;
}
long moveServoUp(const long bottomP, const long pressurePerFloor, const int movePerFloor, long lastP) // servo move according to altitude
{
  long p=bmp.readPressure();
  int i;
  if(lastP==0)
  {
    lastP=p;
  }
  if(p<lastP)
  {
    i= (bottomP-p)/ pressurePerFloor;
    lastP=p;
  }
  else
  {
    i= (bottomP-lastP)/ pressurePerFloor; 
  }
  if(i<0)
    i=0;
  myservo.write(movePerFloor*i);
 
  Serial.print("Floor: ");
  Serial.println(i);
  //Serial.println(p);
  //Serial.println(lastP);
 
  return lastP;
}
long LP=0; //LP= lastPressure has recorded
void loop()
{
  //----------------------------------- [read and write data]
  SDwrite(dataFile, accelRead(), 0); //less than flushRate forever: save time on calculation
  SDwrite(dataFile, ",", 0);
  SDwrite(dataFile, pressureRead(), 0);
  counter= SDwriteNL(dataFile,counter);
 
  
  //------------------------------------ [servo control]
   LP = moveServoUp(initialPressure, 10, 45, LP);
 
 
  //-------------------------------------- [loop frequence 20Hz]
  delay(50);
}