RS405cb.cpp

00001 #include "mbed.h"
00002 #include "RS405cb.h"
00003 
00004 RS405cb::RS405cb(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit){
00005     baud(115200);
00006 }
00007 
00008 void RS405cb::Sho_Init(int center_vert,int center_hori,int pls15_vert,int pls15_hori,int mns15_vert,int mns15_hori){
00009     //TORQUE_ON(1);
00010     TORQUE_ON(2);
00011     TORQUE_ON(3);
00012     //TORQUE_ON(4);
00013     
00014     center_v = center_vert;
00015     center_h = center_hori;
00016     pls15_v = pls15_vert;
00017     pls15_h = pls15_hori;
00018     mns15_v = mns15_vert;
00019     mns15_h = mns15_hori;
00020 }
00021 
00022 void RS405cb::RSW1(unsigned char id,unsigned char flg1,unsigned char adr1,unsigned char len1,unsigned char cnt1,unsigned char data1){
00023     unsigned char sum;
00024     sum = id ^ flg1 ^ adr1 ^ len1 ^ cnt1 ^ data1;
00025     _permit = 1;
00026     putc(0xFA);
00027     putc(0xAF);
00028     putc(id);
00029     putc(flg1);
00030     putc(adr1);
00031     putc(len1);
00032     putc(cnt1);
00033     putc(data1);
00034     putc(sum);
00035     putc(0x00);
00036     //putc(0x00);
00037     while(!writeable()){}
00038     _permit = 0;
00039 }
00040 
00041 void RS405cb::RSW2(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt,unsigned char lowdata,unsigned char highdata){
00042     unsigned char sum;
00043     sum = id ^ flg ^ adr ^ len ^ cnt ^ lowdata ^ highdata;
00044     _permit = 1;
00045     putc(0xFA);
00046     putc(0xAF);
00047     putc(id);
00048     putc(flg);
00049     putc(adr);
00050     putc(len);
00051     putc(cnt);
00052     putc(lowdata);
00053     putc(highdata);
00054     putc(sum);
00055     putc(0x00);
00056     //putc(0x00);
00057     while(!writeable()){}
00058     _permit = 0;
00059 }
00060 
00061 void RS405cb::RSW0(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt){
00062     unsigned char sum;
00063     sum = id ^ flg ^ adr ^ len ^ cnt;
00064     _permit = 1;
00065     putc(0xFA);
00066     putc(0xAF);
00067     putc(id);
00068     putc(flg);
00069     putc(adr);
00070     putc(len);
00071     putc(cnt);
00072     putc(sum);
00073     putc(0x00);
00074     //putc(0x00);
00075     while(!writeable()){}
00076     _permit = 0;
00077 }
00078 
00079 void RS405cb::Rotate_Servo(unsigned char id,char RL,unsigned int ANGLE){
00080   if(RL == RIGHT){
00081       unsigned char hd1,ld1;
00082       hd1 = (ANGLE & 0xFF00) >> 8;
00083       ld1 = (ANGLE & 0x00FF);
00084       RSW2(id,0x00,0x1E,0x02,0x01,ld1,hd1);
00085   }
00086   else{
00087       unsigned char hd2,ld2;
00088       hd2 = ((0x10000 - ANGLE) & 0xFF00) >> 8;
00089       ld2 = ((0x10000 - ANGLE) & 0x00FF);
00090       RSW2(id,0x00,0x1E,0x02,0x01,ld2,hd2);
00091   }
00092 }
00093 
00094 void RS405cb::Rotate_Servo_Float(unsigned char id,float ANGLE){
00095     unsigned char hd,ld;
00096     if(ANGLE >=0){
00097         hd = ((int)(ANGLE*10.0) & 0x0000FF00) >> 8;
00098         ld = ((int)(ANGLE*10.0) & 0x000000FF);
00099     }
00100     else if(ANGLE <0){
00101         hd = ((0x100000000 + (int)(ANGLE*10.0))&0x0000FF00)>>8;
00102         ld = ((0x100000000 + (int)(ANGLE*10.0))&0x000000FF);
00103     }
00104     else{}
00105     RSW2(id,0x00,0x1E,0x02,0x01,ld,hd);
00106 }
00107 
00108 void RS405cb::Rotate_Servo_Float_Test(unsigned char id){
00109     while(1){
00110         TORQUE_ON(id);
00111         Rotate_Servo_Float(id,0.0);
00112         wait(1.0);
00113         Rotate_Servo_Float(id,50.0);
00114         wait(1.0);
00115         Rotate_Servo_Float(id,-20.0);
00116         wait(1.0);
00117     }
00118 }
00119 
00120 void RS405cb::Sho_Rotate_Servo(unsigned char id,int VH,float angle_of_attack){
00121     int center,pls15,mns15;
00122     if(VH == VERTICAL){
00123         center = center_v;
00124         pls15 = pls15_v;
00125         mns15 = mns15_v;
00126     }
00127     else{
00128         center = center_h;
00129         pls15 = pls15_h;
00130         mns15 = mns15_h;    
00131     }
00132     
00133     if(angle_of_attack >= 0){Rotate_Servo_Float(id,center + (pls15 - center)*(angle_of_attack / 15.0));}
00134     else{Rotate_Servo_Float(id,mns15 + (center - mns15)*((15.0 + angle_of_attack) / 15.0));}
00135 }
00136 
00137 void RS405cb::TORQUE_ON(unsigned char id){
00138   RSW1(id,0x00,0x24,0x01,0x01,0x01);
00139 }
00140 
00141 void RS405cb::ID_CHANGE(unsigned char oldid,unsigned char newid){
00142   RSW1(oldid,0x00,0x04,0x01,0x01,newid);
00143   RSW0(newid,0x40,0xFF,0x00,0x00);
00144   wait(2.0);
00145   RSW0(newid,0x20,0xFF,0x00,0x00);
00146   while(1){
00147     Rotate_Servo_Float_Test(newid);
00148   }
00149 }
00150 
00151 void RS405cb::DECIDE_LIMIT_ANGLE(unsigned char id,unsigned int CWlimit,unsigned int CCWlimit){
00152   RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)(CWlimit & 0x00FF),(unsigned char)((CWlimit & 0xFF00)>>8));
00153   RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)((0x10000 - CCWlimit) & 0x00FF),(unsigned char)(((0x10000-CCWlimit) & 0xFF00)>>8));
00154   RSW0(id,0x40,0xFF,0x00,0x00);
00155   wait(2.0);
00156   RSW0(id,0x20,0xFF,0x00,0x00);
00157 }
00158 
00159 void RS405cb::REQUIRE_RETURN_PACKET(unsigned char id){
00160   //RSW1(id,0x09,0x24,0x01,0x01,0x01);
00161   RSW0(id,0x09,0x00,0x00,0x01);
00162 }