move csource and core into place

1 files changed, 207 insertions, 0 deletions

diff --git a/msp430/libraries/Servo/Servo.cpp b/msp430/libraries/Servo/Servo.cpp
new file mode 100644
index 0000000..8853dd9
--- /dev/null
+++ b/msp430/libraries/Servo/Servo.cpp
@@ -0,0 +1,207 @@
+/*

+ Servo.cpp - Interrupt driven Servo library for MSP430

+ Copyright (c) 2012 Petr Baudis.  All right reserved.

+ Modified by Peter Brier 26-6-2012: Fixed timing/IRQ problem

+

+ Derived from:

+ Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2

+ Copyright (c) 2009 Michael Margolis.  All right reserved.

+

+ This library is free software; you can redistribute it and/or

+ modify it under the terms of the GNU Lesser General Public

+ License as published by the Free Software Foundation; either

+ version 2.1 of the License, or (at your option) any later version.

+

+ This library is distributed in the hope that it will be useful,

+ but WITHOUT ANY WARRANTY; without even the implied warranty of

+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

+ Lesser General Public License for more details.

+

+ You should have received a copy of the GNU Lesser General Public

+ License along with this library; if not, write to the Free Software

+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

+ */

+

+#include "Energia.h"

+#include "Servo.h"

+

+#define F_TIMER (F_CPU/8L)

+#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to timer ticks (assumes prescale of 8)

+#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds

+

+#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009

+

+

+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo

+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo 

+

+

+/************ static functions and data structures common to all instances ***********************/

+

+

+static servo_t servos[MAX_SERVOS]; // static array of servo structures

+static unsigned int ServoCount = 0; // the total number of attached servos

+

+static volatile int counter = 0; // Servo counter; -1 before first servo starts being serviced

+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

+

+#ifndef TIMERA0_VECTOR

+#define TIMERA0_VECTOR TIMER0_A0_VECTOR

+#endif /* TIMER0_A0_VECTOR */

+

+// Timer A0 interrupt service routine

+static void

+Timer_A(void)

+{

+  static unsigned long wait;

+  if (counter >= 0) {

+    /* Turn pulse off. */

+    digitalWrite(servos[counter].Pin.nbr, LOW);

+  }

+

+  /* Service next servo, while skipping any inactive servo records. */

+  do {

+    counter++;

+  } while (!servos[counter].Pin.isActive && counter < ServoCount);

+

+  // Counter range is 0-ServoCount, the last count is used to complete the REFRESH_INTERVAL

+  if (counter < ServoCount) {

+    /* Turn pulse on for the next servo. */

+    digitalWrite(servos[counter].Pin.nbr, HIGH);

+    /* And hold! */

+    totalWait += servos[counter].ticks;

+    CCR0 = servos[counter].ticks;

+  } else {

+    /* Wait for the remaining of REFRESH_INTERVAL. */

+    wait = usToTicks(REFRESH_INTERVAL) - totalWait;

+    totalWait = 0;

+    CCR0 = (wait < 1000 ? 1000 : wait);

+    counter = -1;

+  }

+}

+

+// Timer A0 interrupt service routine

+__attribute__((interrupt(TIMERA0_VECTOR)))

+static void

+Timer_A_int(void)

+{

+  Timer_A();

+}

+

+static boolean isTimerActive(void)

+{

+  // returns true if any servo is active

+  for(int i = 0; i < MAX_SERVOS; i++)

+    if (servos[i].Pin.isActive == true)

+      return true;

+  return false;

+}

+

+static void enableTimer(void)

+{

+  counter = -1;

+  totalWait = 0;

+

+  Timer_A(); // enable first servo

+

+  CCTL0 = CCIE;                             // CCR0 interrupt enabled

+  TACTL = TASSEL_2 + MC_1 + ID_3;           // prescale SMCLK/8, upmode

+}

+

+static void disableTimer(void)

+{

+  // disable interrupt

+  CCTL0 = 0;

+  CCR0 = 0;

+}

+

+

+/****************** end of static functions ******************************/

+

+Servo::Servo()

+{

+  if( ServoCount < MAX_SERVOS) {

+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance

+	servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values

+  }

+  else

+    this->servoIndex = INVALID_SERVO ;  // too many servos 

+}

+

+uint8_t Servo::attach(int pin)

+{

+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);

+}

+

+uint8_t Servo::attach(int pin, int min, int max)

+{

+  if(this->servoIndex < MAX_SERVOS ) {

+    pinMode( pin, OUTPUT) ;                                   // set servo pin to output

+    servos[this->servoIndex].Pin.nbr = pin;  

+

+    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 

+    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS

+    this->max  = (MAX_PULSE_WIDTH - max)/4; 

+

+    boolean timer_active = isTimerActive();

+    servos[this->servoIndex].Pin.isActive = true;

+    if (!timer_active)

+      enableTimer();

+  } 

+  return this->servoIndex ;

+}

+

+void Servo::detach()  

+{

+  servos[this->servoIndex].Pin.isActive = false;

+  if (!isTimerActive())

+    disableTimer();

+}

+

+void Servo::write(int value)

+{  

+  if(value < MIN_PULSE_WIDTH)

+  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)

+    if(value < 0) value = 0;

+    if(value > 180) value = 180;

+    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      

+  }

+  this->writeMicroseconds(value);

+}

+

+void Servo::writeMicroseconds(int value)

+{

+  // calculate and store the values for the given channel

+  byte channel = this->servoIndex;

+  if( (channel < MAX_SERVOS) )   // ensure channel is valid

+  {  

+    if( value < SERVO_MIN() )          // ensure pulse width is valid

+      value = SERVO_MIN();

+    else if( value > SERVO_MAX() )

+      value = SERVO_MAX();       

+    value = value - TRIM_DURATION;

+    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009

+    servos[channel].ticks = value; // this is atomic on a 16bit uC, no need to disable Interrupts

+  } 

+}

+

+int Servo::read() // return the value as degrees

+{

+  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);     

+}

+

+int Servo::readMicroseconds()

+{

+  unsigned int pulsewidth;

+  if( this->servoIndex != INVALID_SERVO )

+    pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;

+  else 

+    pulsewidth = 0;

+

+  return pulsewidth;   

+}

+

+bool Servo::attached()

+{

+  return servos[this->servoIndex].Pin.isActive ;

+}