- Main Page
- Related Pages
- Modules
- Data Structures
- Files

- File List
- Globals

# servo.c

Go to the documentation of this file.

    00001 /*********************************************************************************
    00002
    00003  Copyright 2006-2008 MakingThings
    00004
    00005  Licensed under the Apache License,
    00006  Version 2.0 (the "License"); you may not use this file except in compliance
    00007  with the License. You may obtain a copy of the License at
    00008
    00009  http://www.apache.org/licenses/LICENSE-2.0
    00010
    00011  Unless required by applicable law or agreed to in writing, software distributed
    00012  under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
    00013  CONDITIONS OF ANY KIND, either express or implied. See the License for
    00014  the specific language governing permissions and limitations under the License.
    00015
    00016 *********************************************************************************/
    00017
    00018 // MakingThings - Make Controller Board - 2006
    00019
    00020 /** \file servo.c
    00021   Servo Motor Controls.
    00022   Methods for controlling up to 4 servo motors with the Make Application Board.
    00023 */
    00024
    00025 #include "servo.h"
    00026 #include "io.h"
    00027 #include "fasttimer.h"
    00028 #include "config.h"
    00029
    00030 #include "AT91SAM7X256.h"
    00031
    00032 // These constants govern how long the pulse preamble is
    00033 // (SERVO_OFFSET) and how long the pulse can be (SERVO_MAX)
    00034 // So... if you want the servo pulse to be 1ms-2ms, you'd
    00035 // set SERVO_MAX = 1000, SERVO_OFFEST = 1000
    00036 // So... if you want the servo pulse to be 0.3ms-2.3ms, you'd
    00037 // set SERVO_MAX = 2000, SERVO_OFFEST = 300
    00038 //#define SERVO_MAX_PULSE     1000
    00039 #define SERVO_OFFSET  1000
    00040 #define SERVO_MIN_POSITION  -512
    00041 #define SERVO_MAX_POSITION  1536
    00042 #define SERVO_MID_POSITION 512
    00043 #define SERVO_CYCLE 2048
    00044 #define SERVO_SAFE_MIN 0
    00045 #define SERVO_SAFE_MAX 1023
    00046
    00047 #if ( APPBOARD_VERSION == 50 )
    00048   #define SERVO_0_IO IO_PA02
    00049   #define SERVO_1_IO IO_PA02
    00050   #define SERVO_2_IO IO_PA02
    00051   #define SERVO_3_IO IO_PA02
    00052 #endif
    00053 #if ( APPBOARD_VERSION == 90 || APPBOARD_VERSION == 95 || APPBOARD_VERSION == 100 )
    00054   #define SERVO_0_IO IO_PB24
    00055   #define SERVO_1_IO IO_PA23
    00056   #define SERVO_2_IO IO_PA21
    00057   #define SERVO_3_IO IO_PA22
    00058 #endif
    00059
    00060 void DisableFIQFromThumb( void );
    00061 void EnableFIQFromThumb( void );
    00062
    00063
    00064 void Servo_IRQCallback( int id );
    00065
    00066 static int Servo_Start( int index );
    00067 static int Servo_Stop( int index );
    00068 static int Servo_Init( void );
    00069 static int Servo_Deinit( void );
    00070 static int Servo_GetIo( int index );
    00071
    00072 #define SERVO_COUNT 4
    00073
    00074 typedef struct ServoControlS
    00075 {
    00076   int users;
    00077   int speed;
    00078   int positionRequested;
    00079   int position;
    00080   int pin;
    00081   AT91S_PIO* pIoBase;
    00082 } ServoControl;
    00083
    00084 typedef struct ServoS
    00085 {
    00086   int users;
    00087   int gap;
    00088   int index;
    00089   int state;
    00090   FastTimerEntry fastTimerEntry;
    00091   ServoControl* control[ SERVO_COUNT ];
    00092 } Servo_;
    00093
    00094
    00095 Servo_* Servo;
    00096
    00097 /** \defgroup Servo Servo
    00098   The Servo Motor subsystem controls speed and position control for up to 4 standard servo motors.
    00099   Standard servos have a range of motion of approximately 180 degrees, although this varies from motor to motor.
    00100   Be sure to plug in the connector with the correct orientation with regard to the GND/5V signals on the board.
    00101
    00102   Because not all servo motors are created equal, and not all of them can be safely driven across all 180 degrees,
    00103   the default (safe) range of motion is from values 0 to 1023.  The full range of motion is from -512 to 1536, but use this
    00104   extra range cautiously if you don't know how your motor can handle it.  A little gentle experimentation should do the trick.
    00105
    00106   You can also specify the speed with which the motors will respond to new position commands - a high
    00107   value will result in an immediate response, while a lower value can offer some smoothing when appropriate.
    00108
    00109   See the servo section in the <a href="http://www.makingthings.com/documentation/tutorial/application-board-overview/servos">
    00110   Application Board overview</a> for more detailed info.
    00111 * \ingroup Libraries
    00112 * @{
    00113 */
    00114
    00115 /**
    00116   Lock or unlock the I/O lines used by the servos.
    00117   Sets whether the specified Servo I/O is active.
    00118   @param index An integer specifying which servo (0 - 3).
    00119   @param state An integer specifying the active state - 1 (active) or 0 (inactive).
    00120   @return Zero on success.
    00121
    00122   \b Example
    00123   \code
    00124   // enable servo 2
    00125   Servo_SetActive(2, 1);
    00126   \endcode
    00127 */
    00128 int Servo_SetActive( int index, int state )
    00129 {
    00130   if ( index < 0 || index >= SERVO_COUNT )
    00131     return CONTROLLER_ERROR_ILLEGAL_INDEX;
    00132
    00133   if ( state )
    00134   {
    00135     if( Servo == NULL )
    00136     {
    00137       int retVal = Servo_Init( );
    00138       if( retVal != CONTROLLER_OK )
    00139         return retVal;
    00140     }
    00141     return Servo_Start( index );
    00142   }
    00143   else
    00144   {
    00145     if( Servo == NULL )
    00146       return -1;
    00147
    00148     int retVal = Servo_Stop( index );
    00149     if( retVal != CONTROLLER_OK )
    00150       return retVal;
    00151
    00152     if( --Servo->users <= 0 )
    00153       retVal = Servo_Deinit( );
    00154
    00155     return retVal;
    00156   }
    00157 }
    00158
    00159 /**
    00160   Check whether the I/O lines used by the servos are locked.
    00161   @param index An integer specifying which servo (0-3).
    00162   @return State - 1 (active) or 0 (inactive).
    00163
    00164   \b Example
    00165   \code
    00166   if( Servo_GetActive(2) )
    00167   {
    00168     // Servo 2 is active
    00169   }
    00170   else
    00171   {
    00172     // Servo 2 is inactive
    00173   }
    00174   \endcode
    00175 */
    00176 int Servo_GetActive( int index )
    00177 {
    00178   if ( index < 0 || index >= SERVO_COUNT || Servo == NULL )
    00179     return false;
    00180   return Servo->control[ index ]->users > 0;
    00181 }
    00182
    00183
    00184 /**
    00185   Set the position of the specified servo motor.
    00186   Most servos like to be driven within a "safe range" which usually ends up being somewhere around 110-120
    00187   degrees range of motion, or thereabouts.  Some servos don't mind being driven all the way to their 180
    00188   degree range of motion limit.  With this in mind, the range of values you can send to servos connected
    00189   to the Make Controller Kit is as follows:
    00190   - Values from 0-1023 correspond to the normal, or "safe", range of motion.
    00191   - You can also send values from -512 all the way up to 1536 to drive the servo through its full
    00192   range of motion.
    00193
    00194   Note that it is sometimes possible to damage your servo by driving it too far, so proceed with a bit of
    00195   caution when using the extended range until you know your servos can handle it.
    00196
    00197   @param index An integer specifying which servo (0 - 3).
    00198   @param position An integer specifying the servo position (0 - 1023).
    00199   @return status (0 = OK).
    00200
    00201   \b Example
    00202   \code
    00203   // set servo 1 to midway through the "safe" range
    00204   Servo_SetPosition(1, 512);
    00205   \endcode
    00206 */
    00207 int Servo_SetPosition( int index, int position )
    00208 {
    00209   if ( index < 0 || index >= SERVO_COUNT )
    00210     return CONTROLLER_ERROR_ILLEGAL_INDEX;
    00211
    00212   Servo_SetActive( index, 1 );
    00213
    00214   if ( position < SERVO_MIN_POSITION )
    00215     position = SERVO_SAFE_MIN;
    00216   if ( position > SERVO_MAX_POSITION )
    00217     position = SERVO_SAFE_MAX;
    00218
    00219   position += SERVO_OFFSET;
    00220
    00221   DisableFIQFromThumb();
    00222   Servo->control[ index ]->positionRequested = position << 6;
    00223   EnableFIQFromThumb();
    00224
    00225   return CONTROLLER_OK;
    00226 }
    00227
    00228 /**
    00229   Set the speed at which a servo will move in response to a call to Servo_SetPosition().
    00230   Higher values will result in a more immediate response, while lower values will more slowly step through
    00231   all the intermediate values, achieving a smoother motion.
    00232   @param index An integer specifying which servo (0 - 3).
    00233   @param speed An integer specifying the servo speed (0 - 1023).
    00234   @return status (0 = OK).
    00235
    00236   \b Example
    00237   \code
    00238   // set servo 1 half speed
    00239   Servo_SetSpeed(1, 512);
    00240   \endcode
    00241 */
    00242 int Servo_SetSpeed( int index, int speed )
    00243 {
    00244   if ( index < 0 || index >= SERVO_COUNT )
    00245     return CONTROLLER_ERROR_ILLEGAL_INDEX;
    00246
    00247   Servo_SetActive( index, 1 );
    00248
    00249   if ( speed < 1 )
    00250     speed = 1;
    00251   if( speed > 1023 )
    00252     speed = 1023;
    00253   DisableFIQFromThumb();
    00254   Servo->control[ index ]->speed = speed << 6;
    00255   EnableFIQFromThumb();
    00256
    00257   return CONTROLLER_OK;
    00258 }
    00259
    00260 /**
    00261   Read the current position of a servo motor.
    00262   @param index An integer specifying which servo (0 - 3).
    00263   @return The position (0 - 1023), or 0 on error.
    00264
    00265   \b Example
    00266   \code
    00267   int srv0_pos = Servo_GetPosition(0);
    00268   // now srv0_pos is the current position
    00269   \endcode
    00270 */
    00271 int Servo_GetPosition( int index )
    00272 {
    00273   if ( index < 0 || index >= SERVO_COUNT )
    00274     return 0;
    00275
    00276   Servo_SetActive( index, 1 );
    00277   return (Servo->control[ index ]->position >> 6) - SERVO_OFFSET;
    00278 }
    00279
    00280 /**
    00281   Get the speed at which a servo will move in response to a call to Servo_SetPosition().
    00282   Read the value previously set for the speed parameter.
    00283   @param index An integer specifying which servo (0 - 3).
    00284   @return The speed (0 - 1023), or 0 on error.
    00285
    00286   \b Example
    00287   \code
    00288   int srv0_speed = Servo_GetSpeed(0);
    00289   // now srv0_speed is the current speed
    00290   \endcode
    00291 */
    00292 int Servo_GetSpeed( int index )
    00293 {
    00294   if ( index < 0 || index >= SERVO_COUNT )
    00295     return 0;
    00296
    00297   Servo_SetActive( index, 1 );
    00298
    00299   return Servo->control[ index ]->speed >> 6;
    00300 }
    00301
    00302 /** @}
    00303 */
    00304
    00305 int Servo_Start( int index )
    00306 {
    00307   int status;
    00308
    00309   if ( index < 0 || index >= SERVO_COUNT )
    00310     return CONTROLLER_ERROR_ILLEGAL_INDEX;
    00311
    00312   ServoControl* sc = Servo->control[ index ];
    00313   if ( sc->users == 0 )
    00314   {
    00315     int io = Servo_GetIo( index );
    00316
    00317     // Try to lock the servo output
    00318     status = Io_Start( io, true );
    00319     if ( status != CONTROLLER_OK )
    00320     {
    00321       // Damn
    00322       Servo->users--;
    00323       sc->users--;
    00324       return status;
    00325     }
    00326
    00327     Io_SetPio( io, true );
    00328     Io_SetValue( io, true );
    00329     Io_SetDirection( io, IO_OUTPUT );
    00330
    00331     sc->position = (SERVO_MID_POSITION + SERVO_OFFSET) << 6;
    00332     sc->speed = 1023 << 6;
    00333     sc->users++;
    00334   }
    00335
    00336   return CONTROLLER_OK;
    00337 }
    00338
    00339 int Servo_Stop( int index )
    00340 {
    00341   if ( index < 0 || index >= SERVO_COUNT )
    00342     return CONTROLLER_ERROR_ILLEGAL_INDEX;
    00343
    00344   ServoControl* sc = Servo->control[ index ];
    00345
    00346   if ( sc->users <= 0 )
    00347     return CONTROLLER_ERROR_TOO_MANY_STOPS;
    00348
    00349   if ( --sc->users == 0 )
    00350   {
    00351     int io = Servo_GetIo( index );
    00352     //Io_SetInput( io );
    00353     Io_SetDirection( io, IO_INPUT );
    00354     Io_Stop( io );
    00355   }
    00356
    00357   return CONTROLLER_OK;
    00358 }
    00359
    00360
    00361 int Servo_GetIo( int index )
    00362 {
    00363   int io = -1;
    00364   switch ( index )
    00365   {
    00366     case 0:
    00367       io = SERVO_0_IO;
    00368       break;
    00369     case 1:
    00370       io = SERVO_1_IO;
    00371       break;
    00372     case 2:
    00373       io = SERVO_2_IO;
    00374       break;
    00375     case 3:
    00376       io = SERVO_3_IO;
    00377       break;
    00378   }
    00379   return io;
    00380 }
    00381
    00382 int Servo_Init()
    00383 {
    00384   Servo = MallocWait( sizeof( Servo_ ), 100 );
    00385
    00386   Servo->users = 0;
    00387   int i;
    00388   for( i = 0; i < SERVO_COUNT; i++ )
    00389   {
    00390     Servo->control[ i ] = NULL;
    00391     Servo->control[ i ] = MallocWait( sizeof( ServoControl ), 100 );
    00392
    00393     ServoControl* s = Servo->control[ i ];
    00394     s->users = 0;
    00395     s->positionRequested = (SERVO_MID_POSITION + SERVO_OFFSET) << 6;
    00396     switch( i )
    00397     {
    00398       case 0:
    00399         s->pIoBase = AT91C_BASE_PIOB;
    00400         s->pin = AT91C_PIO_PB24;
    00401         break;
    00402       case 1:
    00403         s->pIoBase = AT91C_BASE_PIOA;
    00404         s->pin = AT91C_PIO_PA23;
    00405         break;
    00406       case 2:
    00407         s->pIoBase = AT91C_BASE_PIOA;
    00408         s->pin = AT91C_PIO_PA21;
    00409         break;
    00410       case 3:
    00411         s->pIoBase = AT91C_BASE_PIOA;
    00412         s->pin = AT91C_PIO_PA22;
    00413         break;
    00414     }
    00415   }
    00416
    00417   // Global init stuff
    00418   Servo->state = 0;
    00419   Servo->gap = 1882; // forces 64'ish cycles a second (1894 + 2012 = 3906, 3906 * 4 = 15624, 15624 * 64 = 999936)
    00420   Servo->index = 0;
    00421
    00422   FastTimer_InitializeEntry( &Servo->fastTimerEntry, Servo_IRQCallback, 0, 2000, true );
    00423   FastTimer_Set( &Servo->fastTimerEntry );
    00424
    00425   return CONTROLLER_OK;
    00426 }
    00427
    00428 int Servo_Deinit()
    00429 {
    00430   // Disable the device
    00431   // AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
    00432   FastTimer_Cancel( &Servo->fastTimerEntry );
    00433   int i;
    00434   for( i = 0; i < SERVO_COUNT; i++ )
    00435     Free( Servo->control[i] );
    00436   Free( Servo );
    00437   Servo = NULL;
    00438   return CONTROLLER_OK;
    00439 }
    00440
    00441 void Servo_IRQCallback( int id )
    00442 {
    00443   (void)id;
    00444   int period;
    00445
    00446   switch ( Servo->state )
    00447   {
    00448     case 0:
    00449     {
    00450       if ( ++Servo->index >= SERVO_COUNT || Servo->index < 0 )
    00451         Servo->index = 0;
    00452       ServoControl* s = Servo->control[ Servo->index ];
    00453
    00454       if ( s->position != s->positionRequested )
    00455       {
    00456         int diff = s->positionRequested - s->position;
    00457         if ( diff < 0 )
    00458         {
    00459           s->position -= s->speed;
    00460           if ( s->position < s->positionRequested )
    00461             s->position = s->positionRequested;
    00462         }
    00463         else
    00464         {
    00465           s->position += s->speed;
    00466           if ( s->position > s->positionRequested )
    00467             s->position = s->positionRequested;
    00468         }
    00469       }
    00470
    00471       period = s->position >> 6;
    00472       if ( period >= (SERVO_MIN_POSITION + SERVO_OFFSET) && period <= (SERVO_MAX_POSITION + SERVO_OFFSET) )
    00473         s->pIoBase->PIO_CODR = s->pin;
    00474       else
    00475         period = SERVO_MAX_POSITION;
    00476       FastTimer_SetTime( &Servo->fastTimerEntry, period );
    00477       Servo->state = 1;
    00478       break;
    00479     }
    00480     case 1:
    00481     {
    00482       ServoControl* s = Servo->control[ Servo->index ];
    00483       period = s->position >> 6;
    00484       s->pIoBase->PIO_SODR = s->pin;
    00485       FastTimer_SetTime( &Servo->fastTimerEntry, Servo->gap + ( SERVO_CYCLE - period ) );
    00486       Servo->state = 0;
    00487       break;
    00488     }
    00489   }
    00490 }
    00491
    00492 #ifdef OSC // defined in config.h
    00493
    00494 /** \defgroup ServoOSC Servo - OSC
    00495   Control Servo motors with the Application Board via OSC.
    00496   \ingroup OSC
    00497
    00498   \section devices Devices
    00499   There are 4 Servo controllers available on the Application Board, numbered 0 - 3.\n
    00500   See the servo section in the Application Board user's guide for more information
    00501   on hooking up servos to the board.
    00502
    00503   \section properties Properties
    00504   Each servo controller has three properties:
    00505   - position
    00506   - speed
    00507   - active
    00508
    00509   \par Position
    00510   The \b position property corresponds to the position of the servo motor within its range of motion.
    00511   This value can be both read and written.
    00512   \par
    00513   Most servos like to be driven within a "safe range" which usually ends up being somewhere around 110-120
    00514   degrees range of motion, or thereabouts.  Some servos don't mind being driven all the way to their 180
    00515   degree range of motion limit.  With this in mind, the range of values you can send to servos connected
    00516   to the Make Controller Kit is as follows:
    00517   - Values from 0-1023 correspond to the normal, or "safe", range of motion.
    00518   - You can also send values from -512 to 1536 to drive the servo through its full
    00519   range of motion.
    00520   \par
    00521   Note that it is sometimes possible to damage your servo by driving it too far, so proceed with a bit of
    00522   caution when using the extended range until you know your servos can handle it.
    00523   \par
    00524   To set the first servo to one quarter its position, send the message
    00525   \verbatim /servo/0/position 256 \endverbatim
    00526   Leave the argument value off to read the position of the servo:
    00527   \verbatim /servo/0/position \endverbatim
    00528
    00529   \par Speed
    00530   The \b speed property corresponds to the speed with which the servo responds to changes
    00531   of position.
    00532   This value can be both read and written, and the range of values is 0 - 1023.  A speed of 1023
    00533   means the servo responds immediately, and lower values will result in slower, and smoother, responses.
    00534   \par
    00535   To set the speed of the first servo to just under full speed, send a message like
    00536   \verbatim /servo/0/speed 975 \endverbatim
    00537   Adjust the argument value to one that suits your application.\n
    00538   Leave the argument value off to read the position of the servo:
    00539   \verbatim /servo/0/speed \endverbatim
    00540
    00541   \par Active
    00542   The \b active property corresponds to the active state of the servo.
    00543   If the servo is set to be active, no other tasks will be able to
    00544   write to the same I/O lines.  If you're not seeing appropriate
    00545   responses to your messages to a servo, check the whether it's
    00546   locked by sending a message like
    00547   \verbatim /servo/3/active \endverbatim
    00548 */
    00549
    00550 #include "osc.h"
    00551 #include "string.h"
    00552 #include "stdio.h"
    00553
    00554 // Need a list of property names
    00555 // MUST end in zero
    00556 static char* ServoOsc_Name = "servo";
    00557 static char* ServoOsc_PropertyNames[] = { "active", "position", "speed", 0 }; // must have a trailing 0
    00558
    00559 int ServoOsc_PropertySet( int index, int property, int value );
    00560 int ServoOsc_PropertyGet( int index, int property );
    00561
    00562 // Returns the name of the subsystem
    00563 const char* ServoOsc_GetName( )
    00564 {
    00565   return ServoOsc_Name;
    00566 }
    00567
    00568 // Now getting a message.  This is actually a part message, with the first
    00569 // part (the subsystem) already parsed off.
    00570 int ServoOsc_ReceiveMessage( int channel, char* message, int length )
    00571 {
    00572   int status = Osc_IndexIntReceiverHelper( channel, message, length,
    00573                                            SERVO_COUNT, ServoOsc_Name,
    00574                                            ServoOsc_PropertySet, ServoOsc_PropertyGet,
    00575                                            ServoOsc_PropertyNames );
    00576
    00577   if ( status != CONTROLLER_OK )
    00578     return Osc_SendError( channel, ServoOsc_Name, status );
    00579   return CONTROLLER_OK;
    00580 }
    00581
    00582 // Set the index LED, property with the value
    00583 int ServoOsc_PropertySet( int index, int property, int value )
    00584 {
    00585   switch ( property )
    00586   {
    00587     case 0:
    00588       Servo_SetActive( index, value );
    00589       break;
    00590     case 1:
    00591       Servo_SetPosition( index, value );
    00592       break;
    00593     case 2:
    00594       Servo_SetSpeed( index, value );
    00595       break;
    00596   }
    00597   return CONTROLLER_OK;
    00598 }
    00599
    00600 // Get the index LED, property
    00601 int ServoOsc_PropertyGet( int index, int property )
    00602 {
    00603   int value = 0;
    00604   switch ( property )
    00605   {
    00606     case 0:
    00607       value = Servo_GetActive( index );
    00608       break;
    00609     case 1:
    00610       value = Servo_GetPosition( index );
    00611       break;
    00612     case 2:
    00613       value = Servo_GetSpeed( index );
    00614       break;
    00615   }
    00616
    00617   return value;
    00618 }
    00619
    00620 #endif

The Make Controller Kit is an open source project maintained by MakingThings.
MakingThings code is released under the Apache 2.0 license.
Bug tracker, development wiki and status can be found at http://dev.makingthings.com.
This document was last updated on 18 May 2009.