Abe Takumi / Mbed 2 deprecated RS405CB_test

meister2013 control test program

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Revision 0:6e1fdd3ca40d, committed 2013-08-05

Comitter:
YSB
Date:
Mon Aug 05 08:06:42 2013 +0000
Commit message:
for yamada

Changed in this revision

RS405cb.cpp Show annotated file Show diff for this revision Revisions of this file
RS405cb.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RS405cb.h	Mon Aug 05 08:06:42 2013 +0000
@@ -0,0 +1,47 @@
+#define RIGHT 1
+#define LEFT 0
+
+#define VERTICAL 1
+#define HORIZONTAL 0
+
+#ifndef MBED_RS405CB_H
+#define MBED_RS405CB_h
+
+class RS405cb: public Serial{
+
+public:
+
+    RS405cb(PinName tx,PinName rx,PinName permit);
+        
+    //////////////RS405_SERVO_command/////////////////////////////////////////////
+        /////function for setting
+            void TORQUE_ON(unsigned char id);
+            void ID_CHANGE(unsigned char oldid,unsigned char newid);
+            void DECIDE_LIMIT_ANGLE(unsigned char id,unsigned int CWlimit,unsigned int CCWlimit);
+            void REQUIRE_RETURN_PACKET(unsigned char id);
+        /////function for control
+            void Rotate_Servo(unsigned char id,char RL,unsigned int ANGLE);//ANGLE:0-900 means 0.0 degree to 90.0 degree
+            void Rotate_Servo_Float(unsigned char id,float ANGLE);//ANGLE:-90.0 to -90.0
+            void Rotate_Servo_Float_Test(unsigned char id);//Ones this function called,main sequence is put in "while(1)"
+    
+    //function for 2013 meister's airframe "SHO"  
+        void Sho_Init(int center_vert,int center_hori,int pls15_vert,int pls15_hori,int mns15_vert,int mns15_hori);
+        void Sho_Rotate_Servo(unsigned char id,int VH,float angle_of_attack);//-15.0 deg to 15.0 deg
+
+
+private:
+
+    //////function for RS405 basic command
+        void RSW2(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt,unsigned char lowdata,unsigned char highdata);
+        void RSW1(unsigned char id,unsigned char flg1,unsigned char adr1,unsigned char len1,unsigned char cnt1,unsigned char data1);
+        void RSW0(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt);
+
+    //variable for 2013 meister's airframe "SHO"  
+        int center_v,center_h,pls15_v,pls15_h,mns15_v,mns15_h;
+    
+    //Serial rs405cb;
+        DigitalOut _permit;
+        
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 05 08:06:42 2013 +0000
@@ -0,0 +1,21 @@
+#include "mbed.h"
+//header to use rs405cb class
+#include "RS405cb.h"
+
+//make object
+RS405cb servo(p28,p27,p21);//TX,RX,PERMIT  (PERMIT means RE/DE Pin of ltc485)
+
+//main
+int main() {
+    TORQUE_ON(1); //means "make torque on of servo whose id is 1." 
+    while(1) {
+        Rotate_Servo_Float(1,-90.0);//means "change angle to -90.0 degree :ID=1"    angle scope:-90.0 to -90.0    
+        wait(1.0);
+        Rotate_Servo_Float(1,0.0);
+        wait(1.0);
+        Rotate_Servo_Float(1,90.0);
+        wait(1.0);
+        Rotate_Servo_Float(1,0.0);
+        wait(1.0);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Aug 05 08:06:42 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file