Abe Takumi
/
RS405CB_test
meister2013 control test program
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Aug 6 2022 14:04:56 by 1.7.2