         // calculate the current
         current = (((float)currentValue * V_REF) / ADC_STATES - CURRENT_OFFSET) * CURRENT_MULTIPLIER;
 
-        log_data(latitude, longitude, speed, angle, altitude, voltage, current);
-        // TODO: get pedal positions
+        //log_data(latitude, longitude, speed, angle, altitude, voltage, current);
+        print_data(latitude, longitude, speed, angle, altitude, voltage, current);
+        //print_gps();
     }
 }
 
+void log_data(float latitude, float longitude, float speed, float angle, float altitude, float voltage, float current) {
+    if (logFile) {
+        unsigned long time = millis();
+        sprintf(outputBuffer, "%10d, %12.8f, %12.8f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f", time, latitude, longitude, speed, angle, altitude, voltage, current);
+        logFile.println(outputBuffer);
+    }
+}
+
+void flush_log() {
+    if (logFile) {
+        logFile.flush();
+    }
+}
+
+void print_data(float latitude, float longitude, float speed, float angle, float altitude, float voltage, float current) {
+    unsigned long time = millis();
+    sprintf(outputBuffer, "%10d, %12.8f, %12.8f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f", time, latitude, longitude, speed, angle, altitude, voltage, current);
+    Serial.println(outputBuffer);
+}
+
+void print_gps() {
+    Serial.print(gps->hour, DEC); Serial.print(':');
+    Serial.print(gps->minute, DEC); Serial.print(':');
+    Serial.print(gps->seconds, DEC); Serial.print('.');
+    Serial.println(gps->milliseconds);
+    Serial.print("Date: ");
+    Serial.print(gps->day, DEC); Serial.print('/');
+    Serial.print(gps->month, DEC); Serial.print("/20");
+    Serial.println(gps->year, DEC);
+    Serial.print("Fix: "); Serial.print((int)gps->fix);
+    Serial.print(" quality: "); Serial.println((int)gps->fixquality); 
+    Serial.print("Satellites: "); Serial.println((int)gps->satellites);
+}
+

#define VOLTAGE_PIN A2
#define CURRENT_PIN A5
#define GPSECHO false
#define LOGGING_DELAY 1000


SoftwareSerial gpsSerial(8, 7);
Adafruit_GPS GPS(&gpsSerial);
uint32_t timer = millis();
File logFile;


void setup() {
    // connect at 115200 so we can read the GPS fast enough
    Serial.begin(115200);
    //while (!Serial); // wait for leo to be ready
    delay(5000);

    pinMode(VOLTAGE_PIN, INPUT);
    pinMode(CURRENT_PIN, INPUT);

    // SD card: pin 10 for chip select, 11 for MOSI, 12 for MISO and 13 for SCK
    //if (!SD.begin(10)) {
    if (!SD.begin(10, 11, 12, 13)) {
        Serial.println("SD initialization failed!");
        while(1);

        float speed = 0.0;  // in knots
        float angle = 0.0;
        float altitude = 0.0;  // in meters
        if (gps->fix) {
            latitude = gps->latitude / 100.0;
            longitude = gps->longitude / 100.0;
            speed = gps->speed;
            angle = gps->angle;
            altitude = gps->altitude;
        }

        int voltsValue = analogRead(VOLTAGE_PIN);
