line_sensors.cpp

00001 #include "line_sensors.h"
00002 
00003 DigitalInOut lineSensorsDigital[LINE_SENSOR_COUNT] = {
00004     DigitalInOut(p18), // white wire, left-most sensor
00005     DigitalInOut(p19), // orange wire, middle sensor
00006     DigitalInOut(p20), // brown wire, right-most sensor
00007 };
00008 
00009 void readSensors(uint16_t * values)
00010 {
00011     for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
00012     {
00013         values[i] = 1000;
00014         lineSensorsDigital[i].mode(PullNone);
00015         lineSensorsDigital[i].output();
00016         lineSensorsDigital[i].write(1);
00017     }
00018     
00019     wait_us(10);
00020     
00021     for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
00022     {
00023         lineSensorsDigital[i].input();
00024     }
00025 
00026     Timer timer;
00027     timer.start();
00028 
00029     while(timer.read_us() < 1000)
00030     {
00031         for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
00032         {
00033             if (values[i] == 1000 && lineSensorsDigital[i].read() == 0)
00034             {
00035                 values[i] = timer.read_us();   
00036             }
00037         }
00038     }
00039 }
00040 
00041 
00042 /**
00043 uint16_t analogReadWithFilter(AnalogIn * input)
00044 {
00045     uint16_t readings[3];
00046     for(uint8_t i = 0; i < 3; i++)
00047     {
00048         readings[i] = input->read_u16();   
00049     }
00050     
00051     if (readings[0] <= readings[1] && readings[0] >= readings[2])
00052     {
00053         return readings[0];
00054     }
00055     if (readings[1] <= readings[0] && readings[1] >= readings[2])
00056     {
00057         return readings[1];
00058     }
00059     return readings[2];
00060 }
00061 **/