bendns' repos / ArduinoStuff
arduino stuffs
Diffstat (limited to 'libraries/Servo/src/nrf52/Servo.cpp')
-rw-r--r--libraries/Servo/src/nrf52/Servo.cpp134
1 files changed, 134 insertions, 0 deletions
diff --git a/libraries/Servo/src/nrf52/Servo.cpp b/libraries/Servo/src/nrf52/Servo.cpp
new file mode 100644
index 0000000..72bd504
--- /dev/null
+++ b/libraries/Servo/src/nrf52/Servo.cpp
@@ -0,0 +1,134 @@
+/*
+ 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
+
+
+
+uint32_t group_pins[3][NRF_PWM_CHANNEL_COUNT]={{NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}};
+static uint16_t seq_values[3][NRF_PWM_CHANNEL_COUNT]={{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+
+Servo::Servo()
+{
+ if (ServoCount < MAX_SERVOS) {
+ this->servoIndex = ServoCount++; // assign a servo index to this instance
+ } else {
+ this->servoIndex = INVALID_SERVO; // too many servos
+ }
+
+}
+
+uint8_t Servo::attach(int pin)
+{
+
+ return this->attach(pin, 0, 2500);
+}
+
+
+uint8_t Servo::attach(int pin, int min, int max)
+{
+ int servo_min, servo_max;
+ if (this->servoIndex < MAX_SERVOS) {
+ pinMode(pin, OUTPUT); // set servo pin to output
+ servos[this->servoIndex].Pin.nbr = pin;
+
+ if(min < servo_min) min = servo_min;
+ if (max > servo_max) max = servo_max;
+ this->min = min;
+ this->max = max;
+
+ servos[this->servoIndex].Pin.isActive = true;
+
+ }
+ return this->servoIndex;
+}
+
+void Servo::detach()
+{
+ servos[this->servoIndex].Pin.isActive = false;
+}
+
+
+void Servo::write(int value)
+{
+ if (value < 0)
+ value = 0;
+ else if (value > 180)
+ value = 180;
+ value = map(value, 0, 180, MIN_PULSE, MAX_PULSE);
+
+ writeMicroseconds(value);
+}
+
+
+void Servo::writeMicroseconds(int value)
+{
+ uint8_t channel, instance;
+ uint8_t pin = servos[this->servoIndex].Pin.nbr;
+ //instance of PWM module is MSB - look at VWariant.h
+ instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16;
+ //index of PWM channel is LSB - look at VWariant.h
+ channel=g_APinDescription[pin].ulPWMChannel & 0x0F;
+ group_pins[instance][channel]=g_APinDescription[pin].ulPin;
+ NRF_PWM_Type * PWMInstance = instance == 0 ? NRF_PWM0 : (instance == 1 ? NRF_PWM1 : NRF_PWM2);
+ //configure PWM instance and enable it
+ seq_values[instance][channel]= value | 0x8000;
+ nrf_pwm_sequence_t const seq={
+ seq_values[instance],
+ NRF_PWM_VALUES_LENGTH(seq_values),
+ 0,
+ 0
+ };
+ nrf_pwm_pins_set(PWMInstance, group_pins[instance]);
+ nrf_pwm_enable(PWMInstance);
+ nrf_pwm_configure(PWMInstance, NRF_PWM_CLK_125kHz, NRF_PWM_MODE_UP, 2500); // 20ms - 50Hz
+ nrf_pwm_decoder_set(PWMInstance, NRF_PWM_LOAD_INDIVIDUAL, NRF_PWM_STEP_AUTO);
+ nrf_pwm_sequence_set(PWMInstance, 0, &seq);
+ nrf_pwm_loop_set(PWMInstance, 0UL);
+ nrf_pwm_task_trigger(PWMInstance, NRF_PWM_TASK_SEQSTART0);
+}
+
+int Servo::read() // return the value as degrees
+{
+ return map(readMicroseconds(), MIN_PULSE, MAX_PULSE, 0, 180);
+}
+
+int Servo::readMicroseconds()
+{
+ uint8_t channel, instance;
+ uint8_t pin=servos[this->servoIndex].Pin.nbr;
+ instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16;
+ channel=g_APinDescription[pin].ulPWMChannel & 0x0F;
+ // remove the 16th bit we added before
+ return seq_values[instance][channel] & 0x7FFF;
+}
+
+bool Servo::attached()
+{
+ return servos[this->servoIndex].Pin.isActive;
+}
+
+#endif // ARDUINO_ARCH_NRF52