BioloidServo.c

Go to the documentation of this file.
00001 #include "BioloidServo.h"
00002 
00006 /*BioloidServo::BioloidServo(unsigned char _id, unsigned int _minPos, unsigned int _maxPos)
00007 {
00008      this->minPos = _minPos;
00009      this->maxPos = _maxPos;
00010      this->servoId = _id;
00011 
00012      BoardController::enableSerialBioloidInterface();
00013 
00014      // configure AX-12: send no return messages 
00015      this->txPacket.id = this->servoId;      // Broadcast
00016      this->txPacket.paramLen = 2;       
00017      this->txPacket.instruction = 0x03;      // WRITE
00018      this->txPacket.param[0] = 0x10;    // set status return level
00019      this->txPacket.param[1] = 0x00;         // send no response
00020      this->TxPacket(&this->txPacket, 100);
00021 
00022      // enable torque
00023      this->txPacket.id = this->servoId; //this->servoId;
00024      this->txPacket.paramLen = 2;       // 1x address (parameter0), 2x position(parameter1-2)
00025      this->txPacket.instruction = 0x03;      // WRITE_DATA
00026      this->txPacket.param[0] = 0x18;    // set torque 
00027      this->txPacket.param[1] = 0x01;         // enable
00028      this->TxPacket(&this->txPacket, 0);
00029 
00030      BoardController::disableSerialInterfaces();
00031 }*/
00032 
00033 BioloidServo::BioloidServo(unsigned char _id, unsigned int _minPos, unsigned int _maxPos)
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 }
00064 
00065 /*
00066 unsigned char BioloidServo::getServoId(unsigned char _id) {
00067 
00068      BoardController::enableSerialBioloidInterface();
00069      BioloidPacket txPacket;
00070 
00071      // configure AX-12: send no return messages 
00072      txPacket.id = 0xff;      // Broadcast
00073      txPacket.paramLen = 2;        
00074      txPacket.instruction = 0x03;  // WRITE
00075      txPacket.param[0] = 0x10;     // set status return level
00076      txPacket.param[1] = 0x00;          // send no response
00077      TxPacket(&txPacket, 100);
00078 
00079 
00080 
00081      // By definition, the servo with the highest id is SERVO_X
00082 
00083      // By definition, the servo with the lowest id is SERVO_Y
00084 
00085      BoardController::disableSerialInterfaces();
00086 }
00087 */
00088 
00089 
00090 
00092 void BioloidServo::setPosition(unsigned char _pos, unsigned char _scal) {
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 }
00132 
00133 /* This functions transmit a bioloid-packet */
00134 void BioloidServo::TxPacket(BioloidPacket* bPacket, unsigned int milliDelay) 
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 }
00183 
00184 /* This functions receives a bioloid-packet */
00185 unsigned char BioloidServo::RxPacket(BioloidPacket* bPacket) 
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 }
00220 
00221 /* function receives a byte from the bioloid bus */
00222 bool BioloidServo::RxByte(unsigned char* data) {
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 }
00246 
00247 // returns 1 if servo with ID=_id responds to the ping commando
00248 bool BioloidServo::pingServo(unsigned char _id) {
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 }
00269 
00270 /*
00271 void BioloidServo::setBroadcastReturnDelayTime() {
00272 
00273      BioloidPacket txPacket;
00274      BioloidPacket rxPacket;
00275 
00276      unsigned char bytesReceived = 0x00;
00277 
00278      txPacket.id = _id;
00279      txPacket.paramLen = 0; 
00280      txPacket.instruction = 0x03; // Write
00281 
00282      TxPacket(&txPacket, 0);
00283 }
00284 */
00285 
00286 /* sends a ping packet to all id's on the bioloid bus */
00287 void BioloidServo::scanBioloidBus() {
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 }
00318 
00319 

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