Advertisement
Guest User

Untitled

a guest
Aug 27th, 2010
6,011
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 2.11 KB | None | 0 0
  1. #include <stdint.h>
  2. #include <stdio.h>
  3.  
  4. #include "msp430G2231.h"
  5.  
  6. #define TEST2 1
  7.  
  8. #define SERVO_0 650
  9. #define SERVO_180 2350
  10.  
  11. #define SERVO_COUNT 4
  12.  
  13. #define SETSERVO(servo, pos) periods[servo].cycle = servopos(pos);
  14.  
  15. typedef struct period_t_ {
  16.     uint16_t cycle;
  17.     uint8_t pins;
  18. } period_t;
  19.  
  20. period_t periods[1 + SERVO_COUNT];
  21. uint8_t period = 0;
  22.  
  23. void mysleep(uint16_t i);
  24. uint16_t servopos(uint8_t pos);
  25. void initperiod(uint8_t period, uint16_t cycle, uint8_t pins);
  26.  
  27. int main(void)
  28. {
  29.     WDTCTL = WDTPW + WDTHOLD; /* Stop watchdog timer */
  30.    
  31.     initperiod(0, 20000, 0x00);
  32.     initperiod(1, servopos(90), BIT5);
  33.     initperiod(2, servopos(90), BIT4);
  34.     initperiod(3, servopos(90), BIT3);
  35.     initperiod(4, servopos(90), BIT2);
  36.  
  37.     P1OUT = 0x00;
  38.     P1DIR |= BIT5;
  39.     P1DIR |= BIT4;
  40.     P1DIR |= BIT3;
  41.     P1DIR |= BIT2;
  42.  
  43.     CCTL0 = CCIE;
  44.  
  45.     CCR0 = 65535;
  46.  
  47.     TACTL = MC_1 + TASSEL_2 + TACLR;
  48.  
  49.     //_BIS_SR(LPM0_bits + GIE);
  50.     _BIS_SR(GIE);
  51.  
  52.     while (1) {
  53.         uint16_t i;
  54.        
  55.         SETSERVO(1, 90);
  56.         SETSERVO(2, 90);
  57.         SETSERVO(3, 0);
  58.         mysleep(100);
  59.         for (i = 90; i != 25; i -= 5) {
  60.             SETSERVO(2, i);
  61.             mysleep(3);
  62.         }
  63.         SETSERVO(2, 25);
  64.         mysleep(100);
  65.         SETSERVO(3, 130);
  66.         for (i = 0; i < 15; i ++) {
  67.             SETSERVO(2, 15);
  68.             SETSERVO(3, 120);
  69.             mysleep(5);
  70.             SETSERVO(2, 25);
  71.             SETSERVO(3, 130);
  72.             mysleep(5);
  73.         }
  74.         mysleep(100);
  75.         SETSERVO(2, 10);
  76.         for (i = 0; i < 60; i += 5) {
  77.             SETSERVO(1, (90 - 30) + i);
  78.             mysleep(5);
  79.         }
  80.         for (i = 0; i < 60; i += 5) {
  81.             SETSERVO(1, (90 + 30) - i);
  82.             mysleep(5);
  83.         }
  84.        
  85.     }
  86. }
  87.  
  88. #pragma vector = TIMERA0_VECTOR
  89. __interrupt void Timer_A_0(void)
  90. {
  91.     P1OUT = periods[period].pins;
  92.     CCR0 = periods[period].cycle;
  93.     if (period ++ >= SERVO_COUNT) period = 0;
  94. }
  95.  
  96. void mysleep(uint16_t i)
  97. {
  98.     uint16_t y, x;
  99.     y = i;
  100.     while (--y != 0) {
  101.         for (x = 0; x < 2400; x ++);
  102.     }
  103. }
  104.  
  105. uint16_t servopos(uint8_t pos)
  106. {
  107.         return (uint16_t) ((SERVO_180 - SERVO_0) * (float) pos / 180) + SERVO_0;
  108. }
  109.  
  110.  
  111. void initperiod(uint8_t period, uint16_t cycle, uint8_t pins)
  112. {
  113.     periods[period].cycle = cycle;
  114.     periods[period].pins = pins;
  115.    
  116. }
  117.  
  118. /* vim: set sw=8 noet: */
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement