Go to the documentation of this file.00001
00005
00006
00007 #include "dynamixel.h"
00008
00009
00010 bool TimeOutFlag=false;
00011
00012
00013
00014 void InitDynamixel()
00015 {
00016 #ifdef __DEBUG__DYNAMIXEL__
00017 Serial.println("Initialisation of the communication with servomotors");
00018 #endif
00019
00020 Serial3.begin(117647);
00021 pinMode(2, OUTPUT);
00022 digitalWrite(2,LOW);
00023 }
00024
00025
00026
00027 void ISR_TimeOut_Dynamixel()
00028 {
00029 TimeOutFlag=true;
00030 Timer1.stop();
00031
00032 #ifdef __DEBUG__DYNAMIXEL__
00033 Serial.println("TimeOut...");
00034 #endif
00035 }
00036
00037
00038
00039
00040
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){
00049 Sum+=Parameters[u];
00050 u++;}
00051 unsigned char checksum=(~(ID+Instruction+(Nb+0x02)+Sum));
00052
00053 #ifdef __DEBUG__DYNAMIXEL__
00054 Serial.println(checksum,HEX);
00055 #endif
00056
00057 digitalWrite(2,HIGH);
00058 Serial3.write(0xFF);
00059 Serial3.write(0xFF);
00060 Serial3.write(ID);
00061 Serial3.write(Nb+0x02);
00062 Serial3.write(Instruction);
00063 for(int n=0;n<Nb;n++)
00064 Serial3.write(Parameters[n]);
00065 Serial3.write(checksum);
00066
00067 while (!(UCSR3A &(1<<UDRE3)));
00068 delayMicroseconds(100);
00069
00070 digitalWrite(2,LOW);
00071
00072 #ifdef __DEBUG__DYNAMIXEL__
00073 Serial.println("Transmission complete");
00074 #endif
00075 }
00076
00077
00078
00079
00080
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);
00092 delay(100);
00093
00094 Timer1.attachInterrupt(ISR_TimeOut_Dynamixel);
00095 TimeOutFlag=false;
00096 Timer1.start();
00097
00098 while(TimeOutFlag==false && n<NbMax && Serial3.available()>0)
00099 {
00100 Buffer[n] = Serial3.read();
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){
00113 #ifdef __DEBUG__DYNAMIXEL__
00114 Serial.println("\t \t \tSuccess");
00115 #endif
00116 TimeOutFlag=false;
00117 return 1;
00118 }
00119 #ifdef __DEBUG__DYNAMIXEL__
00120 Serial.println("\t \t \tFailure");
00121 #endif
00122 return 0;
00123 }
00124
00125
00126
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)
00134 {
00135 Serial.println("ERROR : il manque le début de la trame");
00136 return 1;
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;
00170 }
00171 return 0;
00172 }
00173
00174
00175
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())
00184 {
00185 Dummy[k]=Serial3.read();
00186 k++;
00187 }
00188
00189 Serial.println("Reception buffer is now empty");
00190 }
00191
00192