1 files changed, 234 insertions, 0 deletions
diff --git a/OpenModelicaArduino/Firmware/Tiva C/StandardFirmata/Servo.cpp b/OpenModelicaArduino/Firmware/Tiva C/StandardFirmata/Servo.cpp
new file mode 100644
index 0000000..2216219
--- /dev/null
+++ b/OpenModelicaArduino/Firmware/Tiva C/StandardFirmata/Servo.cpp
@@ -0,0 +1,234 @@
+//#include "Energia.h"
+#include "Servo.h"
+
+#include "inc/hw_ints.h"
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "driverlib/debug.h"
+#include "driverlib/gpio.h"
+#include "driverlib/pin_map.h"
+#include "driverlib/rom.h"
+#include "driverlib/sysctl.h"
+#include "driverlib/timer.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+
+
+/** variables and functions common to all Servo instances **/
+
+volatile unsigned long ticksPerMicrosecond;  // Holds the calculated value
+unsigned int servoAssignedMask;
+static servo_t servos[SERVOS_PER_TIMER];
+unsigned int remainderPulseWidth;
+volatile int currentServo;
+bool servoInitialized = false;
+
+// Calculate the new period remainder
+static void calculatePeriodRemainder(void)
+{
+	unsigned long servoPeriodSum = 0;
+	for (int i = 0; i < SERVOS_PER_TIMER; i++){
+		servoPeriodSum += servos[i].pulse_width;
+	}
+	remainderPulseWidth = REFRESH_INTERVAL - servoPeriodSum;
+}
+
+static void initServo(void) {
+
+	/* Work around for clock not yet up in the constructor */
+#ifdef TARGET_IS_BLIZZARD_RB1
+	ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);
+#endif
+
+	// Initialize global variables
+	ticksPerMicrosecond = 0;
+	servoAssignedMask = 0;
+	remainderPulseWidth = 0;
+	currentServo = 0;
+
+	for(int i = 0; i < SERVOS_PER_TIMER; i++)
+	{
+		servos[i].pin_number = 0;
+		servos[i].pulse_width = DEFAULT_SERVO_PULSE_WIDTH;
+		servos[i].enabled = false;
+	}
+
+	calculatePeriodRemainder();
+
+	// Enable TIMER
+	ROM_SysCtlPeripheralEnable(SERVO_TIMER_PERIPH);
+
+	// Enable processor interrupts.
+	ROM_IntMasterEnable();
+
+	TimerIntRegister(SERVO_TIMER, SERVO_TIMER_A, ServoIntHandler);
+	// Configure the TIMER
+	ROM_TimerConfigure(SERVO_TIMER, SERVO_TIME_CFG);
+
+	// Calculate the number of timer counts/microsecond
+	ticksPerMicrosecond = F_CPU / 1000000;
+
+	// Initially load the timer with 20ms interval time
+	ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * REFRESH_INTERVAL);
+
+	// Setup the interrupt for the TIMER1A timeout.
+	ROM_IntEnable(SERVO_TIMER_INTERRUPT);
+	ROM_TimerIntEnable(SERVO_TIMER, SERVO_TIMER_TRIGGER);
+
+	// Enable the timer.
+	ROM_TimerEnable(SERVO_TIMER, SERVO_TIMER_A);
+
+}
+
+/** end of static functions **/
+
+/*
+ * When a new servo is created:
+ * Initialize the servo module if it has not been initialized already.
+ * Add the servo to the assigned servos mask with a new index.
+ */
+Servo::Servo()
+{
+	// If the module has not been initialized
+	if(!servoInitialized)
+	{
+		// Initialize it.
+		initServo();
+		// It has been initialized, prevent further calls to initServo().
+		servoInitialized = true;
+	}
+
+	this->index = INVALID_SERVO;
+
+	// Look for a free servo index.
+	for (int i = 0; i < SERVOS_PER_TIMER; i++)
+	{
+		if (((servoAssignedMask >> i) & 1) == 0)
+		{
+			// Save the index for this instance of Servo.
+			this->index = i;
+
+			// Mark the spot in the mask.
+			servoAssignedMask |= (1 << i);
+
+			// Stop searching for free slots.
+			break;
+		}
+	}
+}
+
+//! Write a pulse width of the given number of microseconds to the Servo's pin
+void Servo::writeMicroseconds(int value)
+{
+	if(value < this->min) value = this->min;
+	if(value > this->max) value = this->max;
+
+	servos[this->index].pulse_width = value;
+
+	calculatePeriodRemainder();
+}
+
+//! Write a pulse width of the given degrees (if in the appropriate range to be degrees)
+//! or of the specified number of microseconds (if in the appropriate range to be microseconds)
+void Servo::write(int value)
+{
+	// treat values less than the min pulse width as angles in degrees (valid values in microseconds are handled as microseconds)
+	if(value < MIN_SERVO_PULSE_WIDTH)
+	{
+		if(value < 0) value = 0;
+		if(value > 180) value = 180;
+
+		value = map(value, 0, 180, this->min,  this->max);
+	}
+	this->writeMicroseconds(value);
+}
+
+//! Returns the current pulse width of the Servo's signal, in microseconds
+int Servo::readMicroseconds()
+{
+	return servos[this->index].pulse_width;
+}
+
+//! Returns the current position of the Servo, in degrees
+int Servo::read() // return the value as degrees
+{
+  return  map( this->readMicroseconds()+1, this->min, this->max, 0, 180);
+}
+
+//! Attach the Servo to the given pin (and, if specified, with the given range of legal pulse widths)
+unsigned int Servo::attach(unsigned int pin, int min, int max)
+{
+	this->min = min;
+	this->max = max;
+
+	servos[this->index].pin_number = pin;
+
+	pinMode(pin, OUTPUT);
+	digitalWrite(pin, LOW);
+
+	calculatePeriodRemainder();
+
+	servos[this->index].enabled = true;
+
+	return this->index;
+}
+
+//! Detach the Servo from its pin
+void Servo::detach()
+{
+    // Disable, clean up
+	servos[this->index].enabled = false;
+	servos[this->index].pulse_width = DEFAULT_SERVO_PULSE_WIDTH;
+	calculatePeriodRemainder();
+
+	digitalWrite(servos[this->index].pin_number, LOW);
+}
+
+bool Servo::attached(){
+	return servos[this->index].enabled;
+}
+
+//! ISR for generating the pulse widths
+void ServoIntHandler(void)
+{
+	// Clear the timer interrupt.
+	ROM_TimerIntClear(SERVO_TIMER, SERVO_TIMER_TRIGGER);
+
+	// Get the pulse width value for the current servo from the array
+	// and reload the timer with the new pulse width count value
+	// if we have already serviced all servos (currentServo = MAX_SERVO_NO)
+	// then this value should be the 20ms period value
+	if(currentServo < SERVOS_PER_TIMER)
+	{
+		ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * servos[currentServo].pulse_width);
+	}
+	else
+	{
+		ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * remainderPulseWidth);
+	}
+
+	// End the servo pulse set previously (if any)
+	if(currentServo > 0)  // If not the 1st Servo....
+	{
+		if (servos[currentServo - 1].enabled)
+		{
+			digitalWrite(servos[currentServo - 1].pin_number, LOW);
+		}
+	}
+
+	// Set the current servo pin HIGH
+	if(currentServo < SERVOS_PER_TIMER)
+	{
+		if (servos[currentServo - 1].enabled)
+		{
+			digitalWrite(servos[currentServo].pin_number, HIGH);
+		}
+		currentServo++;  // Advance to next servo for processing next time
+	}
+	else
+	{
+		currentServo = 0; // Start all over again
+	}
+}