Dependents: controller_with_imu controller_with_autostop ros_openroach
TSL1401CL.cpp@0:57ff45e3cb20, 2017-05-30 (annotated)
- Committer:
- yxyang
- Date:
- Tue May 30 06:41:58 2017 +0000
- Revision:
- 0:57ff45e3cb20
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yxyang | 0:57ff45e3cb20 | 1 | #include "TSL1401CL.h" |
yxyang | 0:57ff45e3cb20 | 2 | |
yxyang | 0:57ff45e3cb20 | 3 | TSL1401CL::TSL1401CL(PinName pin_clk, PinName pin_si, PinName pin_adc) : clk(pin_clk), si(pin_si), adc(pin_adc), integration_time(0) |
yxyang | 0:57ff45e3cb20 | 4 | { |
yxyang | 0:57ff45e3cb20 | 5 | clk.write(0); |
yxyang | 0:57ff45e3cb20 | 6 | integration_timer.start(); |
yxyang | 0:57ff45e3cb20 | 7 | } |
yxyang | 0:57ff45e3cb20 | 8 | |
yxyang | 0:57ff45e3cb20 | 9 | bool TSL1401CL::integrationReady() |
yxyang | 0:57ff45e3cb20 | 10 | { |
yxyang | 0:57ff45e3cb20 | 11 | return static_cast<uint32_t>(integration_timer.read_us()) >= integration_time; |
yxyang | 0:57ff45e3cb20 | 12 | } |
yxyang | 0:57ff45e3cb20 | 13 | |
yxyang | 0:57ff45e3cb20 | 14 | void TSL1401CL::read() |
yxyang | 0:57ff45e3cb20 | 15 | { |
yxyang | 0:57ff45e3cb20 | 16 | integration_timer.reset(); |
yxyang | 0:57ff45e3cb20 | 17 | integration_timer.start(); |
yxyang | 0:57ff45e3cb20 | 18 | clk.write(0); |
yxyang | 0:57ff45e3cb20 | 19 | si.write(1); |
yxyang | 0:57ff45e3cb20 | 20 | clk.write(1); |
yxyang | 0:57ff45e3cb20 | 21 | si.write(0); |
yxyang | 0:57ff45e3cb20 | 22 | |
yxyang | 0:57ff45e3cb20 | 23 | for (uint8_t i = 0; i < TSL1401CL_PIXEL_COUNT; i++) { |
yxyang | 0:57ff45e3cb20 | 24 | clk.write(0); |
yxyang | 0:57ff45e3cb20 | 25 | data[i] = adc.read_u16(); |
yxyang | 0:57ff45e3cb20 | 26 | clk.write(1); |
yxyang | 0:57ff45e3cb20 | 27 | } |
yxyang | 0:57ff45e3cb20 | 28 | clk.write(0); |
yxyang | 0:57ff45e3cb20 | 29 | } |
yxyang | 0:57ff45e3cb20 | 30 | |
yxyang | 0:57ff45e3cb20 | 31 | void TSL1401CL::setIntegrationTime(uint32_t int_time_desired) |
yxyang | 0:57ff45e3cb20 | 32 | { |
yxyang | 0:57ff45e3cb20 | 33 | integration_time = int_time_desired; |
yxyang | 0:57ff45e3cb20 | 34 | } |
yxyang | 0:57ff45e3cb20 | 35 | |
yxyang | 0:57ff45e3cb20 | 36 | int TSL1401CL::findLineEdge (uint16_t threshold, uint8_t precision, size_t crop_amount, bool invert) |
yxyang | 0:57ff45e3cb20 | 37 | { |
yxyang | 0:57ff45e3cb20 | 38 | if(precision == 0) precision = 1; |
yxyang | 0:57ff45e3cb20 | 39 | |
yxyang | 0:57ff45e3cb20 | 40 | // Create an array with resolution determined by precision containing the change between points in data |
yxyang | 0:57ff45e3cb20 | 41 | uint16_t change_len = (TSL1401CL_PIXEL_COUNT - 2*crop_amount)/precision - 1; |
yxyang | 0:57ff45e3cb20 | 42 | int32_t change_in_data[change_len]; |
yxyang | 0:57ff45e3cb20 | 43 | for(uint16_t i = crop_amount, j = 0; i < TSL1401CL_PIXEL_COUNT - crop_amount - precision; i += precision, j++) { |
yxyang | 0:57ff45e3cb20 | 44 | change_in_data[j] = (int)data[i+precision] - (int)data[i]; |
yxyang | 0:57ff45e3cb20 | 45 | } |
yxyang | 0:57ff45e3cb20 | 46 | |
yxyang | 0:57ff45e3cb20 | 47 | // Find the index of the maximum value in change_in_data |
yxyang | 0:57ff45e3cb20 | 48 | uint16_t maxchange = 0; |
yxyang | 0:57ff45e3cb20 | 49 | for(uint16_t i = 1; i < change_len; i++) { |
yxyang | 0:57ff45e3cb20 | 50 | if(invert) { |
yxyang | 0:57ff45e3cb20 | 51 | if(change_in_data[i] < change_in_data[maxchange]) maxchange = i; |
yxyang | 0:57ff45e3cb20 | 52 | } else { |
yxyang | 0:57ff45e3cb20 | 53 | if(change_in_data[i] > change_in_data[maxchange]) maxchange = i; |
yxyang | 0:57ff45e3cb20 | 54 | } |
yxyang | 0:57ff45e3cb20 | 55 | } |
yxyang | 0:57ff45e3cb20 | 56 | |
yxyang | 0:57ff45e3cb20 | 57 | // If there isn't a sharp enough change returns -1 |
yxyang | 0:57ff45e3cb20 | 58 | if((change_in_data[maxchange] >= threshold && !invert) || (change_in_data[maxchange]*-1 >= threshold && invert)) { |
yxyang | 0:57ff45e3cb20 | 59 | return maxchange*precision + crop_amount; |
yxyang | 0:57ff45e3cb20 | 60 | } |
yxyang | 0:57ff45e3cb20 | 61 | else return -1; |
yxyang | 0:57ff45e3cb20 | 62 | } |
yxyang | 0:57ff45e3cb20 | 63 | |
yxyang | 0:57ff45e3cb20 | 64 | int TSL1401CL::findLineCenter (uint16_t threshold, uint8_t precision, size_t crop_amount) |
yxyang | 0:57ff45e3cb20 | 65 | { |
yxyang | 0:57ff45e3cb20 | 66 | if(precision == 0) precision = 1; |
yxyang | 0:57ff45e3cb20 | 67 | |
yxyang | 0:57ff45e3cb20 | 68 | // Create an array with resolution determined by precision containing the change between points in data |
yxyang | 0:57ff45e3cb20 | 69 | uint16_t change_len = (TSL1401CL_PIXEL_COUNT - 2*crop_amount)/precision - 1; |
yxyang | 0:57ff45e3cb20 | 70 | int32_t change_in_data[change_len]; |
yxyang | 0:57ff45e3cb20 | 71 | for(uint16_t i = crop_amount, j = 0; i < TSL1401CL_PIXEL_COUNT - crop_amount - precision; i += precision, j++) { |
yxyang | 0:57ff45e3cb20 | 72 | change_in_data[j] = (int)data[i+precision] - (int)data[i]; |
yxyang | 0:57ff45e3cb20 | 73 | } |
yxyang | 0:57ff45e3cb20 | 74 | |
yxyang | 0:57ff45e3cb20 | 75 | // Find the index of the maximum and minimum value in change_in_data |
yxyang | 0:57ff45e3cb20 | 76 | uint16_t maxchange = 0; |
yxyang | 0:57ff45e3cb20 | 77 | uint16_t minchange = 0; |
yxyang | 0:57ff45e3cb20 | 78 | for(uint16_t i = 1; i < change_len; i++) { |
yxyang | 0:57ff45e3cb20 | 79 | if(change_in_data[i] > change_in_data[maxchange]) maxchange = i; |
yxyang | 0:57ff45e3cb20 | 80 | if(change_in_data[i] < change_in_data[minchange]) minchange = i; |
yxyang | 0:57ff45e3cb20 | 81 | } |
yxyang | 0:57ff45e3cb20 | 82 | |
yxyang | 0:57ff45e3cb20 | 83 | // Return the average of the max and min value |
yxyang | 0:57ff45e3cb20 | 84 | if(change_in_data[maxchange] >= threshold && change_in_data[minchange]*-1 >= threshold) { |
yxyang | 0:57ff45e3cb20 | 85 | return (precision*maxchange+precision*minchange)/2 + crop_amount; |
yxyang | 0:57ff45e3cb20 | 86 | } |
yxyang | 0:57ff45e3cb20 | 87 | return -1; |
yxyang | 0:57ff45e3cb20 | 88 | } |
yxyang | 0:57ff45e3cb20 | 89 | |
yxyang | 0:57ff45e3cb20 | 90 | uint32_t TSL1401CL::getData(uint8_t index) |
yxyang | 0:57ff45e3cb20 | 91 | { |
yxyang | 0:57ff45e3cb20 | 92 | return data[index]; |
yxyang | 0:57ff45e3cb20 | 93 | } |