Diff: main.cpp

Revision:

0:93a950d0e18a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 02 15:09:33 2016 +0000
@@ -0,0 +1,227 @@
+#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);
+    }
+}
\ No newline at end of file