SHARE
TWEET

servo.cpp

a guest Jan 29th, 2014 117 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.    
  2.    
  3. //this code is modified by MLF based on
  4. //http://forum.43oh.com/topic/4981-conflicting-timers-using-servoh-and-pitchesh/    
  5.    
  6.    
  7.    
  8.    
  9.  /*
  10.  Servo.cpp - Interrupt driven Servo library for MSP430
  11.  Copyright (c) 2012 Petr Baudis.  All right reserved.
  12.  Modified by Peter Brier 26-6-2012: Fixed timing/IRQ problem
  13.  Modified by Roadrunner84@43oh.com 28-1-2014: Allow use of Timer A1
  14.  
  15.  Derived from:
  16.  Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
  17.  Copyright (c) 2009 Michael Margolis.  All right reserved.
  18.  
  19.  This library is free software; you can redistribute it and/or
  20.  modify it under the terms of the GNU Lesser General Public
  21.  License as published by the Free Software Foundation; either
  22.  version 2.1 of the License, or (at your option) any later version.
  23.  
  24.  This library is distributed in the hope that it will be useful,
  25.  but WITHOUT ANY WARRANTY; without even the implied warranty of
  26.  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  27.  Lesser General Public License for more details.
  28.  
  29.  You should have received a copy of the GNU Lesser General Public
  30.  License along with this library; if not, write to the Free Software
  31.  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  32.  */
  33.  
  34. #include "Energia.h"
  35. #include "Servo.h"
  36.  
  37. #define F_TIMER (F_CPU/8L)
  38. #define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to timer ticks (assumes prescale of 8)
  39. #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
  40.  
  41. #define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
  42.  
  43.  
  44. #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
  45. #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
  46.  
  47.  
  48. /************ static functions and data structures common to all instances ***********************/
  49.  
  50.  
  51. static servo_t servos[MAX_SERVOS]; // static array of servo structures
  52. static unsigned int ServoCount = 0; // the total number of attached servos
  53.  
  54. static volatile int counter = 0; // Servo counter; -1 before first servo starts being serviced
  55. static volatile unsigned int totalWait = 0; // Total amount waited so far in the current period; after all servos, wait for the rest of REFRESH_INTERVAL
  56.  
  57. #ifndef SERVO_TIMER
  58.   #define SERVO_TIMER 0
  59. #endif
  60.  
  61. #if SERVO_TIMER == 0
  62.   #ifdef TIMERA0_VECTOR
  63.     #define SERVO_VECTOR TIMERA0_VECTOR
  64.   #else
  65.     #define SERVO_VECTOR TIMER0_A0_VECTOR
  66.   #endif
  67.   #define TAservoCCTL0 TA0CCTL0
  68.   #define TAservoCCR0  TA0CCR0
  69.   #define TAservoCTL   TA0CTL
  70. #elif SERVO_TIMER == 1
  71.   #define SERVO_VECTOR TIMER1_A0_VECTOR
  72.   #define TAservoCCTL0 TA1CCTL0
  73.   #define TAservoCCR0  TA1CCR0
  74.   #define TAservoCTL   TA1CTL
  75. #else
  76.   #error Servo: Do not know how to use a timer other than Timer A0 or Timer A1.
  77. #endif
  78.  
  79. // Timer A interrupt service routine
  80. static void
  81. Timer_A(void)
  82. {
  83.   static unsigned long wait;
  84.   if (counter >= 0) {
  85.     /* Turn pulse off. */
  86.     digitalWrite(servos[counter].Pin.nbr, LOW);
  87.   }
  88.  
  89.   /* Service next servo, while skipping any inactive servo records. */
  90.   do {
  91.     counter++;
  92.   /* counter is nonnegative, so it is save to type cast to unsigned */
  93.   } while (!servos[counter].Pin.isActive && (unsigned)counter < ServoCount);
  94.  
  95.   /* Counter range is 0-ServoCount, the last count is used to complete the REFRESH_INTERVAL
  96.    * counter is nonnegative, so it is save to type cast to unsigned */
  97.   if ((unsigned)counter < ServoCount) {
  98.     /* Turn pulse on for the next servo. */
  99.     digitalWrite(servos[counter].Pin.nbr, HIGH);
  100.     /* And hold! */
  101.     totalWait += servos[counter].ticks;
  102.     TAservoCCR0 = servos[counter].ticks;
  103.   } else {
  104.     /* Wait for the remaining of REFRESH_INTERVAL. */
  105.     wait = usToTicks(REFRESH_INTERVAL) - totalWait;
  106.     totalWait = 0;
  107.     TAservoCCR0 = (wait < 1000 ? 1000 : wait);
  108.     counter = -1;
  109.   }
  110. }
  111.  
  112. // Timer A interrupt service routine
  113. __attribute__((interrupt(SERVO_VECTOR)))
  114. static void
  115. Timer_A_int(void)
  116. {
  117.   Timer_A();
  118. }
  119.  
  120. static boolean isTimerActive(void)
  121. {
  122.   // returns true if any servo is active
  123.   for(int i = 0; i < MAX_SERVOS; i++)
  124.     if (servos[i].Pin.isActive == true)
  125.       return true;
  126.   return false;
  127. }
  128.  
  129. static void enableTimer(void)
  130. {
  131.   counter = -1;
  132.   totalWait = 0;
  133.  
  134.   Timer_A(); // enable first servo
  135.  
  136.   TAservoCCTL0 = CCIE;                       // CCR0 interrupt enabled
  137.   TAservoCTL = TASSEL_2 + MC_1 + ID_3;       // prescale SMCLK/8, upmode
  138. }
  139.  
  140. static void disableTimer(void)
  141. {
  142.   // disable interrupt
  143.   TAservoCCTL0 = 0;
  144.   TAservoCCR0 = 0;
  145. }
  146.  
  147.  
  148. /****************** end of static functions ******************************/
  149.  
  150. Servo::Servo()
  151. {
  152.   if( ServoCount < MAX_SERVOS) {
  153.     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
  154.         servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values
  155.   }
  156.   else
  157.     this->servoIndex = INVALID_SERVO ;  // too many servos
  158. }
  159.  
  160. uint8_t Servo::attach(int pin)
  161. {
  162.   return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  163. }
  164.  
  165. uint8_t Servo::attach(int pin, int min, int max)
  166. {
  167.   if(this->servoIndex < MAX_SERVOS ) {
  168.     pinMode( pin, OUTPUT) ;                                   // set servo pin to output
  169.     servos[this->servoIndex].Pin.nbr = pin;  
  170.  
  171.     // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
  172.     this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
  173.     this->max  = (MAX_PULSE_WIDTH - max)/4;
  174.  
  175.     boolean timer_active = isTimerActive();
  176.     servos[this->servoIndex].Pin.isActive = true;
  177.     if (!timer_active)
  178.       enableTimer();
  179.   }
  180.   return this->servoIndex ;
  181. }
  182.  
  183. void Servo::detach()  
  184. {
  185.   servos[this->servoIndex].Pin.isActive = false;
  186.   if (!isTimerActive())
  187.     disableTimer();
  188. }
  189.  
  190. void Servo::write(int value)
  191. {  
  192.   if(value < MIN_PULSE_WIDTH)
  193.   {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
  194.     if(value < 0) value = 0;
  195.     if(value > 180) value = 180;
  196.     value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
  197.   }
  198.   this->writeMicroseconds(value);
  199. }
  200.  
  201. void Servo::writeMicroseconds(int value)
  202. {
  203.   // calculate and store the values for the given channel
  204.   byte channel = this->servoIndex;
  205.   if( (channel < MAX_SERVOS) )   // ensure channel is valid
  206.   {  
  207.     if( value < SERVO_MIN() )          // ensure pulse width is valid
  208.       value = SERVO_MIN();
  209.     else if( value > SERVO_MAX() )
  210.       value = SERVO_MAX();      
  211.     value = value - TRIM_DURATION;
  212.     volatile int v = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
  213.     servos[channel].ticks = v; // this is atomic on a 16bit uC, no need to disable Interrupts
  214.   }
  215. }
  216.  
  217. int Servo::read() // return the value as degrees
  218. {
  219.   return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);    
  220. }
  221.  
  222. int Servo::readMicroseconds()
  223. {
  224.   unsigned int pulsewidth;
  225.   if( this->servoIndex != INVALID_SERVO )
  226.     pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
  227.   else
  228.     pulsewidth = 0;
  229.  
  230.   return pulsewidth;  
  231. }
  232.  
  233. bool Servo::attached()
  234. {
  235.   return servos[this->servoIndex].Pin.isActive ;
  236. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top