Implementation of the fuzzy control algorithm for obstacle avoidance. This algorithm is for a two-wheel differential drive robot. It uses distance information from 3 analogue IR range sensors and controls DC motors using ESCON servo controllers. This C++ library is written for mbed LPC1768. For any questions and/or reporting bugs contact author: vans.edw@gmail.com

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers sensor.cpp Source File

sensor.cpp

00001 #include "sensor.h"
00002 
00003 Sensor::Sensor(PinName pin) : dist(pin)
00004 {
00005     //
00006     distance = 0.0;
00007 }
00008 
00009 float Sensor::getDist()
00010 {
00011     // Write the code for accurately reading sensor value and 
00012     // converting it to distance value
00013     return LinearInterpolate(dist*3.3);
00014 }
00015 
00016 float Sensor::LinearInterpolate(float volts)
00017 {
00018     /* Using sensor data from the datasheet of the manufacturer,
00019      * this function does a linear interpolation to get the most accurate 
00020      * value for distance based on the input voltage.
00021      */
00022     if (volts >= voltage[0])
00023     {
00024         distance = meters[0];
00025     }
00026 
00027     else if (volts <= voltage[N-1])
00028     {
00029         distance = meters[N-1];
00030     }
00031 
00032     else
00033     {
00034         for (int i = 0; i < N-1; i++)
00035         {
00036             if (volts < voltage[i] && volts >= voltage[i+1])
00037             {
00038                 distance = meters[i] - (((meters[i] - meters[i+1]) * (voltage[i] - volts)) / (voltage[i] - voltage[i+1]));
00039             }
00040         }
00041     }
00042 
00043     return distance;
00044 }