Diff: RS405cb.cpp

Revision:

0:6e1fdd3ca40d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RS405cb.cpp	Mon Aug 05 08:06:42 2013 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "RS405cb.h"
+
+RS405cb::RS405cb(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit){
+    baud(115200);
+}
+
+void RS405cb::Sho_Init(int center_vert,int center_hori,int pls15_vert,int pls15_hori,int mns15_vert,int mns15_hori){
+    //TORQUE_ON(1);
+    TORQUE_ON(2);
+    TORQUE_ON(3);
+    //TORQUE_ON(4);
+    
+    center_v = center_vert;
+    center_h = center_hori;
+    pls15_v = pls15_vert;
+    pls15_h = pls15_hori;
+    mns15_v = mns15_vert;
+    mns15_h = mns15_hori;
+}
+
+void RS405cb::RSW1(unsigned char id,unsigned char flg1,unsigned char adr1,unsigned char len1,unsigned char cnt1,unsigned char data1){
+    unsigned char sum;
+    sum = id ^ flg1 ^ adr1 ^ len1 ^ cnt1 ^ data1;
+    _permit = 1;
+    putc(0xFA);
+    putc(0xAF);
+    putc(id);
+    putc(flg1);
+    putc(adr1);
+    putc(len1);
+    putc(cnt1);
+    putc(data1);
+    putc(sum);
+    putc(0x00);
+    //putc(0x00);
+    while(!writeable()){}
+    _permit = 0;
+}
+
+void RS405cb::RSW2(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt,unsigned char lowdata,unsigned char highdata){
+    unsigned char sum;
+    sum = id ^ flg ^ adr ^ len ^ cnt ^ lowdata ^ highdata;
+    _permit = 1;
+    putc(0xFA);
+    putc(0xAF);
+    putc(id);
+    putc(flg);
+    putc(adr);
+    putc(len);
+    putc(cnt);
+    putc(lowdata);
+    putc(highdata);
+    putc(sum);
+    putc(0x00);
+    //putc(0x00);
+    while(!writeable()){}
+    _permit = 0;
+}
+
+void RS405cb::RSW0(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt){
+    unsigned char sum;
+    sum = id ^ flg ^ adr ^ len ^ cnt;
+    _permit = 1;
+    putc(0xFA);
+    putc(0xAF);
+    putc(id);
+    putc(flg);
+    putc(adr);
+    putc(len);
+    putc(cnt);
+    putc(sum);
+    putc(0x00);
+    //putc(0x00);
+    while(!writeable()){}
+    _permit = 0;
+}
+
+void RS405cb::Rotate_Servo(unsigned char id,char RL,unsigned int ANGLE){
+  if(RL == RIGHT){
+      unsigned char hd1,ld1;
+      hd1 = (ANGLE & 0xFF00) >> 8;
+      ld1 = (ANGLE & 0x00FF);
+      RSW2(id,0x00,0x1E,0x02,0x01,ld1,hd1);
+  }
+  else{
+      unsigned char hd2,ld2;
+      hd2 = ((0x10000 - ANGLE) & 0xFF00) >> 8;
+      ld2 = ((0x10000 - ANGLE) & 0x00FF);
+      RSW2(id,0x00,0x1E,0x02,0x01,ld2,hd2);
+  }
+}
+
+void RS405cb::Rotate_Servo_Float(unsigned char id,float ANGLE){
+    unsigned char hd,ld;
+    if(ANGLE >=0){
+        hd = ((int)(ANGLE*10.0) & 0x0000FF00) >> 8;
+        ld = ((int)(ANGLE*10.0) & 0x000000FF);
+    }
+    else if(ANGLE <0){
+        hd = ((0x100000000 + (int)(ANGLE*10.0))&0x0000FF00)>>8;
+        ld = ((0x100000000 + (int)(ANGLE*10.0))&0x000000FF);
+    }
+    else{}
+    RSW2(id,0x00,0x1E,0x02,0x01,ld,hd);
+}
+
+void RS405cb::Rotate_Servo_Float_Test(unsigned char id){
+    while(1){
+        TORQUE_ON(id);
+        Rotate_Servo_Float(id,0.0);
+        wait(1.0);
+        Rotate_Servo_Float(id,50.0);
+        wait(1.0);
+        Rotate_Servo_Float(id,-20.0);
+        wait(1.0);
+    }
+}
+
+void RS405cb::Sho_Rotate_Servo(unsigned char id,int VH,float angle_of_attack){
+    int center,pls15,mns15;
+    if(VH == VERTICAL){
+        center = center_v;
+        pls15 = pls15_v;
+        mns15 = mns15_v;
+    }
+    else{
+        center = center_h;
+        pls15 = pls15_h;
+        mns15 = mns15_h;    
+    }
+    
+    if(angle_of_attack >= 0){Rotate_Servo_Float(id,center + (pls15 - center)*(angle_of_attack / 15.0));}
+    else{Rotate_Servo_Float(id,mns15 + (center - mns15)*((15.0 + angle_of_attack) / 15.0));}
+}
+
+void RS405cb::TORQUE_ON(unsigned char id){
+  RSW1(id,0x00,0x24,0x01,0x01,0x01);
+}
+
+void RS405cb::ID_CHANGE(unsigned char oldid,unsigned char newid){
+  RSW1(oldid,0x00,0x04,0x01,0x01,newid);
+  RSW0(newid,0x40,0xFF,0x00,0x00);
+  wait(2.0);
+  RSW0(newid,0x20,0xFF,0x00,0x00);
+  while(1){
+    Rotate_Servo_Float_Test(newid);
+  }
+}
+
+void RS405cb::DECIDE_LIMIT_ANGLE(unsigned char id,unsigned int CWlimit,unsigned int CCWlimit){
+  RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)(CWlimit & 0x00FF),(unsigned char)((CWlimit & 0xFF00)>>8));
+  RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)((0x10000 - CCWlimit) & 0x00FF),(unsigned char)(((0x10000-CCWlimit) & 0xFF00)>>8));
+  RSW0(id,0x40,0xFF,0x00,0x00);
+  wait(2.0);
+  RSW0(id,0x20,0xFF,0x00,0x00);
+}
+
+void RS405cb::REQUIRE_RETURN_PACKET(unsigned char id){
+  //RSW1(id,0x09,0x24,0x01,0x01,0x01);
+  RSW0(id,0x09,0x00,0x00,0x01);
+}
\ No newline at end of file