adding timer3 functions as servo.* because we need to control bots

src/Shell.cpp

@@ -325,6 +325,16 @@ void PinoccioShell::setup() {
     addBitlashFunction("wifi.stats", (bitlash_function) wifiStats);
   }
 
+  addBitlashFunction("servo.initialize", (bitlash_function)timer3Initialize);
+  addBitlashFunction("servo.setPeriod", (bitlash_function)timer3SetPeriod);
+  addBitlashFunction("servo.start", (bitlash_function)timer3Start);
+  addBitlashFunction("servo.stop", (bitlash_function)timer3Stop);
+  addBitlashFunction("servo.restart", (bitlash_function)timer3Restart);
+  addBitlashFunction("servo.resume", (bitlash_function)timer3Resume);
+  addBitlashFunction("servo.pwm", (bitlash_function)timer3Pwm);
+  addBitlashFunction("servo.setPwmDuty", (bitlash_function)timer3SetPwmDuty);
+  addBitlashFunction("servo.disablePwm", (bitlash_function)timer3DisablePwm); 
+
   // set up event handlers
   Scout.digitalPinEventHandler = digitalPinEventHandler;
   Scout.analogPinEventHandler = analogPinEventHandler;
@@ -2131,6 +2141,48 @@ static numvar wifiStats(void) {
 }
 
 
+/****************************\
+ *          SERVO           *
+\****************************/
+
+
+void timer3Initialize (){
+  Timer3.initialize(getPinFromArg(1));
+}
+
+void timer3SetPeriod (){
+  Timer3.setPeriod(getPinFromArg(1));
+}
+
+void timer3Start (){
+  Timer3.start();
+}
+
+void timer3Stop (){
+  Timer3.stop();
+}
+
+void timer3Restart (){
+  Timer3.restart();
+}
+
+void timer3Resume (){
+  Timer3.resume();
+}
+
+void timer3Pwm (){
+  Timer3.pwm(getPinFromArg(1), getarg(2));
+}
+
+void timer3SetPwmDuty (){
+  Timer3.setPwmDuty(getPinFromArg(1), getarg(2));
+}
+
+void timer3DisablePwm (){
+  Timer3.disablePwm(getPinFromArg(1));
+}
+
+
 /****************************\
  *     HELPER FUNCTIONS     *
 \****************************/

src/Shell.h

@@ -19,6 +19,7 @@
 #include "lwm/nwk/nwk.h"
 #include "lwm/sys/sysTimer.h"
 #include "peripherals/halTemperature.h"
