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 224 of file BioloidServo.c.

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

Referenced by scanBioloidBus().

00224                                               {
00225 
00226      BioloidPacket txPacket;
00227      BioloidPacket rxPacket;
00228 
00229      unsigned char bytesReceived = 0x00;
00230 
00231      txPacket.id = _id;
00232      txPacket.paramLen = 0; 
00233      txPacket.instruction = 0x01;
00234 
00235      TxPacket(&txPacket, 0);
00236      TxPacket(&txPacket, 0);
00237 
00238      if (bytesReceived = RxPacket(&rxPacket)) {
00239           return bytesReceived;
00240      } else {
00241           BoardController::blinkLED((1<<LED_WALK)|(1<<LED_EXPLORATION), 10, 30);
00242           return 0x00;
00243      }
00244 }

Here is the call graph for this function:

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

Definition at line 198 of file BioloidServo.c.

Referenced by RxPacket().

00198                                              {
00199 
00200      //BoardController::powerOnLED(LED_POWER, false);
00201      //unsigned char rx;
00202      int errorLoop = 0;
00203 
00204      // wait a maximum of 1 second until
00205      //while (!(UCSRA & (1<<RXC)));
00206      
00207      while (!(UCSRA & (1<<RXC)) && errorLoop <1000) {
00208           errorLoop++;
00209           //BoardController::msDelay(1);
00210      }
00211      
00212 
00213      if (errorLoop == 1000) {
00214           //BoardController::blinkLED((1<<LED_WALK), 10, 30);
00215           *data = 0xff;
00216           return false;
00217      } else {
00218           *data = UDR;
00219           return true;
00220      }
00221 }

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

Definition at line 161 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().

00162 {
00163      unsigned char i;
00164      unsigned char dummy;
00165 
00166 
00167      // enable reception
00168      PORTC  &= ~(1<<EN_TXD_BIOLOID);
00169      PORTC  |= (1<<EN_RXD_BIOLOID);
00170 
00171 
00172      // receive garbage
00173      if (!RxByte(&dummy)) {   
00174           BoardController::blinkLED((1<<LED_WALK), 10, 20);
00175           return 0;
00176      }
00177      if (!RxByte(&dummy)) {   
00178           BoardController::blinkLED((1<<LED_LEARN), 10, 20);
00179           return 1;
00180      } 
00181 
00182      // receive id
00183      if (!RxByte(&bPacket->id)) {  
00184           BoardController::blinkLED((1<<LED_EXPLORATION), 10, 20);
00185           return 2;
00186      } 
00187 
00188      if (!RxByte(&bPacket->paramLen)) return 3;
00189 
00190      // receive data
00191      for (i=0; i<bPacket->paramLen; i++) {
00192           // receive data
00193           if (!RxByte(&bPacket->param[i])) return 4+i;
00194      }
00195 }

Here is the call graph for this function:

void BioloidServo::scanBioloidBus (  )  [static]

Definition at line 263 of file BioloidServo.c.

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

Referenced by Environment::Environment().

00263                                   {
00264 
00265      unsigned char id;
00266      unsigned char bytesReceived  = 0x00;
00267 
00268 
00269      // set delay time 
00270      BioloidPacket txPacket;
00271 
00272 
00273      for (id=2; id<4; id++) {
00274 
00275           bytesReceived = 0x00;
00276 
00277           // set return-delay-time to 508 microseconds
00278           BoardController::enableSerialBioloidInterface();
00279           txPacket.id = id;
00280           txPacket.paramLen = 1; 
00281           txPacket.instruction = 0x05;
00282           txPacket.param[0] = 254;
00283           //TxPacket(&txPacket, 0);
00284 
00285           bytesReceived = BioloidServo::pingServo(id);
00286           BoardController::disableSerialInterfaces();
00287 
00288           /* wait till transmit buffer is ready */
00289 /*        while (!(UCSRA & (1<<UDRE)));
00290           UDR = ' ';
00291           while (!(UCSRA & (1<<TXC)));
00292 
00293           while (!(UCSRA & (1<<UDRE)));
00294           UDR = id+0x30;
00295           while (!(UCSRA & (1<<TXC)));
00296 */
00297           while (!(UCSRA & (1<<UDRE)));
00298           UDR = bytesReceived+0x30;
00299           while (!(UCSRA & (1<<TXC)));
00300      }
00301 }

