File content as of revision 0:93a950d0e18a:

#include <string>
#include <iostream>
#include "mbed.h"
#include "rtos.h"
#include "LSM9DS1.h"
#include "StepperMotor_X27168.h"
#include "SDFileSystem.h"

#define PI 3.14159
#define DECLINATION -4.94

LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
InterruptIn encoder(p12);
InterruptIn crank(p13);
InterruptIn pb(p14);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut flash(LED4);
Serial pc(USBTX, USBRX);
StepperMotor_X27168 smotor_rpm(p23, p22, p25, p26);
StepperMotor_X27168 smotor_speed(p28, p27, p29, p30);
SDFileSystem sd(p5, p6, p7, p8, "sd");

int ticks = 0;
int crank_ticks = 0;
int crank_pulses = 4;
int num_pulses = 7;
float dist = 0.0; // miles
float compass = 0.0; // degrees
float gear_ratio = 1.0/48.0;
float rpm_shaft = 0.0;
float rpm = 0.0;
float wheel_w = 0.0;
float tire_circumference = 5.925; // measure in feet
float sampling_t_rpm = 0.1;//Vehicle Speed Sensor sample rate
float sampling_t_compass = 0.5;//Compass sampling time
float sampling_t_motor = 0.33;//RPM sampling rate
float sampling_t_distance = 5.0;//Distance->GUI update rate
float speed = 0.0;
float gx = 0.0;
float gy = 0.0;
float gx_log[30];
int i = 0;
int pb_set = 0;
int rpm_counter = 0;
float rpm_values[10];

float rpm_avg = 0.0;

void increment_ticks() {
    ticks++;
    led1 = !led1;
}

void increment_cranks() {
    crank_ticks++;
    led2 = !led2;
}

void set_pb() {
    pb_set = 1;
}

void log_G_data() {
     
        FILE *fp = fopen("/sd/Gdata.txt", "w");
        
        if (fp) {
            for( int i = 0; i < 30; i++ )
            {
                fprintf( fp,"\n%1.4f, \n", gx_log[ i ] );
            }
            fclose(fp);
            //pc.printf("Saved!\n\r");
        } else {
           // pc.printf("Error saving G data\n\r");
        }
    
}

void motor_thread(void const *args) {
    while (1) {
        //smotor.step_position(rpm_avg / 10);
        smotor_rpm.set_position_dynamic(rpm_avg / 10);
        smotor_speed.set_position_dynamic(speed * 4.8);
        
        Thread::wait(sampling_t_motor);
    }   
}

void distance_thread(void const *args) {
    while (1) {
        //pc.printf("Saving distance (%.1f)...\n\r", dist);
        
        FILE *fp = fopen("/sd/distance.txt", "w");
        
        if (fp) {
            fprintf(fp, "%.1f", dist);
            fclose(fp);
            //pc.printf("Saved!\n\r");
        } else {
           // pc.printf("Error saving distance!\n\r");
        }

        Thread::wait(sampling_t_distance * 1000);
    }
}

void rpm_thread(void const *args) {
    float rpm_sum;
    
    while (1) {
        // calculate RPM, speed, and distance
        wheel_w = (ticks / (num_pulses * sampling_t_rpm)) * 60.0;
        speed = (wheel_w * tire_circumference) / 88.0; // 88 = 5280 feet / 60 minutes
        dist += ((ticks / num_pulses) * tire_circumference) / 5280;
        rpm = (crank_ticks / (crank_pulses * sampling_t_rpm)) * 60.0;
        
        // running average
        if (rpm_counter >= 5) rpm_counter = 0;
        rpm_values[rpm_counter] = rpm;
        rpm_counter++;
        
        for (int i = 0; i <= 5; i++) {
            rpm_sum += rpm_values[i];  
        }
        
        rpm_avg = rpm_sum / 5;
        rpm_sum = 0.0;
        
        // reset ticks
        ticks = 0;
        crank_ticks = 0;
        
        Thread::wait(sampling_t_rpm * 1000);
    }
}

void compass_thread(void const *args) {
    float mx, my;

    while (1) {
        while(!imu.magAvailable(X_AXIS));
        imu.readMag();
        
        while(!imu.accelAvailable());
        imu.readAccel();
    
        gx = imu.calcAccel(imu.ax);
        gy = imu.calcAccel(imu.ay);
    
        mx = -imu.calcMag(imu.mx);
        my = imu.calcMag(imu.my);
        
        if (my == 0.0) compass = (mx < 0.0) ? 180.0 : 0.0;
        else compass = atan2(mx, my) * 360.0 / (2.0 * PI);
        
        compass -= DECLINATION;
        if (compass > 180.0) compass = compass - 360.0;
        else if (compass < -180.0) compass = 360.0 + compass;
        else if (compass < 0.0) compass = 360.0  + compass;
        
        /*Log G-data*/
        if(pb_set && i < 30)
        {
            gx_log[i] = gx;
            i++;
        }
        if(i == 30)
        {
            i = 0;
            pb_set = 0;
            log_G_data();
        }
        
        
        //compass *= 180.0 / PI;
        Thread::wait(sampling_t_compass * 1000);
    }
}

int main() {
    char line[1024];
    pc.printf("Starting...\n\r");
    
    // initialize IMU unit
    imu.begin();
    if (!imu.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n\r");
    }
    
    // load distance data from sd card
    FILE *fp = fopen("/sd/distance.txt", "r");
    
    if (fp) {
        fgets(line, 1024, fp);
        dist = (float)strtod(line, NULL);
    }
    
    // sweep the stepper motor in order to reset its position
    pc.printf("&%5.4f,%5.4f,%5.4f,%5.4f,%5.4f,%5.4f&\n", 629.0, 0.0, dist, 0.0, 0.0, 0.0);
    smotor_rpm.set_max_speed(1000);
    smotor_speed.set_max_speed(1000);
    smotor_rpm.set_position_dynamic(629);
    smotor_speed.set_position_dynamic(629);
    wait(0.2);
    smotor_rpm.set_position_dynamic(0);
    smotor_speed.set_position_dynamic(0);
    pc.printf("&%5.4f,%5.4f,%5.4f,%5.4f,%5.4f,%5.4f&\n", 0.0, 0.0, dist, 0.0, 0.0, 0.0);
    
    encoder.rise(&increment_ticks);
    crank.rise(&increment_cranks);
    pb.rise(&set_pb);

    Thread thread1(rpm_thread);
    Thread thread2(compass_thread);
    Thread thread3(motor_thread);
    Thread thread4(distance_thread);
    
    while(1) {
        // rmp,speed,distance,compass
        pc.printf("&%5.4f,%5.4f,%5.4f,%5.4f,%5.4f,%5.4f&\n", rpm, speed, dist, compass, gx, gy);
        //pc.printf("RPM: %5.4f\n\r", rpm_avg);
        flash = !flash;
        wait(0.25);
    }
}