+#include "peripherals/TimerThree.h"
 #include "avr/sleep.h"
 
 class PinoccioShell {

src/config/known_16bit_timers.h

@@ -0,0 +1,85 @@
+#ifndef known_16bit_timers_header_
+#define known_16bit_timers_header_
+
+// Wiring-S
+//
+#if defined(__AVR_ATmega644P__) && defined(WIRING)
+  #define TIMER1_A_PIN   5
+  #define TIMER1_B_PIN   4
+  #define TIMER1_ICP_PIN 6
+
+
+// Teensy 2.0
+//
+#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)
+  #define TIMER1_A_PIN   14
+  #define TIMER1_B_PIN   15
+  #define TIMER1_C_PIN   4
+  #define TIMER1_ICP_PIN 22
+  #define TIMER1_CLK_PIN 11
+  #define TIMER3_A_PIN   9
+  #define TIMER3_ICP_PIN 10
+
+
+// Teensy++ 2.0
+#elif defined(__AVR_AT90USB1286__) && defined(CORE_TEENSY)
+  #define TIMER1_A_PIN   25
+  #define TIMER1_B_PIN   26
+  #define TIMER1_C_PIN   27
+  #define TIMER1_ICP_PIN 4
+  #define TIMER1_CLK_PIN 6
+  #define TIMER3_A_PIN   16
+  #define TIMER3_B_PIN   15
+  #define TIMER3_C_PIN   14
+  #define TIMER3_ICP_PIN 17
+  #define TIMER3_CLK_PIN 13
+
+
+// Arduino Mega
+//
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define TIMER1_A_PIN   11
+  #define TIMER1_B_PIN   12
+  #define TIMER1_C_PIN   13
+  #define TIMER3_A_PIN   5
+  #define TIMER3_B_PIN   2
+  #define TIMER3_C_PIN   3
+  #define TIMER4_A_PIN   6
+  #define TIMER4_B_PIN   7
+  #define TIMER4_C_PIN   8
+  #define TIMER4_ICP_PIN 49
+  #define TIMER5_A_PIN   46
+  #define TIMER5_B_PIN   45
+  #define TIMER5_C_PIN   44
+  #define TIMER3_ICP_PIN 48
+  #define TIMER3_CLK_PIN 47
+
+
+// Arduino Uno, Duemilanove, LilyPad, etc
+//
+#elif defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__)
+  #define TIMER1_A_PIN   9
+  #define TIMER1_B_PIN   10
+  #define TIMER1_ICP_PIN 8
+  #define TIMER1_CLK_PIN 5
+
+
+// Sanguino
+//
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
+  #define TIMER1_A_PIN   13
+  #define TIMER1_B_PIN   12
+  #define TIMER1_ICP_PIN 14
+  #define TIMER1_CLK_PIN 1
+
+
+// Arduino 256RFR2, Just the Pinoccio rihgt now?
+//
+#elif defined(__AVR_ATmega256RFR2__)
+  #define TIMER3_A_PIN   3
+  #define TIMER3_B_PIN   4
+  #define TIMER3_C_PIN   5
+
+#endif
+
+#endif

src/peripherals/TimerThree.cpp

@@ -0,0 +1,28 @@
+/*
+ *  Interrupt and PWM utilities for 16 bit Timer3 on ATmega168/328
+ *  Original code by Jesse Tane for http://labs.ideo.com August 2008
+ *  Modified March 2009 by Jérôme Despatis and Jesse Tane for ATmega328 support
+ *  Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop
+ *  Modified Oct 2009 by Dan Clemens to work with timer3 of the ATMega1280 or Arduino Mega
+ *  Modified April 2012 by Paul Stoffregen
+ *
+ *  This is free software. You can redistribute it and/or modify it under
+ *  the terms of Creative Commons Attribution 3.0 United States License. 
+ *  To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ 
+ *  or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA.
+ *
+ */
+
+#include "TimerThree.h"
+
+TimerThree Timer3;              // preinstatiate
+
+unsigned int TimerThree::pwmPeriod = 0;
+unsigned char TimerThree::clockSelectBits = 0;
+
+// interrupt service routine that wraps a user defined function supplied by attachInterrupt
+ISR(TIMER3_OVF_vect)
+{
+  Timer3.isrCallback();
+}
+

src/peripherals/TimerThree.h

@@ -0,0 +1,160 @@
+/*
+ *  Interrupt and PWM utilities for 16 bit Timer3 on ATmega168/328
+ *  Original code by Jesse Tane for http://labs.ideo.com August 2008
+ *  Modified March 2009 by Jérôme Despatis and Jesse Tane for ATmega328 support
+ *  Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop
+ *  Modified April 2012 by Paul Stoffregen - portable to other AVR chips, use inline functions
+ *
+ *  This is free software. You can redistribute it and/or modify it under
+ *  the terms of Creative Commons Attribution 3.0 United States License. 
+ *  To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ 
+ *  or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA.
+ *
+ */
+
+#ifndef TimerThree_h_
+#define TimerThree_h_
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include "config/known_16bit_timers.h"
+
+#define TIMER3_RESOLUTION 65536UL  // Timer3 is 16 bit
+
+
+// Placing nearly all the code in this .h file allows the functions to be
+// inlined by the compiler.  In the very common case with constant values
+// the compiler will perform all calculations and simply write constants
+// to the hardware registers (for example, setPeriod).
+
+
+class TimerThree
+{
+  public:
+    //****************************
+    //  Configuration
+    //****************************
+    void initialize(unsigned long microseconds=1000000) __attribute__((always_inline)) {
+	TCCR3B = _BV(WGM33);        // set mode as phase and frequency correct pwm, stop the timer
+	TCCR3A = 0;                 // clear control register A 
+	setPeriod(microseconds);
+    }
+    void setPeriod(unsigned long microseconds) __attribute__((always_inline)) {
+	const unsigned long cycles = (F_CPU / 2000000) * microseconds;
+	if (cycles < TIMER3_RESOLUTION) {
+		clockSelectBits = _BV(CS30);
+		pwmPeriod = cycles;
+	} else
+	if (cycles < TIMER3_RESOLUTION * 8) {
+		clockSelectBits = _BV(CS31);
+		pwmPeriod = cycles / 8;
+	} else
+	if (cycles < TIMER3_RESOLUTION * 64) {
+		clockSelectBits = _BV(CS31) | _BV(CS30);
+		pwmPeriod = cycles / 64;
+	} else
+	if (cycles < TIMER3_RESOLUTION * 256) {
+		clockSelectBits = _BV(CS32);
+		pwmPeriod = cycles / 256;
+	} else
+	if (cycles < TIMER3_RESOLUTION * 1024) {
+		clockSelectBits = _BV(CS32) | _BV(CS30);
+		pwmPeriod = cycles / 1024;
+	} else {
+		clockSelectBits = _BV(CS32) | _BV(CS30);
+		pwmPeriod = TIMER3_RESOLUTION - 1;
+	}
+	ICR3 = pwmPeriod;
+	TCCR3B = _BV(WGM33) | clockSelectBits;
+    }
+
+    //****************************
+    //  Run Control
+    //****************************
+    void start() __attribute__((always_inline)) {
+	TCCR3B = 0;
+	TCNT3 = 0;		// TODO: does this cause an undesired interrupt?
+	TCCR3B = _BV(WGM33) | clockSelectBits;
+    }
+    void stop() __attribute__((always_inline)) {
+	TCCR3B = _BV(WGM33);
+    }
+    void restart() __attribute__((always_inline)) {
+	TCCR3B = 0;
+	TCNT3 = 0;
+	TCCR3B = _BV(WGM33) | clockSelectBits;
+    }
+    void resume() __attribute__((always_inline)) {
+	TCCR3B = _BV(WGM33) | clockSelectBits;
+    }
+
+    //****************************
+    //  PWM outputs
+    //****************************
+    void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) {
+	unsigned long dutyCycle = pwmPeriod;
+	dutyCycle *= duty;
+	dutyCycle >>= 10;
+	if (pin == TIMER3_A_PIN) OCR3A = dutyCycle;
+	#ifdef TIMER3_B_PIN
+	else if (pin == TIMER3_B_PIN) OCR3B = dutyCycle;
+	#endif
+	#ifdef TIMER3_C_PIN
+	else if (pin == TIMER3_C_PIN) OCR3C = dutyCycle;
+	#endif
+    }
+    void pwm(char pin, unsigned int duty) __attribute__((always_inline)) {
+	if (pin == TIMER3_A_PIN) { pinMode(TIMER3_A_PIN, OUTPUT); TCCR3A |= _BV(COM3A1); }
+	#ifdef TIMER3_B_PIN
+	else if (pin == TIMER3_B_PIN) { pinMode(TIMER3_B_PIN, OUTPUT); TCCR3A |= _BV(COM3B1); }
+	#endif
+	#ifdef TIMER3_C_PIN
+	else if (pin == TIMER3_C_PIN) { pinMode(TIMER3_C_PIN, OUTPUT); TCCR3A |= _BV(COM3C1); }
+	#endif
+	setPwmDuty(pin, duty);
+	TCCR3B = _BV(WGM33) | clockSelectBits;
+    }
+    void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) {
+	if (microseconds > 0) setPeriod(microseconds);
+	pwm(pin, duty);
+    }
+    void disablePwm(char pin) __attribute__((always_inline)) {
+	if (pin == TIMER3_A_PIN) TCCR3A &= ~_BV(COM3A1);
+	#ifdef TIMER3_B_PIN
+	else if (pin == TIMER3_B_PIN) TCCR3A &= ~_BV(COM3B1);
+	#endif
+	#ifdef TIMER3_C_PIN
+	else if (pin == TIMER3_C_PIN) TCCR3A &= ~_BV(COM3C1);
+	#endif
+    }
+
+    //****************************
+    //  Interrupt Function
+    //****************************
+    void attachInterrupt(void (*isr)()) __attribute__((always_inline)) {
+	isrCallback = isr;
+	TIMSK3 = _BV(TOIE3);
+    }
+    void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) {
+	if(microseconds > 0) setPeriod(microseconds);
+	attachInterrupt(isr);
+    }
+    void detachInterrupt() __attribute__((always_inline)) {
+	TIMSK3 = 0;
+    }
+    void (*isrCallback)();
+
+  protected:
+    // properties
+    static unsigned int pwmPeriod;
+    static unsigned char clockSelectBits;
+};
+
+extern TimerThree Timer3;
+
+#endif
+