#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 |
Definition at line 26 of file BioloidServo.h.
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 }
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 }
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 }
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 }
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 }
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 }
unsigned int BioloidServo::maxPos [private] |
unsigned int BioloidServo::minPos [private] |
unsigned char BioloidServo::servoId [private] |