ArduinoStuff - arduino stuffs


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
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