![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
AutoMotive Instrumetns
Dependencies: LSM9DS1_Library_cal SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed
main.cpp
- Committer:
- clively6
- Date:
- 2016-05-02
- Revision:
- 1:1de62c8928f8
- Parent:
- 0:93a950d0e18a
File content as of revision 1:1de62c8928f8:
#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); } }