#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 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 }
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 }
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 }
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 }
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 }
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().