Subversion Repositories HomeAutomation

Rev

Rev 390 | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1. /*
  2.  * PWM output to control a RC servo
  3.  *
  4.  * It is possible to use OC0A, OC0B and OC1A. The others are not aviable because
  5.  * of the pins is used to interface the mcp2515.
  6.  *
  7.  * Author: Erik Larsson
  8.  * Date: 2007-01-04
  9.  *
  10.  * TODO anpassa för fler hastigheter än 8 MHz
  11.  */
  12.  
  13. /*-----------------------------------------------
  14.  * Includes
  15.  * ---------------------------------------------*/
  16. #include <avr/io.h>
  17. #include <stdio.h>
  18. #include <stdlib.h>
  19. #include <rc_servo.h>
  20.  
  21.  
  22. /*----------------------------------------------
  23.  * Functions
  24.  * --------------------------------------------*/
  25.  
  26. /*
  27.  * Initiate PWM output.
  28.  */
  29. void rcServoInit(){
  30.     // set up counter 1 (16 bit) to act as a dual channel PWM generator
  31.     // we want OC1A and B to be set on reaching BOTTOM, clear on reaching
  32.     // compare match, use ICR1 as TOP and have a prescale of 8.
  33.  
  34. #if NUMBER_OF_RCS >= 1
  35.     /* Use OC1A */
  36.     TCCR1A |= (1<<COM1A1) | (1<<WGM11); /* Set OC1A at TOP, mode 14 */
  37.     TCCR1B |= (1<<WGM13) | (1<<WGM12) | (1<<CS11); /* Timer uses main system clock with 1/8 prescale */
  38.     ICR1 = 20000; /* Used for TOP, makes for 50 Hz PWM */
  39.     OCR1A = PWM1_MIN_PULSE; /* Servo at min position */
  40. //    OCR1A = PWM1_CENTER_PULSE; /* Servo at center */
  41.     DDRB |= (1<<DDB1); /* Enable pin as output */
  42. #endif
  43. #if NUMBER_OF_RCS >= 2
  44.     /* Use OC0A */
  45.     TCCR0A |= (1<<COM0A1)|(1<<WGM01)|(1<<WGM00); /* Set OC0A at TOP, Mode 7 */
  46.     TCCR0B |= (1<<CS02); /* Prescaler 1/256 */
  47.     OCR0A = PWM2_CENTER_PULSE;
  48.     DDRD |= (1<<DDD6);
  49. #endif
  50. #if NUMBER_OF_RCS >= 3
  51.     /* Use OC0B */
  52.     TCCR0A |= (1<<COM0B1);
  53.     OCR0B = PWM3_CENTER_PULSE;
  54.     DDRD |= (1<<DDD5);
  55. #endif
  56.  
  57. }
  58.  
  59. /*
  60.  * Set the servo to an absolute position.
  61.  * Interval 0 to 255.
  62.  */
  63. void setPosition(uint8_t abs_pos, uint8_t servo){
  64.     uint16_t temp_pos;
  65.  
  66.     switch( servo ){
  67. #if NUMBER_OF_RCS > 0
  68.         case 1:
  69.             //temp_pos = (uint16_t)PWM1_CENTER_PULSE + (int8_t)abs_pos*STEP1;
  70.             temp_pos = PWM1_MIN_PULSE + abs_pos*STEP1;
  71.             /*if(temp_pos<PWM1_MIN_PULSE){
  72.                 OCR1A = PWM1_MIN_PULSE;
  73.             }else*/ if(temp_pos>PWM1_MAX_PULSE){
  74.                 OCR1A = PWM1_MAX_PULSE;
  75.             }else{
  76.                 OCR1A = temp_pos;
  77.             }
  78.             break;
  79. #endif
  80. #if NUMBER_OF_RCS > 1
  81.         case 2:
  82.             temp_pos = (uint8_t)PWM2_CENTER_PULSE + (int8_t)((abs_pos*STEP2)/8);
  83.             if(temp_pos<PWM2_MIN_PULSE){
  84.                 OCR0A = PWM2_MIN_PULSE;
  85.             }else if(temp_pos>PWM2_MAX_PULSE){
  86.                 OCR0A = PWM2_MAX_PULSE;
  87.             }else{
  88.                 OCR0A = temp_pos;
  89.             }
  90.             break;
  91. #endif
  92. #if NUMBER_OF_RCS > 2
  93.         case 3:
  94.             temp_pos = (uint8_t)PWM3_CENTER_PULSE + (int8_t)((abs_pos*STEP3)/8);
  95.             if(temp_pos<PWM3_MIN_PULSE){
  96.                 OCR0B = PWM3_MIN_PULSE;
  97.             }else if(temp_pos>PWM3_MAX_PULSE){
  98.                 OCR0B = PWM3_MAX_PULSE;
  99.             }else{
  100.                 OCR0B = temp_pos;
  101.             }
  102.             break;
  103. #endif
  104.     }
  105.  
  106.  
  107. }
  108.  
  109. /*
  110.  * Alter the position rel_pos steps.
  111.  * Interval -128 to 127.
  112.  */
  113.  // FIXME inte ändrat här än
  114. void alterPosition(int8_t rel_pos, uint8_t servo){
  115.     int8_t new_pos, current_pos;
  116.  
  117.     current_pos = getPosition( servo );
  118.     new_pos = current_pos + rel_pos;
  119.  
  120.     if(rel_pos<0 && current_pos<new_pos ){
  121.         new_pos = MIN_POSITION;
  122.     }else if(rel_pos>0 && current_pos>new_pos ){
  123.         new_pos = MAX_POSITION;
  124.     }
  125.  
  126.     setPosition( new_pos, servo );
  127. }
  128.  
  129. /*
  130.  * Get current position of the servo.
  131.  */
  132. uint8_t getPosition(uint8_t servo){
  133.     switch( servo ){
  134.         case 1: return ((OCR1A - PWM1_MIN_PULSE)/STEP1); break;
  135.  /*       case 1: return ((int16_t)(OCR1A - PWM1_CENTER_PULSE)/STEP1); break;
  136.         case 2: return (int8_t)((OCR0A*8 - PWM2_CENTER_PULSE*8)/STEP2); break;
  137.         case 3: return (int8_t)((OCR0B*8 - PWM3_CENTER_PULSE*8)/STEP3); break;
  138.    */ }
  139. }
  140.  
  141. /*
  142. http://members.shaw.ca/climber/avrtimers.html
  143.  
  144.  To change the servo position all you have to do is assign a value to OCR1A or B.
  145.  With an 8 MHz clock the normal range you use is 1000 to 2000.
  146.  I especially like it at 8 MHz because the timing for normal servo control
  147.  is pulse widths of (usually) 1.0 to 2.0 milliseconds. Just multiply that
  148.  by 1000 and you get the value to assign to OCR1A or B.
  149.  
  150. If you want to use a different system clock then all you have to do is
  151. adjust the value you assign ICR1 and the useable range to assign OCR1A
  152. and B to control the servos is 5% to 10% of the value you use for ICR1.
  153. For example, for a 1 MHz clock (like the default RC clock from the factory)
  154. use 2500 for ICR1 and 125 - 250 for the range of values to give OCR1A and B.
  155. */
  156.