DCE-Eurobot: misclibs/sharp.c Source File

00001 #include "sharp.h"
00002 #include <stdint.h>
00003 #include <math.h>
00004 
00010 int s_ir2mmShort(unsigned short int SensorValue){
00011     float a, b, c;
00012     float res;
00013     int8_t max_cm = 30;
00014     
00015     if (SensorValue <= 260) 
00016         res = max_cm;
00017     else {
00018         if (SensorValue > 260 && SensorValue <= 800) { 
00019                 a = 2000.0; 
00020                 b = 260.0; 
00021                 c = 24.0;
00022         }
00023         else { 
00024                 a = 4500.0; 
00025                 b = 600.0; 
00026                 c = 10.0; 
00027         }
00028         res = (int)(-c * log((SensorValue - b) / a));
00029         if (res > max_cm) 
00030                 res = max_cm;
00031     }
00032     return res*10;
00033 }
00034 
00035 int s_ir2mmLong(unsigned short int SensorValue){
00036         float a, b, c;
00037         float res;
00038         int8_t max_cm = 80;
00039     
00040         if (SensorValue <= 260) 
00041                 res = max_cm;
00042         else {
00043                 if (SensorValue > 260 && SensorValue <= 800) { 
00044                         a = 2000.0; 
00045                         b = 260.0; 
00046                         c = 24.0;
00047                 }
00048                 else { 
00049                         a = 4500.0; 
00050                         b = 600.0; 
00051                         c = 10.0; 
00052                 }
00053                 res = (int)(-c * log((SensorValue - b) / a));
00054                 if (res > max_cm) 
00055                         res = max_cm;
00056         }
00057         return res*10;
00058 }