added basic code

1 files changed, 127 insertions, 0 deletions

diff --git a/arduino/libraries/Servo/src/nrf52/Servo.cpp b/arduino/libraries/Servo/src/nrf52/Servo.cpp
new file mode 100755
index 0000000..c9a0375
--- /dev/null
+++ b/arduino/libraries/Servo/src/nrf52/Servo.cpp
@@ -0,0 +1,127 @@
+/*
+  Copyright (c) 2016 Arduino. 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
+*/
+
+#if defined(ARDUINO_ARCH_NRF52)
+
+#include <Arduino.h>
+#include <Servo.h>
+
+
+static servo_t servos[MAX_SERVOS];              // static array of servo structures
+uint8_t ServoCount = 0;                         // the total number of attached servos
+
+Servo::Servo()
+{
+  if (ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;            // assign a servo index to this instance
+  } else {
+    this->servoIndex = INVALID_SERVO;  					// too many servos
+  }
+
+  this->pwm = NULL;
+}
+
+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;
+
+    if (min < MIN_PULSE_WIDTH) min = MIN_PULSE_WIDTH;
+    if (max > MAX_PULSE_WIDTH) max = MAX_PULSE_WIDTH;
+
+    //fix min if conversion to pulse cycle value is too low
+    if((min/DUTY_CYCLE_RESOLUTION)*DUTY_CYCLE_RESOLUTION<min) min+=DUTY_CYCLE_RESOLUTION;
+	  
+    this->min  = min;
+    this->max  = max;
+
+    servos[this->servoIndex].Pin.isActive = true;
+
+    // Adafruit, add pin to 1 of available Hw PWM
+    for(int i=0; i<HWPWM_MODULE_NUM; i++)
+    {
+      if ( HwPWMx[i]->addPin(pin) )
+      {
+        this->pwm = HwPWMx[i];
+        break;
+      }
+    }
+
+    this->pwm->setMaxValue(MAXVALUE);
+    this->pwm->setClockDiv(CLOCKDIV);
+
+  }
+  return this->servoIndex;
+}
+
+void Servo::detach()
+{
+  uint8_t const pin = servos[this->servoIndex].Pin.nbr;
+
+	servos[this->servoIndex].Pin.isActive = false;
+
+	// remove pin from HW PWM
+	this->pwm->removePin(pin);
+}
+
+
+void Servo::write(int value)
+{  
+	if (value < 0)
+		value = 0;
+	else if (value > 180)
+		value = 180;
+	value = map(value, 0, 180, this->min, this->max);
+	
+	writeMicroseconds(value);
+}
+
+
+void Servo::writeMicroseconds(int value)
+{
+	uint8_t pin = servos[this->servoIndex].Pin.nbr;
+	
+	if ( this->pwm ) this->pwm->writePin(pin, value/DUTY_CYCLE_RESOLUTION);
+}
+
+int Servo::read() // return the value as degrees
+{
+	return map(readMicroseconds(), this->min, this->max, 0, 180);
+}
+
+int Servo::readMicroseconds()
+{	
+	uint8_t pin = servos[this->servoIndex].Pin.nbr;
+
+	if ( this->pwm ) return this->pwm->readPin(pin)*DUTY_CYCLE_RESOLUTION;
+
+	return 0;
+}
+
+bool Servo::attached()
+{
+  return servos[this->servoIndex].Pin.isActive;
+}
+
+#endif // ARDUINO_ARCH_NRF52