00001 #include "BioloidServo.h"
00002
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
00043
00044 BoardController::enableSerialBioloidInterface();
00045
00046
00047 txPacket.id = this->servoId;
00048 txPacket.paramLen = 2;
00049 txPacket.instruction = 0x03;
00050 txPacket.param[0] = 0x10;
00051 txPacket.param[1] = 0x00;
00052 TxPacket(&txPacket, 100);
00053
00054
00055 txPacket.id = this->servoId;
00056 txPacket.paramLen = 2;
00057 txPacket.instruction = 0x03;
00058 txPacket.param[0] = 0x18;
00059 txPacket.param[1] = 0x01;
00060 TxPacket(&txPacket, 0);
00061
00062 BoardController::disableSerialInterfaces();
00063 }
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
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
00103
00104 desiredPosition = (_scal-1)-_pos;
00105
00106
00107
00108 posValue = minPos + (((maxPos-minPos) / (unsigned int)(_scal-1)) * (unsigned int)desiredPosition);
00109
00110
00111 txPacket.id = this->servoId;
00112 txPacket.paramLen = 5;
00113 txPacket.instruction = 0x03;
00114 txPacket.param[0] = 0x1e;
00115 txPacket.param[1] = lowbyte(posValue);
00116 txPacket.param[2] = highbyte(posValue);
00117 txPacket.param[3] = lowbyte(speed);
00118 txPacket.param[4] = highbyte(speed);
00119
00120 TxPacket(&txPacket, 10);
00121
00122
00123 txPacket.id = this->servoId;
00124 txPacket.paramLen = 2;
00125 txPacket.instruction = 0x03;
00126 txPacket.param[0] = 0x18;
00127 txPacket.param[1] = 0x01;
00128 TxPacket(&txPacket, 10);
00129
00130 BoardController::disableSerialInterfaces();
00131 }
00132
00133
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
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;
00152 txbuff[4] = bPacket->instruction;
00153
00154
00155 bPacketLength = bPacket->paramLen+4+2;
00156
00157
00158 for(bCount = 0; bCount < bPacket->paramLen; bCount++)
00159 txbuff[bCount+5] = bPacket->param[bCount];
00160
00161
00162 bCheckSum = 0;
00163 for(bCount = 2; bCount < bPacketLength-1; bCount++)
00164 bCheckSum += txbuff[bCount];
00165
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
00176
00177
00178
00179 PORTC |= (1<<EN_RXD_BIOLOID);
00180
00181 BoardController::msDelay(milliDelay);
00182 }
00183
00184
00185 unsigned char BioloidServo::RxPacket(BioloidPacket* bPacket)
00186 {
00187 unsigned char i;
00188 unsigned char dummy;
00189
00190
00191
00192 PORTC &= ~(1<<EN_TXD_BIOLOID);
00193 PORTC |= (1<<EN_RXD_BIOLOID);
00194
00195
00196
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
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
00215 for (i=0; i<bPacket->paramLen; i++) {
00216
00217 if (!RxByte(&bPacket->param[i])) return 4+i;
00218 }
00219 }
00220
00221
00222 bool BioloidServo::RxByte(unsigned char* data) {
00223
00224
00225
00226 int errorLoop = 0;
00227
00228
00229
00230
00231 while (!(UCSRA & (1<<RXC)) && errorLoop <1000) {
00232 errorLoop++;
00233
00234 }
00235
00236
00237 if (errorLoop == 1000) {
00238
00239 *data = 0xff;
00240 return false;
00241 } else {
00242 *data = UDR;
00243 return true;
00244 }
00245 }
00246
00247
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
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287 void BioloidServo::scanBioloidBus() {
00288
00289 unsigned char id;
00290 unsigned char bytesReceived = 0x00;
00291
00292
00293
00294 BioloidPacket txPacket;
00295
00296
00297 for (id=2; id<4; id++) {
00298
00299 bytesReceived = 0x00;
00300
00301
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
00313 while (!(UCSRA & (1<<UDRE)));
00314 UDR = bytesReceived+0x30;
00315 while (!(UCSRA & (1<<TXC)));
00316 }
00317 }
00318
00319