Dynamixel.cpp

00001 #include <mbed.h>
00002 #include <Dynamixel.h>
00003 
00004 uint8_t Instruction_Packet[15];     // Array to hold instruction packet data
00005 uint8_t Status_Packet[8];           // Array to hold returned status packet data
00006 uint8_t Status_Return_Value = READ; // Status package return states (NON, READ, ALL)
00007 
00008 Timer timer;
00009 //-------------------------------------------------------------------------------------------------------------------------------
00010 //            Private Methods
00011 //-------------------------------------------------------------------------------------------------------------------------------
00012 void Dynamixel::writePacket(void)
00013 {
00014     if (dynamixelSerial->readable())
00015         while (dynamixelSerial->readable())
00016             dynamixelSerial->getc();
00017     dynamixelDi->write(1);
00018     dynamixelSerial->putc(HEADER);
00019     dynamixelSerial->putc(HEADER);
00020     dynamixelSerial->putc(Instruction_Packet[0]);
00021     dynamixelSerial->putc(Instruction_Packet[1]);
00022     for (uint8_t i = 0; i < Instruction_Packet[1]; i++)
00023     {
00024         dynamixelSerial->putc(Instruction_Packet[2 + i]);
00025     }
00026     wait_us((Instruction_Packet[1] + 4) * 5);
00027     dynamixelDi->write(0);
00028 }
00029 
00030 unsigned int Dynamixel::readPacket(void)
00031 {
00032     // wait_us(250);
00033     // uint8_t Counter = 0;
00034     // uint8_t InBuff[20];
00035     // uint8_t i = 0, j = 0, RxState = 0;
00036     // Status_Packet[0] = 0;
00037     // Status_Packet[1] = 0;
00038     // Status_Packet[2] = 0;
00039     // Status_Packet[3] = 0;
00040     // timer.start();
00041     // int bytes = 2;
00042     // int timeout = 0;
00043     // int plen = 0;
00044     // // while (timer.read_ms() < 3000)
00045     // // {
00046     // //     if (dynamixelSerial->readable())
00047     // //     {
00048     // //         InBuff[plen] = dynamixelSerial->getc();
00049     // //         plen++;
00050     // //     }
00051     // // }
00052     // while ((timeout < ((6 + bytes) * 10000)) && (plen < (6 + bytes)))
00053     // {
00054 
00055     //     if (dynamixelSerial->readable())
00056     //     {
00057     //         InBuff[plen] = dynamixelSerial->getc();
00058     //         plen++;
00059     //         timeout = 0;
00060     //     }
00061 
00062     //     // wait for the bit period
00063     //     wait_us(1);
00064     //     timeout++;
00065     // }
00066     // timer.stop();
00067     // for (int i = 0; i < plen; i++)
00068     // {
00069     //     Status_Packet[i] = InBuff[i];
00070     //     printf("0x%X,", Status_Packet[i]);
00071     // }
00072     // printf("\r\n");
00073     return 0x01;
00074 }
00075 //-------------------------------------------------------------------------------------------------------------------------------
00076 // Public Methods
00077 //-------------------------------------------------------------------------------------------------------------------------------
00078 Dynamixel::Dynamixel(PinName tx, PinName rx, int baud, PinName di)
00079 {
00080     dynamixelSerial = new Serial(tx, rx);
00081     dynamixelSerial->baud(baud);
00082     dynamixelDi = new DigitalOut(di);
00083     dynamixelDi->write(0);
00084 }
00085 
00086 Dynamixel::~Dynamixel(void)
00087 {
00088     if (dynamixelSerial != NULL)
00089         delete dynamixelSerial;
00090 }
00091 
00092 void Dynamixel::setMode(uint8_t ID, uint8_t Mode, uint16_t CW_limit, uint16_t CCW_limit)
00093 {
00094     uint8_t L_CW = (uint8_t)(CW_limit & 0xFF);
00095     uint8_t H_CW = (uint8_t)(CW_limit >> 8);
00096     uint8_t L_CCW = (uint8_t)(CCW_limit & 0xFF);
00097     uint8_t H_CCW = (uint8_t)(CCW_limit >> 8);
00098     Instruction_Packet[0] = ID;
00099     Instruction_Packet[1] = SET_MODE_LENGTH;
00100     Instruction_Packet[2] = COMMAND_WRITE_DATA;
00101     Instruction_Packet[3] = EEPROM_CW_ANGLE_LIMIT_L;
00102     Instruction_Packet[4] = L_CW;
00103     Instruction_Packet[5] = H_CW;
00104     Instruction_Packet[6] = L_CCW;
00105     Instruction_Packet[7] = H_CCW;
00106     Instruction_Packet[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + L_CW + H_CW + L_CCW + H_CCW);
00107     writePacket();
00108 }
00109 
00110 void Dynamixel::setLed(uint8_t ID, bool State)
00111 {
00112     Instruction_Packet[0] = ID;
00113     Instruction_Packet[1] = LED_LENGTH;
00114     Instruction_Packet[2] = COMMAND_WRITE_DATA;
00115     Instruction_Packet[3] = RAM_LED;
00116     Instruction_Packet[4] = State;
00117     Instruction_Packet[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + State);
00118     writePacket();
00119 }
00120 
00121 void Dynamixel::setPosition(uint8_t ID, uint16_t Position, uint16_t Speed = 0)
00122 {
00123     uint8_t L_Position = (uint8_t)(Position & 0xFF);
00124     uint8_t H_Position = (uint8_t)(Position >> 8);
00125     uint8_t L_Speed = (uint8_t)(Speed & 0xFF);
00126     uint8_t H_Speed = (uint8_t)(Speed >> 8);
00127     Instruction_Packet[0] = ID;
00128     Instruction_Packet[1] = SERVO_GOAL_LENGTH;
00129     Instruction_Packet[2] = COMMAND_WRITE_DATA;
00130     Instruction_Packet[3] = RAM_GOAL_POSITION_L;
00131     Instruction_Packet[4] = L_Position;
00132     Instruction_Packet[5] = H_Position;
00133     Instruction_Packet[6] = L_Speed;
00134     Instruction_Packet[7] = H_Speed;
00135     Instruction_Packet[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + L_Position + H_Position + L_Speed + H_Speed);
00136     writePacket();
00137 }
00138 
00139 void Dynamixel::setWheelSpeed(uint8_t ID, bool Direction, uint16_t Speed)
00140 {
00141     uint8_t L_Speed = (uint8_t)(Speed & 0xFF);
00142     uint8_t H_Speed = (uint8_t)(Direction ? (Speed >> 8) : (Speed >> 8) | 0x04);
00143     Instruction_Packet[0] = ID;
00144     Instruction_Packet[1] = WHEEL_LENGTH;
00145     Instruction_Packet[2] = COMMAND_WRITE_DATA;
00146     Instruction_Packet[3] = RAM_GOAL_SPEED_L;
00147     Instruction_Packet[4] = L_Speed;
00148     Instruction_Packet[5] = H_Speed;
00149     Instruction_Packet[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + L_Speed + H_Speed);
00150     writePacket();
00151 }
00152 
00153 void Dynamixel::setWheel3Speed(uint8_t ID[], bool Direction[], uint16_t Speed[])
00154 {
00155     uint8_t L_Speed[3], H_Speed[3];
00156     L_Speed[0] = (uint8_t)(Speed[0] & 0xFF);
00157     H_Speed[0] = (uint8_t)(Direction[0] ? (Speed[0] >> 8) : (Speed[0] >> 8) | 0x04);
00158     L_Speed[1] = (uint8_t)(Speed[1] & 0xFF);
00159     H_Speed[1] = (uint8_t)(Direction[1] ? (Speed[1] >> 8) : (Speed[1] >> 8) | 0x04);
00160     L_Speed[2] = (uint8_t)(Speed[2] & 0xFF);
00161     H_Speed[2] = (uint8_t)(Direction[2] ? (Speed[2] >> 8) : (Speed[2] >> 8) | 0x04);
00162 
00163     Instruction_Packet[0] = 0xFE;
00164     Instruction_Packet[1] = SYNC_LOAD_LENGTH;
00165     Instruction_Packet[2] = COMMAND_SYNC_WRITE;
00166     Instruction_Packet[3] = RAM_GOAL_SPEED_L;
00167     Instruction_Packet[4] = SYNC_DATA_LENGTH;
00168     Instruction_Packet[5] = ID[0];
00169     Instruction_Packet[6] = L_Speed[0];
00170     Instruction_Packet[7] = H_Speed[0];
00171     Instruction_Packet[8] = ID[1];
00172     Instruction_Packet[9] = L_Speed[1];
00173     Instruction_Packet[10] = H_Speed[1];
00174     Instruction_Packet[11] = ID[2];
00175     Instruction_Packet[12] = L_Speed[2];
00176     Instruction_Packet[13] = H_Speed[2];
00177     Instruction_Packet[14] = ~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID[0] + L_Speed[0] + H_Speed[0] + ID[1] + L_Speed[1] + H_Speed[1] + ID[2] + L_Speed[2] + H_Speed[2]);
00178     writePacket();
00179 }
00180 
00181 // unsigned int Dynamixel::ping(uint8_t ID)
00182 // {
00183 //     Instruction_Packet[0] = ID;
00184 //     Instruction_Packet[1] = PING_LENGTH;
00185 //     Instruction_Packet[2] = COMMAND_PING;
00186 //     Instruction_Packet[3] = ~(ID + PING_LENGTH + COMMAND_PING);
00187 //     transmitInstructionPacket();
00188 //     // if ((ID == 0xFE) || (Status_Return_Value != ALL))
00189 //     //     return 0x00;
00190 //     // else
00191 //     // {
00192 //     readStatusPacket();
00193 //     return 0x01;
00194 //     // if (Status_Packet[2] == 0)
00195 //     // return (Status_Packet[0]);
00196 //     // else
00197 //     // return (Status_Packet[2] | 0xF000);
00198 //     // }
00199 // }
00200 // unsigned int Dynamixel::getTemperature(uint8_t ID)
00201 // {
00202 //     Instruction_Packet[0] = ID;
00203 //     Instruction_Packet[1] = READ_TEMP_LENGTH;
00204 //     Instruction_Packet[2] = COMMAND_READ_DATA;
00205 //     Instruction_Packet[3] = RAM_PRESENT_TEMPERATURE;
00206 //     Instruction_Packet[4] = READ_ONE_BYTE_LENGTH;
00207 //     Instruction_Packet[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH);
00208 
00209 //     transmitInstructionPacket();
00210 //     return readStatusPacket();
00211 //     // return Instruction_Packet[5];
00212 //     /*
00213 //     if (Status_Packet[2] == 0)
00214 //     { // If there is no status packet error return value
00215 //         // return Status_Packet[3];
00216 //         return 0;
00217 //     }
00218 //     else
00219 //     {
00220 //         return 1;
00221 //         // return (Status_Packet[2] | 0xF000); // If there is a error Returns error value
00222 //     }*/
00223 // }