[136] | 1 | /*
|
---|
| 2 | Copyright (c) 2013 Arduino LLC. All right reserved.
|
---|
| 3 |
|
---|
| 4 | This library is free software; you can redistribute it and/or
|
---|
| 5 | modify it under the terms of the GNU Lesser General Public
|
---|
| 6 | License as published by the Free Software Foundation; either
|
---|
| 7 | version 2.1 of the License, or (at your option) any later version.
|
---|
| 8 |
|
---|
| 9 | This library is distributed in the hope that it will be useful,
|
---|
| 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
---|
| 12 | Lesser General Public License for more details.
|
---|
| 13 |
|
---|
| 14 | You should have received a copy of the GNU Lesser General Public
|
---|
| 15 | License along with this library; if not, write to the Free Software
|
---|
| 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
---|
| 17 | */
|
---|
| 18 |
|
---|
| 19 | #if defined(ARDUINO_ARCH_SAMD)
|
---|
| 20 |
|
---|
| 21 | #include <Arduino.h>
|
---|
| 22 | #include <Servo.h>
|
---|
| 23 |
|
---|
| 24 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
| 25 |
|
---|
| 26 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
| 27 | Tcc* TCCx;
|
---|
| 28 | uint8_t Channelx = 0;
|
---|
| 29 |
|
---|
| 30 | // convenience macros
|
---|
| 31 |
|
---|
| 32 | #define SERVO_MIN() (MIN_PULSE_WIDTH_SAMD) // minimum value in uS for this servo
|
---|
| 33 | #define SERVO_MAX() (MAX_PULSE_WIDTH_SAMD) // maximum value in uS for this servo
|
---|
| 34 |
|
---|
| 35 | /************ static functions common to all instances ***********************/
|
---|
| 36 |
|
---|
| 37 | //------------------------------------------------------------------------------
|
---|
| 38 | /// Interrupt handler for the TC0 channel 1.
|
---|
| 39 | //------------------------------------------------------------------------------
|
---|
| 40 |
|
---|
| 41 |
|
---|
| 42 | /****************** end of static functions ******************************/
|
---|
| 43 |
|
---|
| 44 | Servo::Servo()
|
---|
| 45 | {
|
---|
| 46 | if (ServoCount < MAX_SERVOS) {
|
---|
| 47 | this->servoIndex = ServoCount++; // assign a servo index to this instance
|
---|
| 48 | } else { //su questo costruttore forse si deve tornare
|
---|
| 49 | this->servoIndex = INVALID_SERVO; // too many servos
|
---|
| 50 | }
|
---|
| 51 | }
|
---|
| 52 |
|
---|
| 53 | uint8_t Servo::attach(int pin)
|
---|
| 54 | {
|
---|
| 55 | return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD);
|
---|
| 56 | }
|
---|
| 57 |
|
---|
| 58 | uint8_t Servo::attach(int pin, int min, int max)
|
---|
| 59 | {
|
---|
| 60 |
|
---|
| 61 |
|
---|
| 62 | if (this->servoIndex < MAX_SERVOS) {
|
---|
| 63 | pinMode(pin, OUTPUT); // set servo pin to output
|
---|
| 64 | servos[this->servoIndex].Pin.nbr = pin;
|
---|
| 65 | if(min > MIN_PULSE_WIDTH_SAMD) min = MIN_PULSE_WIDTH_SAMD;
|
---|
| 66 | if (max > MAX_PULSE_WIDTH_SAMD) max = MAX_PULSE_WIDTH_SAMD;
|
---|
| 67 | this->min = min;
|
---|
| 68 | this->max = max;
|
---|
| 69 |
|
---|
| 70 | switch(pin)
|
---|
| 71 | {
|
---|
| 72 | case 2:
|
---|
| 73 | {
|
---|
| 74 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 75 | TCCx=TCC0;
|
---|
| 76 | Channelx=0;
|
---|
| 77 | }
|
---|
| 78 | break;
|
---|
| 79 |
|
---|
| 80 | case 3:
|
---|
| 81 | {
|
---|
| 82 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 83 | TCCx=TCC0;
|
---|
| 84 | Channelx=1;
|
---|
| 85 | }
|
---|
| 86 | break;
|
---|
| 87 |
|
---|
| 88 | case 6:
|
---|
| 89 | {
|
---|
| 90 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 91 | TCCx=TCC0;
|
---|
| 92 | Channelx=2;
|
---|
| 93 | }
|
---|
| 94 | break;
|
---|
| 95 |
|
---|
| 96 | case 7:
|
---|
| 97 | {
|
---|
| 98 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 99 | TCCx=TCC0;
|
---|
| 100 | Channelx=3;
|
---|
| 101 | }
|
---|
| 102 | break;
|
---|
| 103 |
|
---|
| 104 | case 8:
|
---|
| 105 | {
|
---|
| 106 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 107 | TCCx=TCC1;
|
---|
| 108 | Channelx=0;
|
---|
| 109 | }
|
---|
| 110 | break;
|
---|
| 111 |
|
---|
| 112 | case 9:
|
---|
| 113 | {
|
---|
| 114 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 115 | TCCx=TCC1;
|
---|
| 116 | Channelx=1;
|
---|
| 117 | }
|
---|
| 118 | break;
|
---|
| 119 |
|
---|
| 120 | case 11:
|
---|
| 121 | {
|
---|
| 122 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 123 | TCCx=TCC2;
|
---|
| 124 | Channelx=0;
|
---|
| 125 | }
|
---|
| 126 | break;
|
---|
| 127 |
|
---|
| 128 | case 13:
|
---|
| 129 | {
|
---|
| 130 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
| 131 | TCCx=TCC2;
|
---|
| 132 | Channelx=1;
|
---|
| 133 | }
|
---|
| 134 | break;
|
---|
| 135 |
|
---|
| 136 | default:
|
---|
| 137 | break;
|
---|
| 138 |
|
---|
| 139 | }
|
---|
| 140 |
|
---|
| 141 | if ((TCCx==TCC0) | (TCCx==TCC1)) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
|
---|
| 142 | else if(TCCx==TCC2) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
|
---|
| 143 | else;
|
---|
| 144 |
|
---|
| 145 | if(servos[this->servoIndex].Pin.isActive == false)
|
---|
| 146 | {
|
---|
| 147 | TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE); //disable TCC module
|
---|
| 148 | TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8; //setting prescaler to divide by 8
|
---|
| 149 | TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; //Set TCCx as normal PWM
|
---|
| 150 | TCCx->CC[Channelx].reg=1500; //default value for servo position
|
---|
| 151 | TCCx->PER.reg=20000; // setting servo frequency (50 hz)
|
---|
| 152 | TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; //ENABLE TCCx
|
---|
| 153 | servos[this->servoIndex].Pin.isActive = true;
|
---|
| 154 | }
|
---|
| 155 |
|
---|
| 156 | //servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
|
---|
| 157 | }
|
---|
| 158 | return this->servoIndex;
|
---|
| 159 | }
|
---|
| 160 |
|
---|
| 161 | void Servo::detach()
|
---|
| 162 | {
|
---|
| 163 |
|
---|
| 164 |
|
---|
| 165 | servos[this->servoIndex].Pin.isActive = false;
|
---|
| 166 | if((servos[this->servoIndex].Pin.nbr == 2) | (servos[this->servoIndex].Pin.nbr == 3) | (servos[this->servoIndex].Pin.nbr == 6) | (servos[this->servoIndex].Pin.nbr == 7)) TCC0->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
| 167 | else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
| 168 | else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
| 169 | }
|
---|
| 170 |
|
---|
| 171 | void Servo::write(int value)
|
---|
| 172 | {
|
---|
| 173 | // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
| 174 | if (value < MIN_PULSE_WIDTH)
|
---|
| 175 | {
|
---|
| 176 | if (value < 0)
|
---|
| 177 | value = 0;
|
---|
| 178 | else if (value > 180)
|
---|
| 179 | value = 180;
|
---|
| 180 |
|
---|
| 181 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
|
---|
| 182 | }
|
---|
| 183 | writeMicroseconds(value);
|
---|
| 184 | }
|
---|
| 185 |
|
---|
| 186 | void Servo::writeMicroseconds(int value)
|
---|
| 187 | {
|
---|
| 188 | // calculate and store the values for the given channel
|
---|
| 189 | byte channel = this->servoIndex;
|
---|
| 190 | if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
---|
| 191 | {
|
---|
| 192 | if (value < SERVO_MIN()) // ensure pulse width is valid
|
---|
| 193 | value = SERVO_MIN();
|
---|
| 194 | else if (value > SERVO_MAX())
|
---|
| 195 | value = SERVO_MAX();
|
---|
| 196 | servos[this->servoIndex].ticks = value; //da sistemare
|
---|
| 197 | switch(servos[this->servoIndex].Pin.nbr)
|
---|
| 198 | {
|
---|
| 199 | case 2:
|
---|
| 200 | TCC0->CC[0].reg=value;
|
---|
| 201 | break;
|
---|
| 202 |
|
---|
| 203 | case 3:
|
---|
| 204 | TCC0->CC[1].reg=value;
|
---|
| 205 | break;
|
---|
| 206 |
|
---|
| 207 | case 6:
|
---|
| 208 | TCC0->CC[2].reg=value;
|
---|
| 209 | break;
|
---|
| 210 |
|
---|
| 211 | case 7:
|
---|
| 212 | TCC0->CC[3].reg=value;
|
---|
| 213 | break;
|
---|
| 214 |
|
---|
| 215 | case 8:
|
---|
| 216 | TCC1->CC[0].reg=value;
|
---|
| 217 | break;
|
---|
| 218 |
|
---|
| 219 | case 9:
|
---|
| 220 | TCC1->CC[1].reg=value;
|
---|
| 221 | break;
|
---|
| 222 |
|
---|
| 223 | case 11:
|
---|
| 224 | TCC2->CC[0].reg=value;
|
---|
| 225 | break;
|
---|
| 226 |
|
---|
| 227 | case 13:
|
---|
| 228 | TCC2->CC[1].reg=value;
|
---|
| 229 | break;
|
---|
| 230 |
|
---|
| 231 | default:
|
---|
| 232 | break;
|
---|
| 233 |
|
---|
| 234 | }
|
---|
| 235 |
|
---|
| 236 | //servos[this->servoIndex].ticks = value; //da sistemare
|
---|
| 237 | //servos[channel].ticks = value;
|
---|
| 238 | }
|
---|
| 239 | }
|
---|
| 240 |
|
---|
| 241 | int Servo::read() // return the value as degrees
|
---|
| 242 | {
|
---|
| 243 | return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
|
---|
| 244 | }
|
---|
| 245 |
|
---|
| 246 | int Servo::readMicroseconds()
|
---|
| 247 | {
|
---|
| 248 | unsigned int pulsewidth;
|
---|
| 249 | if (this->servoIndex != INVALID_SERVO)
|
---|
| 250 | pulsewidth = servos[this->servoIndex].ticks;
|
---|
| 251 | else
|
---|
| 252 | pulsewidth = 0;
|
---|
| 253 |
|
---|
| 254 | return pulsewidth;
|
---|
| 255 | }
|
---|
| 256 |
|
---|
| 257 | bool Servo::attached()
|
---|
| 258 | {
|
---|
| 259 | return servos[this->servoIndex].Pin.isActive;
|
---|
| 260 | }
|
---|
| 261 |
|
---|
| 262 | #endif // ARDUINO_ARCH_SAM
|
---|
| 263 |
|
---|