BioloidServo Class Reference

#include <BioloidServo.h>


Public Member Functions

 BioloidServo (unsigned char _id, unsigned int _minPos, unsigned int _maxPos)
void setPosition (unsigned char _pos, unsigned char _scal)

Static Public Member Functions

static bool pingServo (unsigned char _id)
static bool RxByte (unsigned char *data)
static unsigned char RxPacket (BioloidPacket *bPacket)
static void scanBioloidBus ()
static void TxPacket (BioloidPacket *bPacket, unsigned int milliDelay)

Private Attributes

unsigned int maxPos
unsigned int minPos
unsigned char servoId


Detailed Description

Definition at line 26 of file BioloidServo.h.


Constructor & Destructor Documentation

BioloidServo::BioloidServo ( unsigned char  _id,
unsigned int  _minPos,
unsigned int  _maxPos 
)

set the Id, minimum position and maximum position of servo (Id)

Definition at line 33 of file BioloidServo.c.

References BoardController::disableSerialInterfaces(), BoardController::enableSerialBioloidInterface(), BioloidPacket::id, BioloidPacket::instruction, maxPos, minPos, BioloidPacket::param, BioloidPacket::paramLen, servoId, and TxPacket().

00034 {
00035 
00036      BioloidPacket txPacket;
00037 
00038      this->minPos = _minPos;
00039      this->maxPos = _maxPos;
00040      this->servoId = _id;
00041 
00042      //this->servoId = this->getServoId(_id);
00043 
00044      BoardController::enableSerialBioloidInterface();
00045 
00046      // configure AX-12: send no return messages 
00047      txPacket.id = this->servoId;  // Broadcast
00048      txPacket.paramLen = 2;        
00049      txPacket.instruction = 0x03;  // WRITE
00050      txPacket.param[0] = 0x10;     // set status return level
00051      txPacket.param[1] = 0x00;          // send no response
00052      TxPacket(&txPacket, 100);
00053 
00054      // enable torque
00055      txPacket.id = this->servoId; //this->servoId;
00056      txPacket.paramLen = 2;        // 1x address (parameter0), 2x position(parameter1-2)
00057      txPacket.instruction = 0x03;  // WRITE_DATA
00058      txPacket.param[0] = 0x18;     // set torque 
00059      txPacket.param[1] = 0x01;          // enable
00060      TxPacket(&txPacket, 0);
00061 
00062      BoardController::disableSerialInterfaces();
00063 }

Here is the call graph for this function:


Member Function Documentation

bool BioloidServo::pingServo ( unsigned char  _id  )  [static]

Definition at line 248 of file BioloidServo.c.

References BoardController::blinkLED(), BioloidPacket::id, BioloidPacket::instruction, LED_EXPLORATION, LED_WALK, BioloidPacket::paramLen, RxPacket(), and TxPacket().

Referenced by scanBioloidBus().

00248                                               {
00249 
00250      BioloidPacket txPacket;
00251      BioloidPacket rxPacket;
00252 
00253      unsigned char bytesReceived = 0x00;
00254 
00255      txPacket.id = _id;
00256      txPacket.paramLen = 0; 
00257      txPacket.instruction = 0x01;
00258 
00259      TxPacket(&txPacket, 0);
00260      TxPacket(&txPacket, 0);
00261 
00262      if (bytesReceived = RxPacket(&rxPacket)) {
00263           return bytesReceived;
00264      } else {
00265           BoardController::blinkLED((1<<LED_WALK)|(1<<LED_EXPLORATION), 10, 30);
00266           return 0x00;
00267      }
00268 }

Here is the call graph for this function:

bool BioloidServo::RxByte ( unsigned char *  data  )  [static]

Definition at line 222 of file BioloidServo.c.

Referenced by RxPacket().

00222                                              {
00223 
00224      //BoardController::powerOnLED(LED_POWER, false);
00225      //unsigned char rx;
00226      int errorLoop = 0;
00227 
00228      // wait a maximum of 1 second until
00229      //while (!(UCSRA & (1<<RXC)));
00230      
00231      while (!(UCSRA & (1<<RXC)) && errorLoop <1000) {
00232           errorLoop++;
00233           //BoardController::msDelay(1);
00234      }
00235      
00236 
00237      if (errorLoop == 1000) {
00238           //BoardController::blinkLED((1<<LED_WALK), 10, 30);
00239           *data = 0xff;
00240           return false;
00241      } else {
00242           *data = UDR;
00243           return true;
00244      }
00245 }

