Diff: RS405cb.cpp

Revision:

1:4f4b999b4939

Parent:

0:03b3a91ccd08

--- a/RS405cb.cpp	Fri Apr 12 10:32:21 2013 +0000
+++ b/RS405cb.cpp	Sat Jul 13 17:06:43 2013 +0000
@@ -6,10 +6,10 @@
 }
 
 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(1);
     TORQUE_ON(2);
     TORQUE_ON(3);
-    TORQUE_ON(4);
+    //TORQUE_ON(4);
     
     center_v = center_vert;
     center_h = center_hori;
@@ -105,6 +105,18 @@
     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){
@@ -128,9 +140,13 @@
 
 void RS405cb::ID_CHANGE(unsigned char oldid,unsigned char newid){
   RSW1(oldid,0x00,0x04,0x01,0x01,newid);
+  wait(0.5);
   RSW0(newid,0x40,0xFF,0x00,0x00);
-  wait(2.0);
+  wait(3.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){