AutoMotive Instrumetns
Dependencies: LSM9DS1_Library_cal SDFileSystem Stepper_Motor_X27168 mbed-rtos mbed
main.cpp@0:93a950d0e18a, 2016-05-02 (annotated)
- Committer:
- clively6
- Date:
- Mon May 02 15:09:33 2016 +0000
- Revision:
- 0:93a950d0e18a
Gets speed and RPM data, displays on stepper motors
Who changed what in which revision?
User | Revision | Line number | New 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 | } |