Diff: servo.h

Revision:

1:19d647492936

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,81 @@
+void control_servo( void )
+{
+    short   torque[9]  = { 0xfa, 0xaf, 0x00, 0x00, 0x24, 0x01, 0x01, 0x01, 0x00 };
+    short send_deg[10] = { 0xfa, 0xaf, 0x00, 0x00, 0x1e, 0x02, 0x01, 0x01, 0x00, 0x00 };
+    //                      Hdr,  Hdr,   ID,  Flg,  Adr,  Len,  Cnt,Lower,Upper,  Sum
+    
+    for( int a = 0; a < LENGTH; a ++ )
+    {
+        torque[2] = ID_num[a];
+        torque[8] = cal_sum( torque, 8 );
+        for( int b = 0; b < 9; b ++ )   servo.putc( torque[b] );
+        wait_ms( 5 );
+    }
+    
+    while( 1 )
+    {
+        for( int a = 0; a < LENGTH; a ++ )
+        {
+            for( int i = 0; i < LENGTH; i ++ )
+            {
+                send_deg[2] = ID_num[i];
+                cal_deg( degree[a], &send_deg[7], &send_deg[8] );
+                send_deg[9] = cal_sum( send_deg, 9 );
+                for( int b = 0; b < 10; b ++ )   servo.putc( send_deg[b] );
+                wait_ms( 1000 );
+            }
+        }
+    }
+}
+
+void cal_deg( short degree, short *lower, short *upper )
+{
+    *lower = degree & 0x00ff;
+    *upper = ( degree >> 8 ) & 0x00ff;
+}
+
+void id_set( void )
+{
+    int i, j;
+    short ID[LENGTH][9] = 
+                         {
+                             { 0xfa, 0xaf, 0x01, 0x00, 0x04, 0x01, 0x01, 0x01, 0x00 }    //ID
+                         };
+    
+    for( i = 0; i < LENGTH; i ++ )
+    {
+        ID[i][7] = ID_num[i];
+        ID[i][8] = cal_sum( ID[i], 8 );
+        for( j = 0; j < 9; j ++ )   servo.putc( ID[i][j] );
+        save( ID[i][7] );
+    }
+}
+
+short cal_sum( short *data, int sum_num )
+{
+    int sum = 0, j;
+    
+    for( j = 2; j < sum_num; j ++ )
+    {
+        sum ^= ( int )data[j];
+    }
+    
+    return sum;
+}
+
+void save( short id )
+{
+    int i, j;
+    short save[2][8] =
+                    {
+                        { 0xfa, 0xaf, id, 0x40, 0xff, 0x00, 0x00, 0x00 },   //save
+                        { 0xfa, 0xaf, id, 0x20, 0xff, 0x00, 0x00, 0x00 }    //reboot
+                    };
+    
+    for( i = 0; i < 2; i ++ )
+    {
+        save[i][2] = id;
+        save[i][7] = cal_sum( save[i], 7 );
+        for( j = 0; j < 8; j ++ )   servo.putc( save[i][j] );
+    }
+}
\ No newline at end of file