Here is the call graph for this function:

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

position servo

Definition at line 68 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().

00068                                                                       {
00069 
00070      unsigned int posValue;
00071      unsigned int speed=1023;
00072      unsigned int desiredPosition=0;
00073 
00074      BioloidPacket txPacket;
00075 
00076      BoardController::enableSerialBioloidInterface();
00077 
00078      // invert axis position
00079      //if (this->servoId == 0x11)
00080           desiredPosition = (_scal-1)-_pos;
00081      //else
00082           //desiredPosition = _pos;
00083 
00084      posValue = minPos + (((maxPos-minPos) / (unsigned int)(_scal-1)) * (unsigned int)desiredPosition);
00085      //posValue = 0;
00086 
00087      txPacket.id = this->servoId;
00088      txPacket.paramLen = 5; // 1x address (parameter0), 2x position(parameter1-2)
00089      txPacket.instruction = 0x03; // WRITE_DATA
00090      txPacket.param[0] = 0x1e;     // set Goal Position
00091      txPacket.param[1] = lowbyte(posValue);  // 1) low byte
00092      txPacket.param[2] = highbyte(posValue); // 2) high byte
00093      txPacket.param[3] = lowbyte(speed);//lowbyte(posValue);  // 1) low byte
00094      txPacket.param[4] = highbyte(speed); //highbyte(posValue); // 2) high byte
00095 
00096      TxPacket(&txPacket, 10);
00097 
00098      // enable torque
00099      txPacket.id = this->servoId; //this->servoId;
00100      txPacket.paramLen = 2;        // 1x address (parameter0), 2x position(parameter1-2)
00101      txPacket.instruction = 0x03;  // WRITE_DATA
00102      txPacket.param[0] = 0x18;     // set torque 
00103      txPacket.param[1] = 0x01;          // enable
00104      TxPacket(&txPacket, 10);
00105 
00106      BoardController::disableSerialInterfaces();
00107 }

Here is the call graph for this function:

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

Definition at line 110 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(), and setPosition().

00111 {
00112      unsigned char bCount, bPacketLength, bCheckSum;
00113      unsigned char i;
00114      unsigned char dummy;
00115      static unsigned char txbuff[ BIOLOID_MAX_PACKETLEN ];
00116 
00117 
00118      // send packet - expects serial interface to be proper configured
00119      PORTC  &= ~((1<<EN_RXD_BIOLOID) | (1<<EN_TXD_BIOLOID) | (1<<EN_AIR_V24));
00120      PORTC  |= ((1<<EN_TXD_BIOLOID) | (1<<EN_RXD_BIOLOID));
00121      BoardController::msDelay(1);
00122      
00123 
00124      txbuff[0] = 0xff;
00125      txbuff[1] = 0xff;
00126      txbuff[2] = bPacket->id; 
00127      txbuff[3] = bPacket->paramLen+2; // Length := instruction + paramter's + checksum
00128      txbuff[4] = bPacket->instruction;
00129 
00130      // compute packet length
00131      bPacketLength = bPacket->paramLen+4+2;
00132  
00133      // append parameters
00134      for(bCount = 0; bCount < bPacket->paramLen; bCount++)
00135           txbuff[bCount+5] = bPacket->param[bCount];
00136  
00137      // compute checksum 
00138      bCheckSum = 0;
00139      for(bCount = 2; bCount < bPacketLength-1; bCount++) // except 0xff, checksum
00140           bCheckSum += txbuff[bCount];
00141      // Append checksum with bit inversion
00142      txbuff[bCount] = ~bCheckSum;
00143 
00144 
00145      for(bCount = 0; bCount < bPacketLength; bCount++)
00146      {
00147           while (!(UCSRA & (1<<UDRE)));
00148           UDR = txbuff[bCount];
00149           while(!(UCSRA & (1<<TXC)));
00150      }
00151      //BoardController::msDelay(1);
00152 
00153      // enable bioloid reception
00154      //PORTC  &= ~((1<<EN_RXD_BIOLOID) | (1<<EN_TXD_BIOLOID) | (1<<EN_AIR_V24));
00155      PORTC  |= (1<<EN_RXD_BIOLOID);
00156      
00157      BoardController::msDelay(milliDelay);
00158 }

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 Fri Oct 8 17:10:09 2010 for Crawling Robot Microcontroller Software by  doxygen 1.6.3