One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
Diff: main.cpp
- Revision:
- 3:c57674c348bd
- Parent:
- 2:a8adff46eaca
- Child:
- 4:947c3634b649
diff -r a8adff46eaca -r c57674c348bd main.cpp --- a/main.cpp Tue Mar 29 23:31:32 2016 +0000 +++ b/main.cpp Wed Mar 30 01:21:39 2016 +0000 @@ -76,6 +76,8 @@ // Image processing // ================ int max = -1; +float lum_bg = 0; +float contrast = 0; int argmax = 0; int argmin = 0; int temp[128]; @@ -160,7 +162,7 @@ track(); // Set next frame exposure time - camera_read.attach_us(&read_camera, t_int); + // camera_read.attach_us(&read_camera, t_int); } // Find line center from image @@ -169,6 +171,7 @@ // Calculate and return midpoint of argmax and argmin void track() { max = -1; + lum_bg = 0; argmax = 0; argmin = 0; for (int i = 0; i < 107; i++) { @@ -197,11 +200,30 @@ } } + for (int i = 0; i < 30; i++) { + lum_bg = lum_bg + img[64 - 10 - i] / 60.0 + img[64 + 10 + i] / 60.0; + } + + contrast = (max - lum_bg) / lum_bg; + + if (contrast < 1.5) { + // Underexposed + if (max < 55000) { + t_int = t_int + (55000 - max); + } + // Overexposed + if (lum_bg > 25000) { + t_int = t_int - (lum_bg - 25000); + } + } + if (max > 43253) { center = (argmax + argmin + 2 + 11) / 2; a = 88 + (64 - center) * 0.625; servo = a / 180; } + + camera_read.attach_us(&read_camera, t_int); } // ==== @@ -219,7 +241,7 @@ // Initialize & start camera clk = 0; -// read_camera(); + read_camera(); // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); @@ -251,6 +273,6 @@ d = motor_ctrl.compute(); motor = 1.0 - d; wait(interval); - pc.printf("Velocity: %f\r\n", velocity); + pc.printf("BG Luminosity: %f, Feature Luminosity: %d, Contrast: %f, Exposure: %d\r\n", lum_bg, max, contrast, t_int); } } \ No newline at end of file