cob_3d_mapping_demonstrator: servo.c Source File

Go to the documentation of this file.
00001 /***************************************************************************
00002 ***  Fraunhofer IPA 
00003 ***  Robotersysteme 
00004 ***  Projekt: 3D cartography demonstrator
00005 ****************************************************************************
00006 ****************************************************************************
00007 ***  Autor: Julio Sagardoy (goa-js)
00008 ***************************************************************************/
00009 
00017 
00018 #include <avr/io.h>
00019 #include <util/delay.h>
00020 
00022 #include "servo.h"
00023 #include "utils.h"
00024 
00026 static const uint16_t pwmt[] = { 
00027 2205,
00028 2199,
00029 2192,
00030 2186,
00031 2179,
00032 2173,
00033 2166,
00034 2160,
00035 2154,
00036 2147,
00037 2141,
00038 2134,
00039 2128,
00040 2121,
00041 2115,
00042 2108,
00043 2102,
00044 2095,
00045 2089,
00046 2082,
00047 2076,
00048 2069,
00049 2063,
00050 2056,
00051 2050,
00052 2043,
00053 2037,
00054 2030,
00055 2024,
00056 2017,
00057 2011,
00058 2004,
00059 1998,
00060 1992,
00061 1985,
00062 1979,
00063 1972,
00064 1966,
00065 1959,
00066 1953,
00067 1946,
00068 1940,
00069 1933,
00070 1927,
00071 1920,
00072 1914,
00073 1907,
00074 1901,
00075 1894,
00076 1888,
00077 1881,
00078 1875,
00079 1868,
00080 1862,
00081 1855,
00082 1849,
00083 1842,
00084 1836,
00085 1830,
00086 1823,
00087 1817,
00088 1810,
00089 1804,
00090 1797,
00091 1791,
00092 1784,
00093 1778,
00094 1771,
00095 1765,
00096 1758,
00097 1752,
00098 1745,
00099 1739,
00100 1732,
00101 1726,
00102 1719,
00103 1713,
00104 1706,
00105 1700,
00106 1693,
00107 1687,
00108 1680,
00109 1674,
00110 1668,
00111 1661,
00112 1655,
00113 1648,
00114 1642,
00115 1635,
00116 1629,
00117 1622,
00118 1616,
00119 1609,
00120 1603,
00121 1596,
00122 1590,
00123 1583,
00124 1577,
00125 1570,
00126 1564,
00127 1557,
00128 1551,
00129 1544,
00130 1538,
00131 1531,
00132 1525,
00133 1518,
00134 1512,
00135 1506,
00136 1499,
00137 1493,
00138 1486,
00139 1480,
00140 1473,
00141 1467,
00142 1460,
00143 1454,
00144 1447,
00145 1441,
00146 1434,
00147 1428,
00148 1421,
00149 1415,
00150 1408,
00151 1402,
00152 1395,
00153 1389,
00154 1382,
00155 1376,
00156 1369,
00157 1363,
00158 1356,
00159 1350,
00160 1344,
00161 1337,
00162 1331,
00163 1324,
00164 1318,
00165 1311,
00166 1305,
00167 1298,
00168 1292,
00169 1285,
00170 1279,
00171 1272,
00172 1266,
00173 1259,
00174 1253,
00175 1246,
00176 1240,
00177 1233,
00178 1227,
00179 1220,
00180 1214,
00181 1207,
00182 1201,
00183 1194,
00184 1188,
00185 1182,
00186 1175,
00187 1169,
00188 1162,
00189 1156,
00190 1149,
00191 1143,
00192 1136,
00193 1130,
00194 1123,
00195 1117,
00196 1110,
00197 1104,
00198 1097,
00199 1091,
00200 1084,
00201 1078,
00202 1071,
00203 1065,
00204 1058,
00205 1052,
00206 1045,
00207 1039,
00208 1032,
00209 1026,
00210 1020,
00211 1013,
00212 1007,
00213 1000,
00214 994,
00215 987,
00216 981,
00217 974,
00218 968,
00219 961,
00220 955,
00221 948,
00222 942,
00223 935,
00224 929,
00225 922,
00226 916,
00227 909,
00228 903,
00229 896,
00230 890,
00231 883,
00232 877,
00233 870,
00234 864,
00235 858,
00236 851,
00237 845,
00238 838,
00239 832,
00240 825,
00241 819,
00242 812,
00243 806,
00244 799,
00245 793,
00246 786,
00247 780,
00248 773,
00249 767,
00250 760,
00251 754,
00252 747,
00253 741,
00254 734,
00255 728,
00256 721,
00257 715,
00258 708,
00259 702,
00260 696,
00261 689,
00262 683,
00263 676,
00264 670,
00265 663,
00266 657,
00267 650,
00268 644,
00269 637,
00270 631,
00271 624,
00272 618,
00273 611,
00274 605,
00275 598,
00276 592,
00277 585,
00278 579,
00279 572,
00280 566,
00281 559,
00282 553};
00283 
00284 static int8_t old_val;          // static variable last commanded value
00285 static int8_t direction;
00286 static int16_t rem_steps;
00287 static uint8_t latency;
00288 
00291 char servo_read()
00292 {
00293         return old_val;
00294 }
00295 
00296 void servo_set_direction( const uint8_t dir )
00297 {
00298         if(dir>=1)
00299                 direction = 1;
00300         else
00301                 direction = 0;
00302 }
00303 void servo_advance()
00304 {
00305         if( direction == 1)
00306         {
00307                 old_val ++;
00308                 OCR1A = pwmt[old_val+128];
00309         }
00310         else 
00311         {
00312                 old_val --;
00313                 OCR1A = pwmt[old_val+128];
00314         }
00315 }
00316 void servo_set_rem_steps( const uint8_t steps)
00317 {
00318         rem_steps = steps;
00319 }
00320 int16_t servo_get_rem_steps()
00321 {
00322         return rem_steps;
00323 }
00324 void servo_set_latency( const uint8_t lat)
00325 {
00326         latency = lat;
00327 }
00328 uint8_t servo_get_latency()
00329 {
00330         return latency;
00331 }
00332 
00335 void servo_set_val( const int8_t target_val, const int8_t lat )
00336 {
00337         uint8_t val;
00338         
00339         if(target_val > old_val)        // compute direction
00340         {
00341                 for(val=old_val+128; val < target_val+128; val++)       // +127 converts to unsigned char
00342                 {
00343                         OCR1A = pwmt[val];
00344                         _delay_ms(lat);
00345                         //_delay_ms(1);
00346                 }
00347         }
00348         if(target_val < old_val)
00349         {
00350                 for(val=old_val+128; val > target_val+128; val--)       
00351                 {
00352                         OCR1A = pwmt[val];
00353                         _delay_ms(lat);
00354                         //_delay_ms(1);
00355                 }
00356         }
00357         old_val = target_val;   // update final position value
00358 }
00359 
00362 void servo_set_neutral()
00363 {
00364         OCR1A = pwmt[127];
00365         old_val = 0;
00366 }
00367 
00370 void servo_set_max()
00371 {
00372         OCR1A = pwmt[255];
00373         old_val = 127;
00374 }
00375 
00378 void servo_set_min()
00379 {
00380         OCR1A = pwmt[0];
00381         old_val = -128;
00382 }
00383 
00386 void servo_init()
00387 {
00388         BITSET(DDRB,PB5);       // set PORTB pin 5 as output for OC output (mandatory by datasheet)
00389         TCCR1A = 0x82;          // phase correct PWM, 8-bit, TOP=ICR1, set when up-count/clear when down-count
00390         TCCR1B = 0x12;          // prescaler=8
00391 
00392         ICR1 = 18432;           // after 18432 timer counts (which is 10ms), slope goes down. Because of double-slope feature of Phase-Correct PWM, final period will thus be of 20ms, which is the required by the servo
00393 }