BioloidServo.c
Go to the documentation of this file.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
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
00079
00080 desiredPosition = (_scal-1)-_pos;
00081
00082
00083
00084 posValue = minPos + (((maxPos-minPos) / (unsigned int)(_scal-1)) * (unsigned int)desiredPosition);
00085
00086
00087 txPacket.id = this->servoId;
00088 txPacket.paramLen = 5;
00089 txPacket.instruction = 0x03;
00090 txPacket.param[0] = 0x1e;
00091 txPacket.param[1] = lowbyte(posValue);
00092 txPacket.param[2] = highbyte(posValue);
00093 txPacket.param[3] = lowbyte(speed);
00094 txPacket.param[4] = highbyte(speed);
00095
00096 TxPacket(&txPacket, 10);
00097
00098
00099 txPacket.id = this->servoId;
00100 txPacket.paramLen = 2;
00101 txPacket.instruction = 0x03;
00102 txPacket.param[0] = 0x18;
00103 txPacket.param[1] = 0x01;
00104 TxPacket(&txPacket, 10);
00105
00106 BoardController::disableSerialInterfaces();
00107 }
00108
00109
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
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;
00128 txbuff[4] = bPacket->instruction;
00129
00130
00131 bPacketLength = bPacket->paramLen+4+2;
00132
00133
00134 for(bCount = 0; bCount < bPacket->paramLen; bCount++)
00135 txbuff[bCount+5] = bPacket->param[bCount];
00136
00137
00138 bCheckSum = 0;
00139 for(bCount = 2; bCount < bPacketLength-1; bCount++)
00140 bCheckSum += txbuff[bCount];
00141
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
00152
00153
00154
00155 PORTC |= (1<<EN_RXD_BIOLOID);
00156
00157 BoardController::msDelay(milliDelay);
00158 }
00159
00160
00161 unsigned char BioloidServo::RxPacket(BioloidPacket* bPacket)
00162 {
00163 unsigned char i;
00164 unsigned char dummy;
00165
00166
00167
00168 PORTC &= ~(1<<EN_TXD_BIOLOID);
00169 PORTC |= (1<<EN_RXD_BIOLOID);
00170
00171
00172
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
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
00191 for (i=0; i<bPacket->paramLen; i++) {
00192
00193 if (!RxByte(&bPacket->param[i])) return 4+i;
00194 }
00195 }
00196
00197
00198 bool BioloidServo::RxByte(unsigned char* data) {
00199
00200
00201
00202 int errorLoop = 0;
00203
00204
00205
00206
00207 while (!(UCSRA & (1<<RXC)) && errorLoop <1000) {
00208 errorLoop++;
00209
00210 }
00211
00212
00213 if (errorLoop == 1000) {
00214
00215 *data = 0xff;
00216 return false;
00217 } else {
00218 *data = UDR;
00219 return true;
00220 }
00221 }
00222
00223
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
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 void BioloidServo::scanBioloidBus() {
00264
00265 unsigned char id;
00266 unsigned char bytesReceived = 0x00;
00267
00268
00269
00270 BioloidPacket txPacket;
00271
00272
00273 for (id=2; id<4; id++) {
00274
00275 bytesReceived = 0x00;
00276
00277
00278 BoardController::enableSerialBioloidInterface();
00279 txPacket.id = id;
00280 txPacket.paramLen = 1;
00281 txPacket.instruction = 0x05;
00282 txPacket.param[0] = 254;
00283
00284
00285 bytesReceived = BioloidServo::pingServo(id);
00286 BoardController::disableSerialInterfaces();
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297 while (!(UCSRA & (1<<UDRE)));
00298 UDR = bytesReceived+0x30;
00299 while (!(UCSRA & (1<<TXC)));
00300 }
00301 }