00001 #include "Servo.h" 00002 00004 void Servo::setTimer(char _timer) { 00005 this->timer = _timer; 00006 this->initTimer(); 00007 } 00008 00009 00017 void Servo::initTimer(void) 00018 { 00019 00020 unsigned int intValue; 00021 00022 // ******************************************************************* 00023 // Timer init: 8-bit, normal pwm, prescale 1024 00024 // ******************************************************************* 00025 if (this->timer == TIMER0) { 00026 00027 TCCR0 = (1<<WGM00)|(1<<COM01) |(1<<CS02) | (1<<CS00); 00028 00029 maxUp = T0_MAX_UP; 00030 maxDown = T0_MAX_DOWN; 00031 servoMiddle = T0_SERVO_MIDDLE; 00032 00033 // ******************************************************************* 00034 // Timer1 init: 16-bit, normal pwm, prescale 8 00035 // ******************************************************************* 00036 } else if (this->timer == TIMER1A || this->timer == TIMER1B) { 00037 00038 TCCR1A = (1<<WGM11) | (1<<COM1A1) |(1<<COM1B1); 00039 TCCR1B = (1<<CS11) | (1<<WGM12) |(1<<WGM13); 00040 00041 maxUp = T1_MAX_UP; 00042 maxDown = T1_MAX_DOWN; 00043 servoMiddle = T1_SERVO_MIDDLE; 00044 00045 // at period end start from 0 again 00046 ICR1 = T1_T_PERIODE; 00047 } 00048 00049 00050 // ******************** 00051 // configure PWM Pins * 00052 // ******************** 00053 if (this->timer == TIMER0) { 00054 00055 // PWM Port 00056 DDRB |= (1<<PB3); 00057 OCR0 = (unsigned char)(maxDown); 00058 00059 } else if (this->timer == TIMER1A) { 00060 00061 DDRD |= (1<<PD5); 00062 00063 // initial position 00064 OCR1A = maxUp; 00065 00066 } else if (this->timer == TIMER1B) { 00067 00068 DDRD |= (1<<PD4); 00069 00070 // initial position 00071 OCR1B = maxDown; 00072 } 00073 } 00074 00076 void Servo::setPosition(char _pos, char _scal) { 00077 00078 unsigned int intValue; 00079 00080 // invert position 00081 _pos = (_scal-1)-_pos; 00082 00083 // check for valid action 00084 if (_pos <0 || _pos >= _scal) { 00085 return; 00086 00087 // position servo 00088 } else { 00089 00090 // PWM: compute new timer value 00091 intValue = (unsigned int)(maxDown + (_pos * (float)((float)(maxUp-maxDown)/(_scal-1)))); 00092 00093 // PWM: set new value to timer register 00094 if (this->timer == TIMER0) { 00095 OCR0 = (unsigned char)(intValue%256); 00096 } else if (this->timer == TIMER1A) { 00097 OCR1A = intValue; 00098 } else if (this->timer == TIMER1B) { 00099 OCR1B = intValue; 00100 } 00101 } 00102 00103 } 00104