Ignore:
Timestamp:
Apr 30, 2016, 11:29:25 PM (8 years ago)
Author:
ertl-honda
Message:

1.7.10のファイルに更新

File:
1 edited

Legend:

Unmodified
Added
Removed

  • rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp

    r136 r224  
    1616  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
    1717*/
     18//Edited by Arduino Srl development team.
    1819
    1920#if defined(ARDUINO_ARCH_SAMD)
    … …  
    2223#include <Servo.h>
    2324
     25
    2426static servo_t servos[MAX_SERVOS];                          // static array of servo structures
    2527
    2628uint8_t ServoCount = 0;                                     // the total number of attached servos
     29uint8_t isTC = 0 ;
     30Tc* TCx ;
    2731Tcc* TCCx;
    2832uint8_t Channelx = 0;
    … …  
    3034// convenience macros
    3135
    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
     36#define SERVO_MIN_TCC() (MIN_PULSE_WIDTH_SAMD_TCC)  // minimum value in uS for this servo if TCC timer is used
     37#define SERVO_MAX_TCC() (MAX_PULSE_WIDTH_SAMD_TCC)  // maximum value in uS for this servo if TCC timer is used
     38#define SERVO_MIN_TC() (MIN_PULSE_WIDTH_SAMD_TC)  // minimum value in uS for this servo if TC timer is used
     39#define SERVO_MAX_TC() (MAX_PULSE_WIDTH_SAMD_TC)  // maximum value in uS for this servo if TC timer is used
    3440
    3541/************ static functions common to all instances ***********************/
    … …  
    4652  if (ServoCount < MAX_SERVOS) {
    4753    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
     54  } else {                                                 
     55    this->servoIndex = INVALID_SERVO;                                   // too many servos
    5056  }
    5157}
    … …  
    5359uint8_t Servo::attach(int pin)
    5460{
    55   return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD);
     61  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
     62        return this->attach(pin, SERVO_MIN_TC(), SERVO_MAX_TC());
     63  }
     64  else{
     65        return this->attach(pin, SERVO_MIN_TCC(), SERVO_MAX_TCC());
     66  }
    5667}
    5768
    … …  
    6374    pinMode(pin, OUTPUT);                                   // set servo pin to output
    6475    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;
     76        int servo_min, servo_max;
     77        if(pin==4 | pin==5 | pin==10 | pin==12){
     78                servo_min=SERVO_MIN_TC();
     79                servo_max=SERVO_MAX_TC();
     80        }
     81        else{
     82                servo_min=SERVO_MIN_TCC();
     83                servo_max=SERVO_MAX_TCC();
     84        }
     85    if(min > servo_min) min = servo_min;
     86        if (max > servo_max) max = servo_max;
    6787        this->min  = min;
    6888    this->max  = max;
    … …  
    7595                        TCCx=TCC0;
    7696                        Channelx=0;
     97                        isTC=0;
    7798                }
    7899                break;
    … …  
    83104                        TCCx=TCC0;
    84105                        Channelx=1;
    85                 }
    86                 break;
    87                
     106                        isTC=0;
     107                }
     108                break;
     109
     110                        case 4:
     111                {
     112                        pinPeripheral(pin, g_APinDescription[pin].ulPinType);
     113                        TCx=TC3;
     114                        Channelx=0;
     115                        isTC=1;
     116                         
     117                }
     118                break;
     119
     120                case 5:
     121                {
     122                        pinPeripheral(pin, g_APinDescription[pin].ulPinType);
     123                        TCx=TC3;
     124                        Channelx=1;
     125                        isTC=1;
     126                         
     127                }
     128                break;
    88129                case 6:
    89130                {
    … …  
    91132                        TCCx=TCC0;
    92133                        Channelx=2;
     134                        isTC=0;
    93135                }
    94136                break;
    … …  
    99141                        TCCx=TCC0;
    100142                        Channelx=3;
     143                        isTC=0;
    101144                }
    102145                break;
    … …  
    107150                        TCCx=TCC1;
    108151                        Channelx=0;
     152                        isTC=0;
    109153                }
    110154                break;
    … …  
    115159                        TCCx=TCC1;
    116160                        Channelx=1;
     161                        isTC=0;
     162                }
     163                break;
     164               
     165                case 10:
     166                {
     167                        pinPeripheral(pin, g_APinDescription[pin].ulPinType);
     168                        TCx=TC3;
     169                        Channelx=0;
     170                        isTC=1;
     171                         
    117172                }
    118173                break;
    … …  
    123178                        TCCx=TCC2;
    124179                        Channelx=0;
     180                        isTC=0;
     181                }
     182                break;
     183               
     184                case 12:
     185                {
     186                        pinPeripheral(pin, g_APinDescription[pin].ulPinType);
     187                        TCx=TC3;
     188                        Channelx=1;
     189                        isTC=1;
     190                         
    125191                }
    126192                break;
    … …  
    131197                        TCCx=TCC2;
    132198                        Channelx=1;
     199                        isTC=0;
    133200                }
    134201                break;
    … …  
    140207       
    141208        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 )) ;
     209        else if((TCCx==TCC2) | (TCx==TC3 ))GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
    143210        else;
    144                
    145         if(servos[this->servoIndex].Pin.isActive == false)
    146         {       
    147                 TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE);        //disable TCC module
     211
     212        if(servos[this->servoIndex].Pin.isActive == false){
     213        // Set PORT
     214    if ( isTC )
     215    {
     216      // -- Configure TC
     217      //DISABLE TCx
     218      TCx->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
     219      //Set Timer counter Mode to 16 bits
     220          TCx->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
     221          //Set Prescaler to divide by 2
     222          TCx->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2;
     223      //Set TCx as normal PWM
     224      TCx->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM;
     225          //default value for servo position
     226      TCx->COUNT16.CC[Channelx].reg = 1500;
     227          //ENABLE TCx
     228      TCx->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
     229          servos[this->servoIndex].Pin.isActive = true;
     230    }
     231    else
     232    {
     233      // -- Configure TCC
     234         
     235         TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE);        //disable TCC module
    148236                TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8;   //setting prescaler to divide by 8
    149237                TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM;      //Set TCCx as normal PWM
    … …  
    151239                TCCx->PER.reg=20000;                          // setting servo frequency (50 hz)
    152240                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
     241                servos[this->servoIndex].Pin.isActive = true;  
     242    }
     243       
     244        }
    157245  }
    158246  return this->servoIndex;
    … …  
    167255  else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
    168256  else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE); 
     257  else if ((servos[this->servoIndex].Pin.nbr == 4 ) | (servos[this->servoIndex].Pin.nbr == 5 ) | (servos[this->servoIndex].Pin.nbr == 10 ) | (servos[this->servoIndex].Pin.nbr == 12 ))TC3->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
    169258}
    170259
    171260void 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   }
     261{ 
     262  //select the right values for servo motor
     263  int servo_min;
     264  int servo_max;
     265  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
     266        servo_min=SERVO_MIN_TC();
     267        servo_max=SERVO_MAX_TC();
     268        // treat values less than 1700 as angles in degrees (valid values in microseconds are handled as microseconds)
     269        if (value < servo_min)
     270        {
     271                if (value < 0)
     272                        value = 0;
     273                else if (value > 180)
     274                        value = 180;
     275                value = map(value, 0, 180, servo_min, servo_max);
     276        }
     277  }
     278  else{
     279        servo_min=SERVO_MIN_TCC();
     280        servo_max=SERVO_MAX_TCC();
     281        // treat values less than 400 as angles in degrees (valid values in microseconds are handled as microseconds)
     282        if (value < servo_min)
     283        {
     284                if (value < 0)
     285                        value = 0;
     286                else if (value > 180)
     287                        value = 180;
     288                value = map(value, 0, 180, servo_min, servo_max);
     289        }
     290  }
     291   
    183292  writeMicroseconds(value);
    184293}
    … …  
    190299  if( (channel < MAX_SERVOS) )   // ensure channel is valid
    191300  {
    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
     301        //select the right values for servo motor
     302        int servo_min;
     303        int servo_max;
     304        if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
     305                servo_min=SERVO_MIN_TC();
     306                servo_max=SERVO_MAX_TC();
     307        }
     308        else{
     309                servo_min=SERVO_MIN_TCC();
     310                servo_max=SERVO_MAX_TCC();
     311        }
     312    if (value < servo_min)          // ensure pulse width is valid
     313      value = servo_min;
     314    else if (value > servo_max)
     315      value = servo_max;
     316    servos[this->servoIndex].ticks = value;
    197317        switch(servos[this->servoIndex].Pin.nbr)
    198318        {
    … …  
    205325                        break;
    206326                       
     327                case 4:
     328                        TC3->COUNT16.CC[0].reg = value;
     329                        break; 
     330                       
     331                case 5:
     332                        TC3->COUNT16.CC[1].reg = value;
     333                        break; 
     334                       
    207335                case 6:
    208336                        TCC0->CC[2].reg=value;
    … …  
    220348                        TCC1->CC[1].reg=value;
    221349                        break;
     350               
     351                case 10:
     352                        TC3->COUNT16.CC[0].reg = value;
     353                        break;         
    222354                       
    223355                case 11:
    … …  
    225357                        break;
    226358
     359                case 12:
     360                        TC3->COUNT16.CC[1].reg = value;
     361                        break; 
     362
    227363                case 13:
    228364                        TCC2->CC[1].reg=value;
    … …  
    234370        }
    235371   
    236     //servos[this->servoIndex].ticks = value;   //da sistemare
     372    //servos[this->servoIndex].ticks = value;   //to be fixed
    237373        //servos[channel].ticks = value;
    238374  }
    … …  
    241377int Servo::read() // return the value as degrees
    242378{
    243   return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
     379  //select the right values for servo motor
     380  int servo_min;
     381  int servo_max;
     382  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
     383        servo_min=SERVO_MIN_TC();
     384        servo_max=SERVO_MAX_TC();
     385        }
     386  else{
     387        servo_min=SERVO_MIN_TCC();
     388        servo_max=SERVO_MAX_TCC();
     389        }
     390  return map(readMicroseconds(), servo_min, servo_max, 0, 180);
    244391}
    245392
Note: See TracChangeset for help on using the changeset viewer.