Keisuke Fujii
Dependencies: mbed Motordriver QEI
servo.h@1:19d647492936, 2011-11-10 (annotated)
- Committer:
- Keisuke_Fujii
- Date:
- Thu Nov 10 13:29:43 2011 +0000
- Revision:
- 1:19d647492936
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Keisuke_Fujii | 1:19d647492936 | 1 | void control_servo( void ) |
| Keisuke_Fujii | 1:19d647492936 | 2 | { |
| Keisuke_Fujii | 1:19d647492936 | 3 | short torque[9] = { 0xfa, 0xaf, 0x00, 0x00, 0x24, 0x01, 0x01, 0x01, 0x00 }; |
| Keisuke_Fujii | 1:19d647492936 | 4 | short send_deg[10] = { 0xfa, 0xaf, 0x00, 0x00, 0x1e, 0x02, 0x01, 0x01, 0x00, 0x00 }; |
| Keisuke_Fujii | 1:19d647492936 | 5 | // Hdr, Hdr, ID, Flg, Adr, Len, Cnt,Lower,Upper, Sum |
| Keisuke_Fujii | 1:19d647492936 | 6 | |
| Keisuke_Fujii | 1:19d647492936 | 7 | for( int a = 0; a < LENGTH; a ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 8 | { |
| Keisuke_Fujii | 1:19d647492936 | 9 | torque[2] = ID_num[a]; |
| Keisuke_Fujii | 1:19d647492936 | 10 | torque[8] = cal_sum( torque, 8 ); |
| Keisuke_Fujii | 1:19d647492936 | 11 | for( int b = 0; b < 9; b ++ ) servo.putc( torque[b] ); |
| Keisuke_Fujii | 1:19d647492936 | 12 | wait_ms( 5 ); |
| Keisuke_Fujii | 1:19d647492936 | 13 | } |
| Keisuke_Fujii | 1:19d647492936 | 14 | |
| Keisuke_Fujii | 1:19d647492936 | 15 | while( 1 ) |
| Keisuke_Fujii | 1:19d647492936 | 16 | { |
| Keisuke_Fujii | 1:19d647492936 | 17 | for( int a = 0; a < LENGTH; a ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 18 | { |
| Keisuke_Fujii | 1:19d647492936 | 19 | for( int i = 0; i < LENGTH; i ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 20 | { |
| Keisuke_Fujii | 1:19d647492936 | 21 | send_deg[2] = ID_num[i]; |
| Keisuke_Fujii | 1:19d647492936 | 22 | cal_deg( degree[a], &send_deg[7], &send_deg[8] ); |
| Keisuke_Fujii | 1:19d647492936 | 23 | send_deg[9] = cal_sum( send_deg, 9 ); |
| Keisuke_Fujii | 1:19d647492936 | 24 | for( int b = 0; b < 10; b ++ ) servo.putc( send_deg[b] ); |
| Keisuke_Fujii | 1:19d647492936 | 25 | wait_ms( 1000 ); |
| Keisuke_Fujii | 1:19d647492936 | 26 | } |
| Keisuke_Fujii | 1:19d647492936 | 27 | } |
| Keisuke_Fujii | 1:19d647492936 | 28 | } |
| Keisuke_Fujii | 1:19d647492936 | 29 | } |
| Keisuke_Fujii | 1:19d647492936 | 30 | |
| Keisuke_Fujii | 1:19d647492936 | 31 | void cal_deg( short degree, short *lower, short *upper ) |
| Keisuke_Fujii | 1:19d647492936 | 32 | { |
| Keisuke_Fujii | 1:19d647492936 | 33 | *lower = degree & 0x00ff; |
| Keisuke_Fujii | 1:19d647492936 | 34 | *upper = ( degree >> 8 ) & 0x00ff; |
| Keisuke_Fujii | 1:19d647492936 | 35 | } |
| Keisuke_Fujii | 1:19d647492936 | 36 | |
| Keisuke_Fujii | 1:19d647492936 | 37 | void id_set( void ) |
| Keisuke_Fujii | 1:19d647492936 | 38 | { |
| Keisuke_Fujii | 1:19d647492936 | 39 | int i, j; |
| Keisuke_Fujii | 1:19d647492936 | 40 | short ID[LENGTH][9] = |
| Keisuke_Fujii | 1:19d647492936 | 41 | { |
| Keisuke_Fujii | 1:19d647492936 | 42 | { 0xfa, 0xaf, 0x01, 0x00, 0x04, 0x01, 0x01, 0x01, 0x00 } //ID |
| Keisuke_Fujii | 1:19d647492936 | 43 | }; |
| Keisuke_Fujii | 1:19d647492936 | 44 | |
| Keisuke_Fujii | 1:19d647492936 | 45 | for( i = 0; i < LENGTH; i ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 46 | { |
| Keisuke_Fujii | 1:19d647492936 | 47 | ID[i][7] = ID_num[i]; |
| Keisuke_Fujii | 1:19d647492936 | 48 | ID[i][8] = cal_sum( ID[i], 8 ); |
| Keisuke_Fujii | 1:19d647492936 | 49 | for( j = 0; j < 9; j ++ ) servo.putc( ID[i][j] ); |
| Keisuke_Fujii | 1:19d647492936 | 50 | save( ID[i][7] ); |
| Keisuke_Fujii | 1:19d647492936 | 51 | } |
| Keisuke_Fujii | 1:19d647492936 | 52 | } |
| Keisuke_Fujii | 1:19d647492936 | 53 | |
| Keisuke_Fujii | 1:19d647492936 | 54 | short cal_sum( short *data, int sum_num ) |
| Keisuke_Fujii | 1:19d647492936 | 55 | { |
| Keisuke_Fujii | 1:19d647492936 | 56 | int sum = 0, j; |
| Keisuke_Fujii | 1:19d647492936 | 57 | |
| Keisuke_Fujii | 1:19d647492936 | 58 | for( j = 2; j < sum_num; j ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 59 | { |
| Keisuke_Fujii | 1:19d647492936 | 60 | sum ^= ( int )data[j]; |
| Keisuke_Fujii | 1:19d647492936 | 61 | } |
| Keisuke_Fujii | 1:19d647492936 | 62 | |
| Keisuke_Fujii | 1:19d647492936 | 63 | return sum; |
| Keisuke_Fujii | 1:19d647492936 | 64 | } |
| Keisuke_Fujii | 1:19d647492936 | 65 | |
| Keisuke_Fujii | 1:19d647492936 | 66 | void save( short id ) |
| Keisuke_Fujii | 1:19d647492936 | 67 | { |
| Keisuke_Fujii | 1:19d647492936 | 68 | int i, j; |
| Keisuke_Fujii | 1:19d647492936 | 69 | short save[2][8] = |
| Keisuke_Fujii | 1:19d647492936 | 70 | { |
| Keisuke_Fujii | 1:19d647492936 | 71 | { 0xfa, 0xaf, id, 0x40, 0xff, 0x00, 0x00, 0x00 }, //save |
| Keisuke_Fujii | 1:19d647492936 | 72 | { 0xfa, 0xaf, id, 0x20, 0xff, 0x00, 0x00, 0x00 } //reboot |
| Keisuke_Fujii | 1:19d647492936 | 73 | }; |
| Keisuke_Fujii | 1:19d647492936 | 74 | |
| Keisuke_Fujii | 1:19d647492936 | 75 | for( i = 0; i < 2; i ++ ) |
| Keisuke_Fujii | 1:19d647492936 | 76 | { |
| Keisuke_Fujii | 1:19d647492936 | 77 | save[i][2] = id; |
| Keisuke_Fujii | 1:19d647492936 | 78 | save[i][7] = cal_sum( save[i], 7 ); |
| Keisuke_Fujii | 1:19d647492936 | 79 | for( j = 0; j < 8; j ++ ) servo.putc( save[i][j] ); |
| Keisuke_Fujii | 1:19d647492936 | 80 | } |
| Keisuke_Fujii | 1:19d647492936 | 81 | } |