unsigned char BioloidServo::RxPacket ( BioloidPacket bPacket  )  [static]

Definition at line 185 of file BioloidServo.c.

References BoardController::blinkLED(), EN_RXD_BIOLOID, EN_TXD_BIOLOID, BioloidPacket::id, LED_EXPLORATION, LED_LEARN, LED_WALK, BioloidPacket::param, BioloidPacket::paramLen, and RxByte().

Referenced by pingServo().

00186 {
00187      unsigned char i;
00188      unsigned char dummy;
00189 
00190 
00191      // enable reception
00192      PORTC  &= ~(1<<EN_TXD_BIOLOID);
00193      PORTC  |= (1<<EN_RXD_BIOLOID);
00194 
00195 
00196      // receive garbage
00197      if (!RxByte(&dummy)) {   
00198           BoardController::blinkLED((1<<LED_WALK), 10, 20);
00199           return 0;
00200      }
00201      if (!RxByte(&dummy)) {   
00202           BoardController::blinkLED((1<<LED_LEARN), 10, 20);
00203           return 1;
00204      } 
00205 
00206      // receive id
00207      if (!RxByte(&bPacket->id)) {  
00208           BoardController::blinkLED((1<<LED_EXPLORATION), 10, 20);
00209           return 2;
00210      } 
00211 
00212      if (!RxByte(&bPacket->paramLen)) return 3;
00213 
00214      // receive data
00215      for (i=0; i<bPacket->paramLen; i++) {
00216           // receive data
00217           if (!RxByte(&bPacket->param[i])) return 4+i;
00218      }
00219 }

Here is the call graph for this function:

void BioloidServo::scanBioloidBus (  )  [static]

Definition at line 287 of file BioloidServo.c.

References BoardController::disableSerialInterfaces(), BoardController::enableSerialBioloidInterface(), BioloidPacket::id, BioloidPacket::instruction, BioloidPacket::param, BioloidPacket::paramLen, pingServo(), and TxPacket().

Referenced by Environment::Environment().

00287                                   {
00288 
00289      unsigned char id;
00290      unsigned char bytesReceived  = 0x00;
00291 
00292 
00293      // set delay time 
00294      BioloidPacket txPacket;
00295 
00296 
00297      for (id=2; id<4; id++) {
00298 
00299           bytesReceived = 0x00;
00300 
00301           // set return-delay-time to 508 microseconds
00302           BoardController::enableSerialBioloidInterface();
00303           txPacket.id = id;
00304           txPacket.paramLen = 1; 
00305           txPacket.instruction = 0x05;
00306           txPacket.param[0] = 254;
00307           TxPacket(&txPacket, 0);
00308 
00309           bytesReceived = BioloidServo::pingServo(id);
00310           BoardController::disableSerialInterfaces();
00311 
00312           /* wait till transmit buffer is ready */
00313           while (!(UCSRA & (1<<UDRE)));
00314           UDR = bytesReceived+0x30;
00315           while (!(UCSRA & (1<<TXC)));
00316      }
00317 }

Here is the call graph for this function:

void BioloidServo::setPosition ( unsigned char  _pos,
unsigned char  _scal 
)

position servo

Definition at line 92 of file BioloidServo.c.

References BoardController::disableSerialInterfaces(), BoardController::enableSerialBioloidInterface(), highbyte, BioloidPacket::id, BioloidPacket::instruction, lowbyte, maxPos, minPos, BioloidPacket::param, BioloidPacket::paramLen, servoId, and TxPacket().

Referenced by Environment::beamRobotToState(), Environment::doAction(), and Environment::Environment().

