Casey Lively / Mbed 2 deprecated Auto

AutoMotive Instrumetns

Dependencies:   LSM9DS1_Library_cal SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed

main.cpp@1:1de62c8928f8, 2016-05-02 (annotated)

Committer:
clively6
Date:
Mon May 02 15:11:45 2016 +0000
Revision:
1:1de62c8928f8
Parent:
0:93a950d0e18a 
Automotive instruments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clively6 0:93a950d0e18a 1 #include <string>
clively6 0:93a950d0e18a 2 #include <iostream>
clively6 0:93a950d0e18a 3 #include "mbed.h"
clively6 0:93a950d0e18a 4 #include "rtos.h"
clively6 0:93a950d0e18a 5 #include "LSM9DS1.h"
clively6 0:93a950d0e18a 6 #include "StepperMotor_X27168.h"
clively6 0:93a950d0e18a 7 #include "SDFileSystem.h"
clively6 0:93a950d0e18a 8
clively6 0:93a950d0e18a 9 #define PI 3.14159
clively6 0:93a950d0e18a 10 #define DECLINATION -4.94
clively6 0:93a950d0e18a 11
clively6 0:93a950d0e18a 12 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
clively6 0:93a950d0e18a 13 InterruptIn encoder(p12);
clively6 0:93a950d0e18a 14 InterruptIn crank(p13);
clively6 0:93a950d0e18a 15 InterruptIn pb(p14);
clively6 0:93a950d0e18a 16 DigitalOut led1(LED1);
clively6 0:93a950d0e18a 17 DigitalOut led2(LED2);
clively6 0:93a950d0e18a 18 DigitalOut flash(LED4);
clively6 0:93a950d0e18a 19 Serial pc(USBTX, USBRX);
clively6 0:93a950d0e18a 20 StepperMotor_X27168 smotor_rpm(p23, p22, p25, p26);
clively6 0:93a950d0e18a 21 StepperMotor_X27168 smotor_speed(p28, p27, p29, p30);
clively6 0:93a950d0e18a 22 SDFileSystem sd(p5, p6, p7, p8, "sd");
clively6 0:93a950d0e18a 23
clively6 0:93a950d0e18a 24 int ticks = 0;
clively6 0:93a950d0e18a 25 int crank_ticks = 0;
clively6 0:93a950d0e18a 26 int crank_pulses = 4;
clively6 0:93a950d0e18a 27 int num_pulses = 7;
clively6 0:93a950d0e18a 28 float dist = 0.0; // miles
clively6 0:93a950d0e18a 29 float compass = 0.0; // degrees
clively6 0:93a950d0e18a 30 float gear_ratio = 1.0/48.0;
clively6 0:93a950d0e18a 31 float rpm_shaft = 0.0;
clively6 0:93a950d0e18a 32 float rpm = 0.0;
clively6 0:93a950d0e18a 33 float wheel_w = 0.0;
clively6 0:93a950d0e18a 34 float tire_circumference = 5.925; // measure in feet
clively6 0:93a950d0e18a 35 float sampling_t_rpm = 0.1;//Vehicle Speed Sensor sample rate
clively6 0:93a950d0e18a 36 float sampling_t_compass = 0.5;//Compass sampling time
clively6 0:93a950d0e18a 37 float sampling_t_motor = 0.33;//RPM sampling rate
clively6 0:93a950d0e18a 38 float sampling_t_distance = 5.0;//Distance->GUI update rate
clively6 0:93a950d0e18a 39 float speed = 0.0;
clively6 0:93a950d0e18a 40 float gx = 0.0;
clively6 0:93a950d0e18a 41 float gy = 0.0;
clively6 0:93a950d0e18a 42 float gx_log[30];
clively6 0:93a950d0e18a 43 int i = 0;
clively6 0:93a950d0e18a 44 int pb_set = 0;
clively6 0:93a950d0e18a 45 int rpm_counter = 0;
clively6 0:93a950d0e18a 46 float rpm_values[10];
clively6 0:93a950d0e18a 47
clively6 0:93a950d0e18a 48 float rpm_avg = 0.0;
clively6 0:93a950d0e18a 49
clively6 0:93a950d0e18a 50 void increment_ticks() {
clively6 0:93a950d0e18a 51 ticks++;
clively6 0:93a950d0e18a 52 led1 = !led1;
clively6 0:93a950d0e18a 53 }
clively6 0:93a950d0e18a 54
clively6 0:93a950d0e18a 55 void increment_cranks() {
clively6 0:93a950d0e18a 56 crank_ticks++;
clively6 0:93a950d0e18a 57 led2 = !led2;
clively6 0:93a950d0e18a 58 }
clively6 0:93a950d0e18a 59
clively6 0:93a950d0e18a 60 void set_pb() {
clively6 0:93a950d0e18a 61 pb_set = 1;
clively6 0:93a950d0e18a 62 }
clively6 0:93a950d0e18a 63
clively6 0:93a950d0e18a 64 void log_G_data() {
clively6 0:93a950d0e18a 65
clively6 0:93a950d0e18a 66 FILE *fp = fopen("/sd/Gdata.txt", "w");
clively6 0:93a950d0e18a 67
clively6 0:93a950d0e18a 68 if (fp) {
clively6 0:93a950d0e18a 69 for( int i = 0; i < 30; i++ )
clively6 0:93a950d0e18a 70 {
clively6 0:93a950d0e18a 71 fprintf( fp,"\n%1.4f, \n", gx_log[ i ] );
clively6 0:93a950d0e18a 72 }
clively6 0:93a950d0e18a 73 fclose(fp);
clively6 0:93a950d0e18a 74 //pc.printf("Saved!\n\r");
clively6 0:93a950d0e18a 75 } else {
clively6 0:93a950d0e18a 76 // pc.printf("Error saving G data\n\r");
clively6 0:93a950d0e18a 77 }
clively6 0:93a950d0e18a 78
clively6 0:93a950d0e18a 79 }
clively6 0:93a950d0e18a 80
clively6 0:93a950d0e18a 81 void motor_thread(void const *args) {
clively6 0:93a950d0e18a 82 while (1) {
clively6 0:93a950d0e18a 83 //smotor.step_position(rpm_avg / 10);
clively6 0:93a950d0e18a 84 smotor_rpm.set_position_dynamic(rpm_avg / 10);
clively6 0:93a950d0e18a 85 smotor_speed.set_position_dynamic(speed * 4.8);
clively6 0:93a950d0e18a 86
clively6 0:93a950d0e18a 87 Thread::wait(sampling_t_motor);
clively6 0:93a950d0e18a 88 }
clively6 0:93a950d0e18a 89 }
clively6 0:93a950d0e18a 90
clively6 0:93a950d0e18a 91 void distance_thread(void const *args) {
clively6 0:93a950d0e18a 92 while (1) {
clively6 0:93a950d0e18a 93 //pc.printf("Saving distance (%.1f)...\n\r", dist);
clively6 0:93a950d0e18a 94
clively6 0:93a950d0e18a 95 FILE *fp = fopen("/sd/distance.txt", "w");
clively6 0:93a950d0e18a 96
clively6 0:93a950d0e18a 97 if (fp) {
clively6 0:93a950d0e18a 98 fprintf(fp, "%.1f", dist);
clively6 0:93a950d0e18a 99 fclose(fp);
clively6 0:93a950d0e18a 100 //pc.printf("Saved!\n\r");
clively6 0:93a950d0e18a 101 } else {
clively6 0:93a950d0e18a 102 // pc.printf("Error saving distance!\n\r");
clively6 0:93a950d0e18a 103 }
clively6 0:93a950d0e18a 104
clively6 0:93a950d0e18a 105 Thread::wait(sampling_t_distance * 1000);
clively6 0:93a950d0e18a 106 }
clively6 0:93a950d0e18a 107 }
clively6 0:93a950d0e18a 108
clively6 0:93a950d0e18a 109 void rpm_thread(void const *args) {
clively6 0:93a950d0e18a 110 float rpm_sum;
clively6 0:93a950d0e18a 111
clively6 0:93a950d0e18a 112 while (1) {
clively6 0:93a950d0e18a 113 // calculate RPM, speed, and distance
clively6 0:93a950d0e18a 114 wheel_w = (ticks / (num_pulses * sampling_t_rpm)) * 60.0;
clively6 0:93a950d0e18a 115 speed = (wheel_w * tire_circumference) / 88.0; // 88 = 5280 feet / 60 minutes
clively6 0:93a950d0e18a 116 dist += ((ticks / num_pulses) * tire_circumference) / 5280;
clively6 0:93a950d0e18a 117 rpm = (crank_ticks / (crank_pulses * sampling_t_rpm)) * 60.0;
clively6 0:93a950d0e18a 118
clively6 0:93a950d0e18a 119 // running average
clively6 0:93a950d0e18a 120 if (rpm_counter >= 5) rpm_counter = 0;
clively6 0:93a950d0e18a 121 rpm_values[rpm_counter] = rpm;
clively6 0:93a950d0e18a 122 rpm_counter++;
clively6 0:93a950d0e18a 123
clively6 0:93a950d0e18a 124 for (int i = 0; i <= 5; i++) {
clively6 0:93a950d0e18a 125 rpm_sum += rpm_values[i];
clively6 0:93a950d0e18a 126 }
clively6 0:93a950d0e18a 127
clively6 0:93a950d0e18a 128 rpm_avg = rpm_sum / 5;
clively6 0:93a950d0e18a 129 rpm_sum = 0.0;
clively6 0:93a950d0e18a 130
clively6 0:93a950d0e18a 131 // reset ticks
clively6 0:93a950d0e18a 132 ticks = 0;
clively6 0:93a950d0e18a 133 crank_ticks = 0;
clively6 0:93a950d0e18a 134
clively6 0:93a950d0e18a 135 Thread::wait(sampling_t_rpm * 1000);
clively6 0:93a950d0e18a 136 }
clively6 0:93a950d0e18a 137 }
clively6 0:93a950d0e18a 138
clively6 0:93a950d0e18a 139 void compass_thread(void const *args) {
clively6 0:93a950d0e18a 140 float mx, my;
clively6 0:93a950d0e18a 141
clively6 0:93a950d0e18a 142 while (1) {
clively6 0:93a950d0e18a 143 while(!imu.magAvailable(X_AXIS));
clively6 0:93a950d0e18a 144 imu.readMag();
clively6 0:93a950d0e18a 145
clively6 0:93a950d0e18a 146 while(!imu.accelAvailable());
clively6 0:93a950d0e18a 147 imu.readAccel();
clively6 0:93a950d0e18a 148
clively6 0:93a950d0e18a 149 gx = imu.calcAccel(imu.ax);
clively6 0:93a950d0e18a 150 gy = imu.calcAccel(imu.ay);
clively6 0:93a950d0e18a 151
clively6 0:93a950d0e18a 152 mx = -imu.calcMag(imu.mx);
clively6 0:93a950d0e18a 153 my = imu.calcMag(imu.my);
clively6 0:93a950d0e18a 154
clively6 0:93a950d0e18a 155 if (my == 0.0) compass = (mx < 0.0) ? 180.0 : 0.0;
clively6 0:93a950d0e18a 156 else compass = atan2(mx, my) * 360.0 / (2.0 * PI);
clively6 0:93a950d0e18a 157
clively6 0:93a950d0e18a 158 compass -= DECLINATION;
clively6 0:93a950d0e18a 159 if (compass > 180.0) compass = compass - 360.0;
clively6 0:93a950d0e18a 160 else if (compass < -180.0) compass = 360.0 + compass;
clively6 0:93a950d0e18a 161 else if (compass < 0.0) compass = 360.0 + compass;
clively6 0:93a950d0e18a 162
clively6 0:93a950d0e18a 163 /*Log G-data*/
clively6 0:93a950d0e18a 164 if(pb_set && i < 30)
clively6 0:93a950d0e18a 165 {
clively6 0:93a950d0e18a 166 gx_log[i] = gx;
clively6 0:93a950d0e18a 167 i++;
clively6 0:93a950d0e18a 168 }
clively6 0:93a950d0e18a 169 if(i == 30)
clively6 0:93a950d0e18a 170 {
clively6 0:93a950d0e18a 171 i = 0;
clively6 0:93a950d0e18a 172 pb_set = 0;
clively6 0:93a950d0e18a 173 log_G_data();
clively6 0:93a950d0e18a 174 }
clively6 0:93a950d0e18a 175
clively6 0:93a950d0e18a 176
clively6 0:93a950d0e18a 177 //compass *= 180.0 / PI;
clively6 0:93a950d0e18a 178 Thread::wait(sampling_t_compass * 1000);
clively6 0:93a950d0e18a 179 }
clively6 0:93a950d0e18a 180 }
clively6 0:93a950d0e18a 181
clively6 0:93a950d0e18a 182 int main() {
clively6 0:93a950d0e18a 183 char line[1024];
clively6 0:93a950d0e18a 184 pc.printf("Starting...\n\r");
clively6 0:93a950d0e18a 185
clively6 0:93a950d0e18a 186 // initialize IMU unit
clively6 0:93a950d0e18a 187 imu.begin();
clively6 0:93a950d0e18a 188 if (!imu.begin()) {
clively6 0:93a950d0e18a 189 pc.printf("Failed to communicate with LSM9DS1.\n\r");
clively6 0:93a950d0e18a 190 }
clively6 0:93a950d0e18a 191
clively6 0:93a950d0e18a 192 // load distance data from sd card
clively6 0:93a950d0e18a 193 FILE *fp = fopen("/sd/distance.txt", "r");
clively6 0:93a950d0e18a 194
clively6 0:93a950d0e18a 195 if (fp) {
clively6 0:93a950d0e18a 196 fgets(line, 1024, fp);
clively6 0:93a950d0e18a 197 dist = (float)strtod(line, NULL);
clively6 0:93a950d0e18a 198 }
clively6 0:93a950d0e18a 199
clively6 0:93a950d0e18a 200 // sweep the stepper motor in order to reset its position
clively6 0:93a950d0e18a 201 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);
clively6 0:93a950d0e18a 202 smotor_rpm.set_max_speed(1000);
clively6 0:93a950d0e18a 203 smotor_speed.set_max_speed(1000);
clively6 0:93a950d0e18a 204 smotor_rpm.set_position_dynamic(629);
clively6 0:93a950d0e18a 205 smotor_speed.set_position_dynamic(629);
clively6 0:93a950d0e18a 206 wait(0.2);
clively6 0:93a950d0e18a 207 smotor_rpm.set_position_dynamic(0);
clively6 0:93a950d0e18a 208 smotor_speed.set_position_dynamic(0);
clively6 0:93a950d0e18a 209 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);
clively6 0:93a950d0e18a 210
clively6 0:93a950d0e18a 211 encoder.rise(&increment_ticks);
clively6 0:93a950d0e18a 212 crank.rise(&increment_cranks);
clively6 0:93a950d0e18a 213 pb.rise(&set_pb);
clively6 0:93a950d0e18a 214
clively6 0:93a950d0e18a 215 Thread thread1(rpm_thread);
clively6 0:93a950d0e18a 216 Thread thread2(compass_thread);
clively6 0:93a950d0e18a 217 Thread thread3(motor_thread);
clively6 0:93a950d0e18a 218 Thread thread4(distance_thread);
clively6 0:93a950d0e18a 219
clively6 0:93a950d0e18a 220 while(1) {
clively6 0:93a950d0e18a 221 // rmp,speed,distance,compass
clively6 0:93a950d0e18a 222 pc.printf("&%5.4f,%5.4f,%5.4f,%5.4f,%5.4f,%5.4f&\n", rpm, speed, dist, compass, gx, gy);
clively6 0:93a950d0e18a 223 //pc.printf("RPM: %5.4f\n\r", rpm_avg);
clively6 0:93a950d0e18a 224 flash = !flash;
clively6 0:93a950d0e18a 225 wait(0.25);
clively6 0:93a950d0e18a 226 }
clively6 0:93a950d0e18a 227 }