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 
00068 void BioloidServo::setPosition(unsigned char _pos, unsigned char _scal) {
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 }
00108 
00109 /* This functions transmit a bioloid-packet */
00110 void BioloidServo::TxPacket(BioloidPacket* bPacket, unsigned int milliDelay) 
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 }
00159 
00160 /* This functions receives a bioloid-packet */
00161 unsigned char BioloidServo::RxPacket(BioloidPacket* bPacket) 
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 }
00196 
00197 /* function receives a byte from the bioloid bus */
00198 bool BioloidServo::RxByte(unsigned char* data) {
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 }
00222 
00223 // returns 1 if servo with ID=_id responds to the ping commando
00224 bool BioloidServo::pingServo(unsigned char _id) {
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 }
00245 
00246 /*
00247 void BioloidServo::setBroadcastReturnDelayTime() {
00248 
00249      BioloidPacket txPacket;
00250      BioloidPacket rxPacket;
00251 
00252      unsigned char bytesReceived = 0x00;
00253 
00254      txPacket.id = _id;
00255      txPacket.paramLen = 0; 
00256      txPacket.instruction = 0x03; // Write
00257 
00258      TxPacket(&txPacket, 0);
00259 }
00260 */
00261 
00262 /* sends a ping packet to all id's on the bioloid bus */
00263 void BioloidServo::scanBioloidBus() {
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 }
Generated on Fri Oct 8 17:10:07 2010 for Crawling Robot Microcontroller Software by  doxygen 1.6.3