src/eb_ebb/servo.c

   1
   2
   3 #include <lpc21xx.h>                          // LPC21XX Peripheral Registers
   4 #include <deb_led.h>
   5 #include <system_def.h>
   6 #include "servo.h"
   7
   8
   9 #define SERVO2 (1<<10)
  10 #define SERVO0 (1<<12)
  11 #define SERVO1 (1<<13)
  12
  13
  14
  15 #define TIM_EMR_NOTHING 0
  16 #define TIM_EMR_CLEAR   1
  17 #define TIM_EMR_SET     2
  18 #define TIM_EMR_TOGLE   3
  19
  20 #define TIM_EMR_PIN_ON 1
  21 #define TIM_EMR_PIN_OFF 0
  22
  23 #define TIME20MS        ((CPU_APB_HZ) / 50)
  24 #define SERVOTICK       (((CPU_APB_HZ / 50) / 20) / 256)
  25
  26
  27 unsigned char servo[3];
  28
  29
  30
  31 // ---------------- SERVO PART -------------------------------
  32
  33
  34 void tc1 (void) __attribute__ ((interrupt));
  35
  36 void tc1 (void)   {
  37
  38         time_ms +=20;
  39
  40         T1EMR |= (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3);
  41
  42         T1MR0 = (servo[0] + 256) * SERVOTICK;
  43         T1MR1 = (servo[1] + 256) * SERVOTICK;
  44         T1MR3 = (servo[2] + 256) * SERVOTICK;
  45
  46         if (T1IR != 4)
  47         {
  48             __deb_led_on(LEDR);
  49         }
  50
  51         T1IR        = 4;                            // Vynulovani priznaku preruseni
  52         VICVectAddr = 0;                            // Potvrzeni o obsluze preruseni
  53 }
  54
  55 void set_servo(char serv, char position)
  56 {
  57         if (serv == 0) servo[0] = position;
  58         if (serv == 1) servo[1] = position;
  59         if (serv == 2) servo[2] = position;
  60 }
  61
  62 /* Setup the Timer Counter 1 Interrupt */
  63 void init_servo (unsigned rx_isr_vect)
  64 {
  65
  66         IO0DIR |= (SERVO0 | SERVO1 | SERVO2);                   // enables servo output
  67         IO0SET |= (SERVO0 | SERVO1 | SERVO2);                   // sets to High level
  68
  69         PINSEL0 &= ~((PINSEL_3 << 24)   | (PINSEL_3 << 26));
  70         PINSEL0 |= (PINSEL_2 << 24) | (PINSEL_2 << 26);
  71         PINSEL1 &= ~(PINSEL_3 << 8);
  72         PINSEL1 |= (PINSEL_1 << 8);
  73
  74
  75         servo[0] = 0;
  76         servo[1] = 127;
  77         servo[2] = 0xFF;
  78
  79         T1PR = 0;
  80         T1MR2 = TIME20MS;
  81         T1MR0 = (servo[0] + 256) * SERVOTICK;
  82         T1MR1 = (servo[1] + 256) * SERVOTICK;
  83         T1MR3 = (servo[2] + 256)* SERVOTICK;
  84         T1MCR = (3<<6);                 // interrupt on MR1
  85
  86         T1EMR = (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3) \
  87                                 | (TIM_EMR_CLEAR << 4) | (TIM_EMR_CLEAR << 6) | (TIM_EMR_CLEAR << 10);
  88
  89
  90         T1TCR = 1;                                  // Starts Timer 1
  91
  92
  93         ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned long)tc1;          // Nastaveni adresy vektotu preruseni
  94         ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x20 | 0x5;                    // vyber casovece pro preruseni
  95         VICIntEnable = (1<<5);                  // Povoli obsluhu preruseni
  96 }
  97
  98
  99 // ---------------- powSitch PART -------------------------------
 100