daily pastebin goal
3%
SHARE
TWEET

Untitled

a guest Aug 27th, 2010 5,487 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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: */
RAW Paste Data
Top