00092                                                                       {
00093 
00094      unsigned int posValue;
00095      unsigned int speed=1023;
00096      unsigned int desiredPosition=0;
00097 
00098      BioloidPacket txPacket;
00099 
00100      BoardController::enableSerialBioloidInterface();
00101 
00102      // invert axis position
00103      //if (this->servoId == 0x11)
00104           desiredPosition = (_scal-1)-_pos;
00105      //else
00106           //desiredPosition = _pos;
00107 
00108      posValue = minPos + (((maxPos-minPos) / (unsigned int)(_scal-1)) * (unsigned int)desiredPosition);
00109      //posValue = 0;
00110 
00111      txPacket.id = this->servoId;
00112      txPacket.paramLen = 5; // 1x address (parameter0), 2x position(parameter1-2)
00113      txPacket.instruction = 0x03; // WRITE_DATA
00114      txPacket.param[0] = 0x1e;     // set Goal Position
00115      txPacket.param[1] = lowbyte(posValue);  // 1) low byte
00116      txPacket.param[2] = highbyte(posValue); // 2) high byte
00117      txPacket.param[3] = lowbyte(speed);//lowbyte(posValue);  // 1) low byte
00118      txPacket.param[4] = highbyte(speed); //highbyte(posValue); // 2) high byte
00119 
00120      TxPacket(&txPacket, 10);
00121 
00122      // enable torque
00123      txPacket.id = this->servoId; //this->servoId;
00124      txPacket.paramLen = 2;        // 1x address (parameter0), 2x position(parameter1-2)
00125      txPacket.instruction = 0x03;  // WRITE_DATA
00126      txPacket.param[0] = 0x18;     // set torque 
00127      txPacket.param[1] = 0x01;          // enable
00128      TxPacket(&txPacket, 10);
00129 
00130      BoardController::disableSerialInterfaces();
00131 }

Here is the call graph for this function:

void BioloidServo::TxPacket ( BioloidPacket bPacket,
unsigned int  milliDelay 
) [static]

Definition at line 134 of file BioloidServo.c.

References BIOLOID_MAX_PACKETLEN, EN_AIR_V24, EN_RXD_BIOLOID, EN_TXD_BIOLOID, BioloidPacket::id, BioloidPacket::instruction, BoardController::msDelay(), BioloidPacket::param, and BioloidPacket::paramLen.

Referenced by BioloidServo(), pingServo(), scanBioloidBus(), and setPosition().

00135 {
00136      unsigned char bCount, bPacketLength, bCheckSum;
00137      unsigned char i;
00138      unsigned char dummy;
00139      static unsigned char txbuff[ BIOLOID_MAX_PACKETLEN ];
00140 
00141 
00142      // send packet - expects serial interface to be proper configured
00143      PORTC  &= ~((1<<EN_RXD_BIOLOID) | (1<<EN_TXD_BIOLOID) | (1<<EN_AIR_V24));
00144      PORTC  |= ((1<<EN_TXD_BIOLOID) | (1<<EN_RXD_BIOLOID));
00145      BoardController::msDelay(1);
00146      
00147 
00148      txbuff[0] = 0xff;
00149      txbuff[1] = 0xff;
00150      txbuff[2] = bPacket->id; 
00151      txbuff[3] = bPacket->paramLen+2; // Length := instruction + paramter's + checksum
00152      txbuff[4] = bPacket->instruction;
00153 
00154      // compute packet length
00155      bPacketLength = bPacket->paramLen+4+2;
00156  
00157      // append parameters
00158      for(bCount = 0; bCount < bPacket->paramLen; bCount++)
00159           txbuff[bCount+5] = bPacket->param[bCount];
00160  
00161      // compute checksum 
00162      bCheckSum = 0;
00163      for(bCount = 2; bCount < bPacketLength-1; bCount++) // except 0xff, checksum
00164           bCheckSum += txbuff[bCount];
00165      // Append checksum with bit inversion
00166      txbuff[bCount] = ~bCheckSum;
00167 
00168 
00169      for(bCount = 0; bCount < bPacketLength; bCount++)
00170      {
00171           while (!(UCSRA & (1<<UDRE)));
00172           UDR = txbuff[bCount];
00173           while(!(UCSRA & (1<<TXC)));
00174      }
00175      //BoardController::msDelay(1);
00176 
00177      // enable bioloid reception
00178      //PORTC  &= ~((1<<EN_RXD_BIOLOID) | (1<<EN_TXD_BIOLOID) | (1<<EN_AIR_V24));
00179      PORTC  |= (1<<EN_RXD_BIOLOID);
00180      
00181      BoardController::msDelay(milliDelay);
00182 }

Here is the call graph for this function:


Field Documentation

unsigned int BioloidServo::maxPos [private]

Definition at line 41 of file BioloidServo.h.

Referenced by BioloidServo(), and setPosition().

unsigned int BioloidServo::minPos [private]

Definition at line 41 of file BioloidServo.h.

Referenced by BioloidServo(), and setPosition().

unsigned char BioloidServo::servoId [private]

Definition at line 42 of file BioloidServo.h.

Referenced by BioloidServo(), and setPosition().


The documentation for this class was generated from the following files:

Generated on Wed Nov 25 11:27:44 2009 for Crawling Robot Microcontroller Software by  doxygen 1.5.5