Revision 0:5f11daf9b84d, committed 2017-12-20
- Comitter:
- cpul5338
- Date:
- Wed Dec 20 15:36:57 2017 +0000
- Commit message:
- 123
Changed in this revision
diff -r 000000000000 -r 5f11daf9b84d Mx28.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mx28.cpp Wed Dec 20 15:36:57 2017 +0000
@@ -0,0 +1,1024 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include "mbed.h"
+#include "Mx28.h"
+
+
+ unsigned char Instruction_Packet_Array[15]; // Array to hold instruction packet data
+ unsigned char Status_Packet_Array[8]; // Array to hold returned status packet data
+ unsigned int Time_Counter; // Timer for time out watchers
+ unsigned char Direction_Pin; // Pin to control TX/RX buffer chip
+ unsigned char Status_Return_Value = READ; // Status packet return states ( NON , READ , ALL )
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Private Methods
+//-------------------------------------------------------------------------------------------------------------------------------
+void DynamixelClass::debugframe(void) {
+ for (int i=0; i<10; i++)
+ printf("%x,",Instruction_Packet_Array[i]);
+ printf("\r\n");
+}
+
+void DynamixelClass::debugStatusframe(void) {
+ for (int i=0; i<10; i++)
+ printf("%x,",Status_Packet_Array[i]);
+ printf("\r\n");
+}
+
+
+void DynamixelClass::transmitInstructionPacket(void){ // Transmit instruction packet to Dynamixel
+
+ unsigned char Counter;
+ Counter = 0;
+ servoSerialDir->write(1); // Set TX Buffer pin to HIGH
+
+ servoSerial->putc(HEADER); // Write Header (0xFF) data 1 to serial
+ servoSerial->putc(HEADER); // Write Header (0xFF) data 2 to serial
+ servoSerial->putc(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial
+ servoSerial->putc(Instruction_Packet_Array[1]); // Write packet length to serial
+
+ do{
+ servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write Instuction & Parameters (if there is any) to serial
+ Counter++;
+ }while((Instruction_Packet_Array[1] - 2) >= Counter);
+
+ servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial
+ wait_us((Counter + 4)*11);
+ servoSerialDir->write(0); // Set TX Buffer pin to LOW after data has been sent
+
+}
+
+
+unsigned int DynamixelClass::readStatusPacket(void){
+
+ unsigned char Counter = 0x00;
+
+ static unsigned char InBuff[20];
+ unsigned char i, j, RxState;
+
+
+ Status_Packet_Array[0] = 0x00;
+ Status_Packet_Array[1] = 0x00;
+ Status_Packet_Array[2] = 0x00;
+ Status_Packet_Array[3] = 0x00;
+ i=0; RxState=0; j=0; InBuff[0]=0;
+ Timer timer;
+ timer.start();
+
+
+while (RxState<3){ // Wait for " header + header + frame length + error " RX data
+ if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){
+ return Status_Packet_Array[2] = 0x80; // Return with Error if Serial data not received with in time limit
+
+ }
+
+ if (servoSerial->readable()) {
+ InBuff[i]=servoSerial->getc();
+ if (InBuff[0]==0xff) i++; // When we have the first header we starts to inc data to inbuffer
+
+
+ if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header
+
+ switch (RxState) {
+ case 1: {//Read header
+ if ((InBuff[j] == 0xFF) && (InBuff[j+1]== 0xFF)){ // checkes that we have got the buffer
+ j=2;
+ Status_Packet_Array[0] = InBuff[j++]; // ID sent from Dynamixel
+ Status_Packet_Array[1] = InBuff[j++]; // Frame Length of status packet
+ Status_Packet_Array[2] = InBuff[j++]; // Error (char)
+ RxState++; led2->write(0);
+ }
+ } break;
+ case 2: {//Read data
+ if (i>Status_Packet_Array[1]+3) { //We have recieved all data
+ do{
+ Status_Packet_Array[3 + Counter] = InBuff[j++];
+ Counter++;
+ }while(Status_Packet_Array[1] > Counter); // Read Parameter(s) into array
+
+ Status_Packet_Array[Counter + 4] = InBuff[j++]; // Read Check sum
+ RxState++;
+ }
+ } break;
+ } //switch
+ }
+ }//while..
+ timer.stop();
+// debugStatusframe();
+
+ return 0x00;
+}
+
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Public Methods
+//-------------------------------------------------------------------------------------------------------------------------------
+
+ DynamixelClass::DynamixelClass(int baud, PinName D_Pin, PinName tx, PinName rx){
+ servoSerial=new Serial(tx, rx);
+ servoSerial->baud(baud);
+ servoSerialDir= new DigitalOut(D_Pin);
+ servoSerialDir->write(0);
+ led2=new DigitalOut(LED2);
+
+}
+
+ DynamixelClass::~DynamixelClass(){
+ if(servoSerial != NULL)
+ delete servoSerial;
+}
+
+unsigned int DynamixelClass::reset(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = RESET_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_RESET;
+ Instruction_Packet_Array[3] = ~(ID + RESET_LENGTH + COMMAND_RESET); //Checksum;
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+void DynamixelClass::NewBaudRate(int baud) {
+//Change the baudrate on then MBED
+ int Baudrate_BPS = 0;
+ Baudrate_BPS =(int) 2000000 / (baud + 1); // Calculate Baudrate as ber "Robotis e-manual"
+ servoSerial->baud(Baudrate_BPS);
+}
+
+unsigned int DynamixelClass::ping(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = PING_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_PING;
+ Instruction_Packet_Array[3] = ~(ID + PING_LENGTH + COMMAND_PING);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::setStatusPaketReturnDelay(unsigned char ID,unsigned char ReturnDelay){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_RETURN_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_RETURN_DELAY_TIME;
+ Instruction_Packet_Array[4] = (char) (ReturnDelay/2);
+ Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_DELAY_TIME + (char)(ReturnDelay/2));
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+unsigned int DynamixelClass::setID(unsigned char ID, unsigned char New_ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_ID_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_ID;
+ Instruction_Packet_Array[4] = New_ID;
+ Instruction_Packet_Array[5] = ~(ID + SET_ID_LENGTH + COMMAND_WRITE_DATA+ EEPROM_ID + New_ID);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::setBaudRate(unsigned char ID, long Baud){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_BD_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_BAUD_RATE;
+// if (Baud > 2250000){
+if (Baud >= 1000000){
+ switch (Baud){
+ case 2250000:
+ Instruction_Packet_Array[4] = 0xFA;
+ break;
+ case 2500000:
+ Instruction_Packet_Array[4] = 0xFB;
+ break;
+ case 3000000:
+ Instruction_Packet_Array[4] = 0xFC;
+ break;
+ case 1000000:
+ Instruction_Packet_Array[4] = 0x01;
+ }
+ }else{
+ Instruction_Packet_Array[4] = (char)((2000000/Baud) - 1);
+ }
+ Instruction_Packet_Array[5] = ~(ID + SET_BD_LENGTH + COMMAND_WRITE_DATA + EEPROM_BAUD_RATE + (char)((2000000/Baud) - 1) );
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::setMaxTorque( unsigned char ID, int Torque){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ;
+ Instruction_Packet_Array[4] = (char)(Torque);
+ Instruction_Packet_Array[5] = (char)(Torque >> 8);
+ Instruction_Packet_Array[6] = ~(ID + SET_MAX_TORQUE_LENGTH + COMMAND_WRITE_DATA + EEPROM_MAX_TORQUE_L + (char)(Torque) + (char)(Torque >> 8));
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::setHoldingTorque(unsigned char ID, bool Set){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_HOLDING_TORQUE_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE;
+ Instruction_Packet_Array[4] = Set;
+ Instruction_Packet_Array[5] = ~(ID + SET_HOLDING_TORQUE_LENGTH + COMMAND_WRITE_DATA + RAM_TORQUE_ENABLE + Set);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::setAlarmShutdown(unsigned char ID,unsigned char Set){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_ALARM_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_ALARM_SHUTDOWN;
+ Instruction_Packet_Array[4] = Set;
+ Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_SHUTDOWN + Set);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+unsigned int DynamixelClass::setStatusPaket(unsigned char ID,unsigned char Set){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL;
+ Instruction_Packet_Array[4] = Set;
+ Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set);
+
+ Status_Return_Value = Set;
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+
+unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_MODE_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L;
+ if ( Dynamixel_Mode == WHEEL) { // Set WHEEL mode, this is done by setting both the clockwise and anti-clockwise angle limits to ZERO
+ Instruction_Packet_Array[4] = 0x00;
+ Instruction_Packet_Array[5] = 0x00;
+ Instruction_Packet_Array[6] = 0x00;
+ Instruction_Packet_Array[7] = 0x00;
+ Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L);
+ }else { // Else set SERVO mode
+ Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit);
+ Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8);
+ Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit);
+ Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8);
+ Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + (char)(Dynamixel_CW_Limit) + (char)((Dynamixel_CW_Limit & 0x0F00) >> 8) + (char)(Dynamixel_CCW_Limit) + (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8));
+ }
+
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (Status_Return_Value == ALL){
+ readStatusPacket();
+ if (Status_Packet_Array[2] != 0){
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+
+ }
+ return 0x00; //if no errors
+ }
+
+ unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_PUNCH_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_PUNCH_L;
+ Instruction_Packet_Array[4] = (char)(Punch);
+ Instruction_Packet_Array[5] = (char)(Punch >> 8);
+ Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) );
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+ }
+
+ unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_PID_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN;
+ Instruction_Packet_Array[4] = P;
+ Instruction_Packet_Array[5] = I;
+ Instruction_Packet_Array[6] = D;
+ Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D );
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+ }
+
+unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_TEMP_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE;
+ Instruction_Packet_Array[4] = temp;
+ Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SET_VOLT_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE;
+ Instruction_Packet_Array[4] = Volt_L;
+ Instruction_Packet_Array[5] = Volt_H;
+ Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
+ Instruction_Packet_Array[4] = (char)(Position);
+ Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8);
+ Instruction_Packet_Array[6] = (char)(Speed);
+ Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8);
+ Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + Position + (char)((Position & 0x0F00) >> 8) + Speed + (char)((Speed & 0x0F00) >> 8));
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
+ Instruction_Packet_Array[4] = (char)(Position);
+ Instruction_Packet_Array[5] = (char)(Position >> 8);
+ Instruction_Packet_Array[6] = (char)(Speed);
+ Instruction_Packet_Array[7] = (char)(Speed >> 8);
+ Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_POSITION_L + (char)(Position) + (char)(Position >> 8) + (char)(Speed) + (char)(Speed >> 8));
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){
+
+ char Speed_H,Speed_L;
+ Speed_L = Speed;
+ if (Rotation == 0){ // Move Left
+ Speed_H = Speed >> 8;
+ }
+ else if (Rotation == 1){ // Move Right
+ Speed_H = (Speed >> 8)+4;
+ }
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = WHEEL_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+ Instruction_Packet_Array[4] = Speed_L;
+ Instruction_Packet_Array[5] = Speed_H;
+ Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+void DynamixelClass::wheelSync(unsigned char ID1, bool Dir1, unsigned int Speed1, unsigned char ID2, bool Dir2, unsigned int Speed2, unsigned char ID3, bool Dir3, unsigned int Speed3){
+
+ char Speed1_H,Speed1_L;
+ Speed1_L = Speed1;
+ if (Dir1 == 0){ // Move Left
+ Speed1_H = Speed1 >> 8;
+ }
+ else if (Dir1 == 1) // Move Right
+ {
+ Speed1_H = (Speed1 >> 8)+4;
+ }
+
+ char Speed2_H,Speed2_L;
+ Speed2_L = Speed2;
+ if (Dir2 == 0){ // Move Left
+ Speed2_H = Speed2 >> 8;
+ }
+ else if (Dir2 == 1) // Move Right
+ {
+ Speed2_H = (Speed2 >> 8)+4;
+ }
+
+ char Speed3_H,Speed3_L;
+ Speed3_L = Speed3;
+ if (Dir3 == 0){ // Move Left
+ Speed3_H = Speed3 >> 8;
+ }
+ else if (Dir3 == 1) // Move Right
+ {
+ Speed3_H = (Speed3 >> 8)+4;
+ }
+
+ Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos
+ Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE;
+ Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+ Instruction_Packet_Array[4] = SYNC_DATA_LENGTH;
+ Instruction_Packet_Array[5] = ID1;
+ Instruction_Packet_Array[6] = Speed1_L;
+ Instruction_Packet_Array[7] = Speed1_H;
+ Instruction_Packet_Array[8] = ID2;
+ Instruction_Packet_Array[9] = Speed2_L;
+ Instruction_Packet_Array[10] = Speed2_H;
+ Instruction_Packet_Array[11] = ID3;
+ Instruction_Packet_Array[12] = Speed3_L;
+ Instruction_Packet_Array[13] = Speed3_H;
+ Instruction_Packet_Array[14] = (char)(~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID1 + Speed1_L + Speed1_H + ID2 + Speed2_L + Speed2_H + ID3 + Speed3_L + Speed3_H));
+
+ transmitInstructionPacket();
+
+}
+
+unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){
+
+ char Speed_H,Speed_L;
+ Speed_L = Speed;
+ if (Dir == 0){ // Move Left
+ Speed_H = Speed >> 8;
+ }
+ else if (Dir == 1) // Move Right
+ {
+ Speed_H = (Speed >> 8)+4;
+ }
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = WHEEL_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+ Instruction_Packet_Array[4] = Speed_L;
+ Instruction_Packet_Array[5] = Speed_H;
+ Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+unsigned int DynamixelClass::action(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = RESET_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_ACTION;
+ Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = LED_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_LED;
+ Instruction_Packet_Array[4] = Status;
+ Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::readTemperature(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_TEMP_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return Status_Packet_Array[3];
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::readPosition(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_POS_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
+ Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value
+ return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::readLoad(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_LOAD_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L;
+ Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::readSpeed(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_SPEED_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L;
+ Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+
+unsigned int DynamixelClass::readVoltage(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_VOLT_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage)
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::checkRegister(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_REGISTER_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_REGISTER;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[3]); // Return register value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::checkMovement(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_MOVING_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_MOVING;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[3]); // Return movement value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::checkLock(unsigned char ID){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = READ_LOCK_LENGTH;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = RAM_LOCK;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[3]); // Return Lock value
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = 0x04;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE;
+ Instruction_Packet_Array[4] = Status;
+ Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+}
+
+unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){
+
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = 0x05;
+ Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+ Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L;
+ Instruction_Packet_Array[4] = (char)(Torque);
+ Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8);
+ Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) );
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+
+ if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it
+ return (0x00);
+ }else{
+ readStatusPacket();
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return (Status_Packet_Array[0]); // Return SERVO ID
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+ }
+
+}
+
+unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){
+
+ Instruction_Packet_Array[0] = ID;
+ Instruction_Packet_Array[1] = 0x04;
+ Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+ Instruction_Packet_Array[3] = Register;
+ Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+ Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH);
+
+ if (servoSerial->readable())
+ while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+ transmitInstructionPacket();
+ readStatusPacket();
+
+ if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
+ return Status_Packet_Array[3];
+ }else{
+ return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
+ }
+}
+
+
diff -r 000000000000 -r 5f11daf9b84d Mx28.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mx28.h Wed Dec 20 15:36:57 2017 +0000
@@ -0,0 +1,206 @@
+/*
+How Dynamixel work can be found
+--------------------------------
+Robotis e-Manual
+http://support.robotis.com
+
+Overview of Communication
+http://support.robotis.com/en/product/dynamixel/dxl_communication.htm
+
+Kind of Instruction
+http://support.robotis.com/en/product/dynamixel/communication/dxl_instruction.htm
+
+Instruction Packet & Status Packet (Return Packet)
+http://support.robotis.com/en/product/dynamixel/communication/dxl_packet.htm
+
+Control Table
+http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm
+
+ */
+
+#ifndef Mx28_h
+#define Mx28_h
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// define - Dynamixel Hex code table
+//-------------------------------------------------------------------------------------------------------------------------------
+// EEPROM AREA
+#define EEPROM_MODEL_NUMBER_L 0x00
+#define EEPROM_MODEL_NUMBER_H 0x01
+#define EEPROM_VERSION 0x02
+#define EEPROM_ID 0x03
+#define EEPROM_BAUD_RATE 0x04
+#define EEPROM_RETURN_DELAY_TIME 0x05
+#define EEPROM_CW_ANGLE_LIMIT_L 0x06
+#define EEPROM_CW_ANGLE_LIMIT_H 0x07
+#define EEPROM_CCW_ANGLE_LIMIT_L 0x08
+#define EEPROM_CCW_ANGLE_LIMIT_H 0x09
+#define EEPROM_LIMIT_TEMPERATURE 0x0B
+#define EEPROM_LOW_LIMIT_VOLTAGE 0x0C
+#define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D
+#define EEPROM_MAX_TORQUE_L 0x0E
+#define EEPROM_MAX_TORQUE_H 0x0F
+#define EEPROM_RETURN_LEVEL 0x10
+#define EEPROM_ALARM_LED 0x11
+#define EEPROM_ALARM_SHUTDOWN 0x12
+// RAM AREA
+#define RAM_TORQUE_ENABLE 0x18
+#define RAM_LED 0x19
+#define RAM_PROPORTIONAL_GAIN 0x1A
+#define RAM_INTERGRAL_GAIN 0x1B
+#define RAM_DERIVATIVE_GAIN 0x1C
+#define RAM_GOAL_POSITION_L 0x1E
+#define RAM_GOAL_POSITION_H 0x1F
+#define RAM_GOAL_SPEED_L 0x20
+#define RAM_GOAL_SPEED_H 0x21
+#define RAM_TORQUE_LIMIT_L 0x22
+#define RAM_TORQUE_LIMIT_H 0x23
+#define RAM_PRESENT_POSITION_L 0x24
+#define RAM_PRESENT_POSITION_H 0x25
+#define RAM_PRESENT_SPEED_L 0x26
+#define RAM_PRESENT_SPEED_H 0x27
+#define RAM_PRESENT_LOAD_L 0x28
+#define RAM_PRESENT_LOAD_H 0x29
+#define RAM_PRESENT_VOLTAGE 0x2A
+#define RAM_PRESENT_TEMPERATURE 0x2B
+#define RAM_REGISTER 0x2C
+#define RAM_MOVING 0x2E
+#define RAM_LOCK 0x2F
+#define RAM_PUNCH_L 0x30
+#define RAM_PUNCH_H 0x31
+
+#define RAM_TORQUE_CONTROL_MODE_ENABLE 0X46
+#define RAM_GOAL_TORQUE_L 0X47
+#define RAM_GOAL_TORQUE_H 0X48
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Instruction commands Set
+//-------------------------------------------------------------------------------------------------------------------------------
+#define COMMAND_PING 0x01
+#define COMMAND_READ_DATA 0x02
+#define COMMAND_WRITE_DATA 0x03
+#define COMMAND_REG_WRITE_DATA 0x04
+#define COMMAND_ACTION 0x05
+#define COMMAND_RESET 0x06
+#define COMMAND_SYNC_WRITE 0x83
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+//Instruction packet lengths
+#define READ_ONE_BYTE_LENGTH 0x01
+#define READ_TWO_BYTE_LENGTH 0x02
+#define RESET_LENGTH 0x02
+#define PING_LENGTH 0x02
+#define ACTION_LENGTH 0x02
+#define SET_ID_LENGTH 0x04
+#define SET_BD_LENGTH 0x04
+#define SET_RETURN_LEVEL_LENGTH 0x04
+#define READ_TEMP_LENGTH 0x04
+#define READ_POS_LENGTH 0x04
+#define READ_LOAD_LENGTH 0x04
+#define READ_SPEED_LENGTH 0x04
+#define READ_VOLT_LENGTH 0x04
+#define READ_REGISTER_LENGTH 0x04
+#define READ_MOVING_LENGTH 0x04
+#define READ_LOCK_LENGTH 0x04
+#define LED_LENGTH 0x04
+#define SET_HOLDING_TORQUE_LENGTH 0x04
+#define SET_MAX_TORQUE_LENGTH 0x05
+#define SET_ALARM_LENGTH 0x04
+#define READ_LOAD_LENGTH 0x04
+#define SET_RETURN_LENGTH 0x04
+#define WHEEL_LENGTH 0x05
+#define SERVO_GOAL_LENGTH 0x07
+#define SET_MODE_LENGTH 0x07
+#define SET_PUNCH_LENGTH 0x04
+#define SET_PID_LENGTH 0x06
+#define SET_TEMP_LENGTH 0x04
+#define SET_VOLT_LENGTH 0x05
+#define SYNC_LOAD_LENGTH 0x0D
+#define SYNC_DATA_LENGTH 0x02
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Specials
+//-------------------------------------------------------------------------------------------------------------------------------
+
+#define OFF 0x00
+#define ON 0x01
+
+#define SERVO 0x01
+#define WHEEL 0x00
+
+#define LEFT 0x00
+#define RIGHT 0x01
+
+#define NONE 0x00
+#define READ 0x01
+#define ALL 0x02
+
+#define BROADCAST_ID 0xFE
+
+#define HEADER 0xFF
+
+#define STATUS_PACKET_TIMEOUT 50 // in millis()
+#define STATUS_FRAME_BUFFER 5
+
+
+
+class DynamixelClass {
+private:
+ DigitalOut *servoSerialDir;
+ Serial *servoSerial;
+ void transmitInstructionPacket(void);
+ unsigned int readStatusPacket(void);
+ bool overflow;
+ void debugframe(void);
+ void debugStatusframe(void);
+ DigitalOut *led2;
+public:
+
+ DynamixelClass(int baud,PinName D_Pin,PinName tx, PinName rx); //Constructor
+ ~DynamixelClass(void); //destruktor
+
+ void NewBaudRate(int baud);
+ unsigned int reset(unsigned char);
+ unsigned int ping(unsigned char);
+
+ unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char);
+ unsigned int setID(unsigned char, unsigned char);
+ unsigned int setBaudRate(unsigned char, long);
+ unsigned int setMaxTorque(unsigned char, int);
+ unsigned int setHoldingTorque(unsigned char, bool);
+ unsigned int setAlarmShutdown(unsigned char,unsigned char);
+ unsigned int setStatusPaket(unsigned char,unsigned char);
+ unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
+ unsigned int setPunch(unsigned char,unsigned int);
+ unsigned int setPID(unsigned char,unsigned char,unsigned char,unsigned char);
+ unsigned int setTemp(unsigned char,unsigned char);
+ unsigned int setVoltage(unsigned char,unsigned char,unsigned char);
+
+ unsigned int servo(unsigned char, unsigned int, unsigned int);
+ unsigned int servoPreload(unsigned char, unsigned int, unsigned int);
+ unsigned int wheel(unsigned char, bool, unsigned int);
+ void wheelSync(unsigned char,bool,unsigned int,unsigned char, bool,unsigned int,unsigned char, bool,unsigned int);
+ unsigned int wheelPreload(unsigned char, bool, unsigned int);
+
+ unsigned int action(unsigned char);
+
+ unsigned int readTemperature(unsigned char);
+ unsigned int readVoltage(unsigned char);
+ unsigned int readPosition(unsigned char);
+ unsigned int readLoad(unsigned char);
+ unsigned int readSpeed(unsigned char);
+
+ unsigned int checkRegister(unsigned char);
+ unsigned int checkMovement(unsigned char);
+ unsigned int checkLock(unsigned char);
+
+ unsigned int ledState(unsigned char, bool);
+
+ unsigned int torqueMode(unsigned char ID, bool Status);
+ unsigned int torque(unsigned char ID,unsigned int Torque);
+ unsigned int readRegister(unsigned char ID,unsigned char Register);
+};
+
+#endif
diff -r 000000000000 -r 5f11daf9b84d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Dec 20 15:36:57 2017 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "Mx28.h"
+#include <math.h>
+
+// Dynamixel
+#define SERVO_ID 3 // ID of which we will set Dynamixel too
+#define SERVO_ControlPin A2 // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
+#define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
+#define TxPin A0
+#define RxPin A1
+#define CW_LIMIT_ANGLE 0x001 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
+#define CCW_LIMIT_ANGLE 0xFFF // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
+#define MAX_TORQUE_LIMIT 0x3FF
+#define MIN_TORQUE_LIMIT 0x3FF
+
+#define pi 3.14
+#define Ts 1 // 週期
+#define A 25//15 // angle
+#define f 0.5 // Hz
+unsigned long tt = 0;
+double deg = 0;
+int Pos = 0;
+int zero = 190;
+char mode='a';
+DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
+DigitalIn mybutton(USER_BUTTON);
+DigitalOut pin_pc2(PC_3);
+//
+// Interrupt
+//Ticker timer1;
+// timer 1
+#define LOOPTIME 1000 // 1 ms
+unsigned long lastMilli = 0;
+// sampling time
+float T = 0.001;
+
+Timer t;
+
+int sensor_flag = 0; // sensor initial flag
+int sensor_count = 0;
+int i=0;
+
+
+int main()
+{
+ dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits
+ dynamixelClass.setMaxTorque( SERVO_ID, MAX_TORQUE_LIMIT);
+// dynamixelClass.setPID(SERVO_ID,0,0,254);
+// timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
+// USB.attach(&change_mode); //
+// dynamixelClass.torqueMode(SERVO_ID,OFF);
+// wait_ms(1);
+// dynamixelClass.setHoldingTorque(SERVO_ID,ON);
+// dynamixelClass.ledState(SERVO_ID,ON);
+// wait_ms(1);
+ //dynamixelClass.torque(SERVO_ID,1023);
+ t.start();
+// SW = 0;
+
+ while(1)
+ {
+// pin_pc2 = 0;
+// wait(0.2);
+// pin_pc2 = 1;
+// wait(0.2);
+ //id 1: 5~55
+ //id 2: 40~
+// deg = (45)*4096.0f/360.0f;
+ deg = (100)*4096.0f/360.0f;
+//// case 'a':
+//// deg = (zero+60)*4096.0f/360.0f;
+//// break;
+//// case 'b':
+//// deg = (zero+10)*4096.0f/360.0f;
+//// break;
+//// case 'c':
+//// deg = (zero+45-A*sin(2*pi*f*tt*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖15
+// case 'a':
+//// deg = (zero+60)*4096.0f/360.0f;
+// break;
+// case 'b':
+// deg = (zero+10)*4096.0f/360.0f;
+// break;
+// case 'c':
+// deg = (30-A*sin(2*pi*f*tt*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖15
+
+
+ i++;
+ if(i==5000)dynamixelClass.setHoldingTorque(SERVO_ID,ON);
+ else if (i<5000)dynamixelClass.servo(SERVO_ID,deg,0x400);
+ else;
+}
+}
\ No newline at end of file
diff -r 000000000000 -r 5f11daf9b84d mbed.bld
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Wed Dec 20 15:36:57 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e7ca05fa8600
\ No newline at end of file