Mindstorms 3rd Party ROBOTC Drivers RobotC
[Home] [Download] [Submit a bug/suggestion] [ROBOTC Forums] [Blog] [Support this project]

mindensors-servo.h

Go to the documentation of this file.
00001 /*!@addtogroup mindsensors
00002  * @{
00003  * @defgroup nxtservo Servo Controller
00004  * Servo Controller
00005  * @{
00006  */
00007 
00008 /*
00009  * $Id: mindensors-servo.h 133 2013-03-10 15:15:38Z xander $
00010  */
00011 
00012 #ifndef __NXTSERVO_H__
00013 #define __NXTSERVO_H__
00014 /** \file mindensors-servo.h
00015  * \brief Mindsensors NXTServo Sensor Driver
00016  *
00017  * mindensors-servo.h provides an API for the Mindsensors NXTServo Sensor
00018  *
00019  * Changelog:
00020  * - 0.1: Initial release
00021  *
00022  * Credits:
00023  * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.
00024  *
00025  * License: You may use this code as you wish, provided you give credit where its due.
00026  *
00027  * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. 
00028 
00029  * \author Xander Soldaat (xander_at_botbench.com)
00030  * \date 30 September 2009
00031  * \version 0.1
00032  * \example mindensors-servo-test1.c
00033  */
00034 
00035 #pragma systemFile
00036 
00037 #ifndef __COMMON_H__
00038 #include "common.h"
00039 #endif
00040 
00041 #define NXTSERVO_I2C_ADDR     0xB0      /*!< NXT Servo I2C device address */
00042 
00043 // Command register, can be written for commands and read for battery voltage level
00044 #define NXTSERVO_CMD          0x41      /*!< Servo command register
00045 */
00046 // Position registers, can be read and written
00047 #define NXTSERVO_POS_CHAN1    0x42      /*!< Servo channel 1 position (low byte) */
00048 #define NXTSERVO_POS_CHAN2    0x44      /*!< Servo channel 2 position (low byte) */
00049 #define NXTSERVO_POS_CHAN3    0x46      /*!< Servo channel 3 position (low byte) */
00050 #define NXTSERVO_POS_CHAN4    0x48      /*!< Servo channel 4 position (low byte) */
00051 #define NXTSERVO_POS_CHAN5    0x4A      /*!< Servo channel 5 position (low byte) */
00052 #define NXTSERVO_POS_CHAN6    0x4C      /*!< Servo channel 6 position (low byte) */
00053 #define NXTSERVO_POS_CHAN7    0x4E      /*!< Servo channel 7 position (low byte) */
00054 #define NXTSERVO_POS_CHAN8    0x50      /*!< Servo channel 8 position (low byte) */
00055 
00056 // Speed registers, can be read and written
00057 #define NXTSERVO_SPEED_CHAN1  0x52      /*!< Servo channel 1 speed */
00058 #define NXTSERVO_SPEED_CHAN2  0x53      /*!< Servo channel 2 speed */
00059 #define NXTSERVO_SPEED_CHAN3  0x54      /*!< Servo channel 3 speed */
00060 #define NXTSERVO_SPEED_CHAN4  0x55      /*!< Servo channel 4 speed */
00061 #define NXTSERVO_SPEED_CHAN5  0x56      /*!< Servo channel 5 speed */
00062 #define NXTSERVO_SPEED_CHAN6  0x57      /*!< Servo channel 6 speed */
00063 #define NXTSERVO_SPEED_CHAN7  0x58      /*!< Servo channel 7 speed */
00064 #define NXTSERVO_SPEED_CHAN8  0x59      /*!< Servo channel 8 speed */
00065 
00066 // Quick position register, can only be written
00067 #define NXTSERVO_QPOS_CHAN1   0x42      /*!< Servo channel 1 quick position */
00068 #define NXTSERVO_QPOS_CHAN2   0x44      /*!< Servo channel 2 quick position */
00069 #define NXTSERVO_QPOS_CHAN3   0x46      /*!< Servo channel 3 quick position */
00070 #define NXTSERVO_QPOS_CHAN4   0x48      /*!< Servo channel 4 quick position */
00071 #define NXTSERVO_QPOS_CHAN5   0x4A      /*!< Servo channel 5 quick position */
00072 #define NXTSERVO_QPOS_CHAN6   0x4C      /*!< Servo channel 6 quick position */
00073 #define NXTSERVO_QPOS_CHAN7   0x4E      /*!< Servo channel 7 quick position */
00074 #define NXTSERVO_QPOS_CHAN8   0x50      /*!< Servo channel 8 quick position */
00075 
00076 /*! Some other defines. */
00077 #define NXTSERVO_MIN_POS      500       /*!< Servo minimum pulse width in uS */
00078 #define NXTSERVO_MAX_POS     2500       /*!< Servo maximum pulse width in uS */
00079 #define NXTSERVO_MID_POS     1500       /*!< Servo centered pulse width in uS */
00080 
00081 /*
00082 <function prototypes>
00083 */
00084 bool NXTServoSetSpeed(tSensors link, ubyte servochan, ubyte speed, ubyte address);
00085 bool NXTServoSetPos(tSensors link, ubyte servochan, int position, ubyte speed, ubyte address = NXTSERVO_I2C_ADDR);
00086 bool NXTServoQSetPos(tSensors link, ubyte servochan, ubyte position, byte speed, ubyte address = NXTSERVO_I2C_ADDR);
00087 int NXTServoReadVoltage(tSensors link, ubyte address = NXTSERVO_I2C_ADDR);
00088 
00089 tByteArray NXTSERVO_I2CRequest;         /*!< Array to hold I2C command data */
00090 tByteArray NXTSERVO_I2CReply;           /*!< Array to hold I2C reply data */
00091 
00092 
00093 /**
00094  * Set the speed register for the specified servo.  This is the amount to increase
00095  * the current position by every 24ms when the servo position is changed.
00096  *
00097  * Note: this is an internal function and should not be called directly.
00098  * @param link the NXTServo port number
00099  * @param servochan the servo channel to use
00100  * @param speed the amount to increase the position by every 24ms
00101  * @param address the I2C address to use, optional, defaults to 0xB0
00102  * @return true if no error occured, false if it did
00103  */
00104 bool NXTServoSetSpeed(tSensors link, ubyte servochan, ubyte speed, ubyte address) {
00105   memset(NXTSERVO_I2CRequest, 0, sizeof(tByteArray));
00106   NXTSERVO_I2CRequest[0] = 3;
00107   NXTSERVO_I2CRequest[1] = address;
00108   NXTSERVO_I2CRequest[2] = NXTSERVO_SPEED_CHAN1 + (servochan - 1);
00109   NXTSERVO_I2CRequest[3] = speed;
00110 
00111   return writeI2C(link, NXTSERVO_I2CRequest);
00112 }
00113 
00114 
00115 /**
00116  * Tell the servo to move to the specified position using the specified speed.
00117  * @param link the NXTServo port number
00118  * @param servochan the servo channel to use
00119  * @param position the position to move the servo to
00120  * @param speed the amount to increase the position by every 24ms
00121  * @param address the I2C address to use, optional, defaults to 0xB0
00122  * @return true if no error occured, false if it did
00123  */
00124 bool NXTServoSetPos(tSensors link, ubyte servochan, int position, ubyte speed, ubyte address) {
00125   memset(NXTSERVO_I2CRequest, 0, sizeof(tByteArray));
00126   if (!NXTServoSetSpeed(link, servochan, speed, address))
00127     return false;
00128 
00129   position = clip(position, 500, 2500);
00130 
00131   // set the position register and tell NXTServo to move the servo
00132   memset(NXTSERVO_I2CRequest, 0, sizeof(tByteArray));
00133   NXTSERVO_I2CRequest[0] = 4;
00134   NXTSERVO_I2CRequest[1] = address;
00135   NXTSERVO_I2CRequest[2] = NXTSERVO_POS_CHAN1 + ((servochan - 1) * 2);
00136   NXTSERVO_I2CRequest[3] = position & 0x00FF;
00137   NXTSERVO_I2CRequest[4] = (position >> 8) & 0x00FF;
00138 
00139   return writeI2C(link, NXTSERVO_I2CRequest);
00140 }
00141 
00142 
00143 
00144 /**
00145  * Tell the servo to move to the specified position using the specified speed.
00146  * This uses the Quick Position register which is less accurate.  It only accepts
00147  * values from 50 to 250
00148  * @param link the NXTServo port number
00149  * @param servochan the servo channel to use
00150  * @param position the position to move the servo to
00151  * @param speed the amount to increase the position by every 24ms
00152  * @param address the I2C address to use, optional, defaults to 0xB0
00153  * @return true if no error occured, false if it did
00154  */
00155 bool NXTServoQSetPos(tSensors link, ubyte servochan, ubyte position, byte speed, ubyte address) {
00156   memset(NXTSERVO_I2CRequest, 0, sizeof(tByteArray));
00157 
00158   position = clip(position, 50, 250);
00159 
00160   if (!NXTServoSetSpeed(link, servochan, speed, address))
00161     return false;
00162 
00163   // set the position register and tell NXTServo to move the servo
00164 
00165   NXTSERVO_I2CRequest[0] = 3;
00166   NXTSERVO_I2CRequest[1] = address;
00167   NXTSERVO_I2CRequest[2] = NXTSERVO_QPOS_CHAN1 + (servochan - 1);
00168   NXTSERVO_I2CRequest[3] = position;
00169 
00170   return writeI2C(link, NXTSERVO_I2CRequest);
00171 }
00172 
00173 
00174 /**
00175  * Get the voltage level of the battery pack.
00176  * @param link the NXTServo port number
00177  * @param address the I2C address to use, optional, defaults to 0xB0
00178  * @return the voltage of the battery pack or -1 if an error occurred
00179  */
00180 int NXTServoReadVoltage(tSensors link, ubyte address) {
00181   long mvs = 0;
00182 
00183   memset(NXTSERVO_I2CRequest, 0, sizeof(tByteArray));
00184 
00185   NXTSERVO_I2CRequest[0] = 2;                 // Message size
00186   NXTSERVO_I2CRequest[1] = address;           // I2C Address
00187   NXTSERVO_I2CRequest[2] = NXTSERVO_CMD;      // Start red sensor value
00188 
00189   if (!writeI2C(link, NXTSERVO_I2CRequest, NXTSERVO_I2CReply, 1))
00190     return -1;
00191 
00192   mvs = ((long)NXTSERVO_I2CReply[0] * 3886) / 100;
00193 
00194   return mvs;
00195 }
00196 
00197 #endif // __NXTSERVO_H__
00198 /*
00199  * $Id: mindensors-servo.h 133 2013-03-10 15:15:38Z xander $
00200  */
00201 /* @} */
00202 /* @} */