D:/DRISSI/arduino-0022/arduino-0022/libraries/dynamixel/dynamixel.cpp
Go to the documentation of this file.
00001 
00005 /* Basic methods to communicate with servomotors */ 
00006 
00007 #include "dynamixel.h"
00008 
00009 
00010 bool   TimeOutFlag=false; 
00011 
00012 
00013 // Initialize communication with servomotors  
00014 void InitDynamixel()  
00015 { 
00016 #ifdef __DEBUG__DYNAMIXEL__
00017         Serial.println("Initialisation of the communication with servomotors");
00018 #endif
00019 
00020         Serial3.begin(117647);                                                                  // Serial link connected via USART3
00021     pinMode(2, OUTPUT);                                                                         // Pin of Half-Duplex configured as an output
00022     digitalWrite(2,LOW);                                                                        // Communication in receiving mode
00023 } 
00024   
00025 
00026 // Time out of serial link in receiving mode
00027 void ISR_TimeOut_Dynamixel()
00028 {
00029     TimeOutFlag=true;                                                                           // Set the TimeOutFlag to true means that receive has failed
00030     Timer1.stop();                                                                                      // Stop Timer1
00031 
00032 #ifdef __DEBUG__DYNAMIXEL__
00033     Serial.println("TimeOut...");
00034 #endif
00035 }
00036 
00037 
00038 // Method to send a packet to the servomotor which ID is specified in parameter 
00039 // Packet Structure : 0xFF, 0xFF, ID, Length, Instruction, Parameters, Checksum
00040 // Nb represents the number of parameters 
00041 void SendPacket (unsigned char ID, unsigned char Instruction,unsigned char * Parameters=0,int Nb=0)
00042 {
00043 #ifdef __DEBUG__DYNAMIXEL__
00044     Serial.println("Sending packet to servomotors...");
00045 #endif
00046 
00047     unsigned int Sum=0,u=0;
00048     while(u<Nb){                                                                                        // Sum of the buffer terms
00049       Sum+=Parameters[u];
00050       u++;}
00051     unsigned char checksum=(~(ID+Instruction+(Nb+0x02)+Sum));// Calculation of the checksum with the Sum calculated previouly
00052 
00053 #ifdef __DEBUG__DYNAMIXEL__
00054     Serial.println(checksum,HEX);
00055 #endif
00056 
00057     digitalWrite(2,HIGH);                                                                       // Pin of Half-duplex in sending mode
00058     Serial3.write(0xFF);                                                                        // Start of frame
00059     Serial3.write(0xFF);                                                                        // Start of frame
00060     Serial3.write(ID);                                                                          // ID of the receiver
00061     Serial3.write(Nb+0x02);                                                                     // Length
00062     Serial3.write(Instruction);
00063     for(int n=0;n<Nb;n++)                                                                       // Buffer (instruction + param.)
00064       Serial3.write(Parameters[n]);
00065     Serial3.write(checksum);                                                            // Checksum
00066       
00067     while (!(UCSR3A &(1<<UDRE3)));                                                      // Wait for the last byte to be send
00068     delayMicroseconds(100);
00069 
00070     digitalWrite(2,LOW);                                                                        // Communication in receiving mode
00071 
00072 #ifdef __DEBUG__DYNAMIXEL__
00073     Serial.println("Transmission complete");    
00074 #endif
00075 }
00076 
00077 
00078 //Method to receive a return packet of servomotor
00079 //The return packet is write in the buffer until Time Out is not finished
00080 //and written data has not overflowed the NbMax data in parameter
00081 int ReceivePacket (unsigned char * Buffer,int NbMax, long TimeOut_Us=1000000)
00082 {
00083 #ifdef __DEBUG__DYNAMIXEL__
00084     Serial.println("Reception in progress...");
00085         Serial.print("A lire : ");
00086     Serial.println(Serial3.available());
00087     Serial.println();
00088 #endif
00089 
00090     int n=0;
00091     Timer1.initialize(TimeOut_Us);                                                      // Initialize the duration of the TimeOut (in µs)
00092     delay(100);
00093     
00094     Timer1.attachInterrupt(ISR_TimeOut_Dynamixel);                      // Attach the interrupt method to the TimeOut
00095     TimeOutFlag=false;
00096     Timer1.start();                                                                                     // Start of Timer1
00097     
00098         while(TimeOutFlag==false && n<NbMax && Serial3.available()>0)// Write in buffer until conditions are verified (see function description for details)
00099     {       
00100             Buffer[n] = Serial3.read();                                         // Read next value and write it in the buffer
00101 #ifdef __DEBUG__DYNAMIXEL__            
00102                         Serial.print(Buffer[n],HEX);
00103             Serial.print("\t");
00104             Serial.print("Reste : ");
00105             Serial.println(Serial3.available());
00106 #endif
00107                         n++;    
00108     }
00109     Timer1.stop();
00110     Timer1.detachInterrupt();
00111 
00112     if(n>=6&&TimeOutFlag==false){                           // If n>=6 (minimal size of a return frame)
00113 #ifdef __DEBUG__DYNAMIXEL__
00114       Serial.println("\t \t \tSuccess");                                        // Indicate that reception is a success
00115 #endif
00116       TimeOutFlag=false;                                                                        // Return to the TimeOutFlag default value
00117       return 1;
00118     }
00119 #ifdef __DEBUG__DYNAMIXEL__
00120     Serial.println("\t \t \tFailure");                                  // Else indicate that reception is a failure
00121 #endif
00122         return 0;
00123 }
00124 
00125 
00126 // Check if a return packet contains no error
00127 int Errors(unsigned char * Packet)
00128 {
00129 #ifdef __DEBUG__DYNAMIXEL__
00130     Serial.println("Checking packet...");    
00131 #endif
00132 
00133         if(Packet[0]!=0xFF || Packet[1]!=0xFF)                                  // First, check that initial frame is correct                      
00134         {
00135                 Serial.println("ERROR : il manque le début de la trame");
00136                 return 1;                                                                                       // If initial frame incorrect, cancel operation
00137     }
00138     else
00139     {
00140        int err=0;
00141        if(Packet[4]&1){
00142          Serial.println("ERROR : Input Voltage Error");
00143          err++;
00144        }
00145        if(Packet[4]&2){
00146          Serial.println("ERROR : Angle Limit Error");
00147          err++;
00148        }
00149        if(Packet[4]&4){
00150          Serial.println("ERROR : Overheating Error");
00151          err++;
00152        }
00153        if(Packet[4]&8){
00154          Serial.println("ERROR : Range Error");
00155          err++;
00156        }
00157        if(Packet[4]&16){
00158          Serial.println("ERROR : Checksum Error");
00159          err++;
00160        }
00161        if(Packet[4]&32){
00162          Serial.println("ERROR : Overload Error");
00163          err++;
00164        }
00165        if(Packet[4]&64){
00166          Serial.println("ERROR : Instruction Error");
00167          err++;
00168        }
00169        if(err!=0)return 2;                                                                      // If  errors are reported, return 2
00170         }
00171         return 0;                                                                                                       // If no errors return 0
00172 }  
00173 
00174 
00175 // Empty the reception buffer (usefull when a reception error occurenced)
00176 void EmptyReceptionBuffer ()
00177 {
00178 #ifdef __DEBUG__DYNAMIXEL__
00179     Serial.println("Emptying reception buffer...");    
00180 #endif
00181 
00182         int *Dummy,k=0;
00183         while(Serial3.available())                                                              // Read while there is data available
00184         {
00185                 Dummy[k]=Serial3.read();
00186                 k++;
00187         }
00188 
00189         Serial.println("Reception buffer is now empty");
00190 }
00191 
00192