Serial-servo駆動プログラム
Dependencies: def_number mbed
main.cpp@0:f1e50cadca05, 2016-11-18 (annotated)
- Committer:
- naoakiarimoto
- Date:
- Fri Nov 18 06:20:07 2016 +0000
- Revision:
- 0:f1e50cadca05
serial_servo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
naoakiarimoto | 0:f1e50cadca05 | 1 | //近藤科学製サーボKHR-6003HV用プログラム |
naoakiarimoto | 0:f1e50cadca05 | 2 | //ICS3.0対応 |
naoakiarimoto | 0:f1e50cadca05 | 3 | //半2重通信(HalfDuplex)使用 |
naoakiarimoto | 0:f1e50cadca05 | 4 | //※mbedライブラリのリビジョンを43に戻してコンパイルしてください |
naoakiarimoto | 0:f1e50cadca05 | 5 | //RX,TXを2.2kΩでプルアップ、RXに2.2kΩの電流制限をいれる |
naoakiarimoto | 0:f1e50cadca05 | 6 | //回路図は近藤科学の「シリアルサーボ制御方法(1) 回路編」 |
naoakiarimoto | 0:f1e50cadca05 | 7 | //http://kondo-robot.com/faq/serial-servo-method-tech 参照のこと |
naoakiarimoto | 0:f1e50cadca05 | 8 | #include "mbed.h" |
naoakiarimoto | 0:f1e50cadca05 | 9 | #include "number.h" |
naoakiarimoto | 0:f1e50cadca05 | 10 | |
naoakiarimoto | 0:f1e50cadca05 | 11 | #define READ_MODE |
naoakiarimoto | 0:f1e50cadca05 | 12 | //#define PRINT |
naoakiarimoto | 0:f1e50cadca05 | 13 | #define ID 2 |
naoakiarimoto | 0:f1e50cadca05 | 14 | |
naoakiarimoto | 0:f1e50cadca05 | 15 | Num n(USBTX,USBRX,3); |
naoakiarimoto | 0:f1e50cadca05 | 16 | //Serial pc(USBTX,USBRX); |
naoakiarimoto | 0:f1e50cadca05 | 17 | SerialHalfDuplex master(p13,p14); |
naoakiarimoto | 0:f1e50cadca05 | 18 | //SerialHalfDuplex master(p9,p10); |
naoakiarimoto | 0:f1e50cadca05 | 19 | |
naoakiarimoto | 0:f1e50cadca05 | 20 | DigitalOut leds[4]={LED1,LED2,LED3,LED4}; |
naoakiarimoto | 0:f1e50cadca05 | 21 | void set_degree(char id, float degree); |
naoakiarimoto | 0:f1e50cadca05 | 22 | void setID(char id); |
naoakiarimoto | 0:f1e50cadca05 | 23 | void readID(void); |
naoakiarimoto | 0:f1e50cadca05 | 24 | |
naoakiarimoto | 0:f1e50cadca05 | 25 | |
naoakiarimoto | 0:f1e50cadca05 | 26 | int main() { |
naoakiarimoto | 0:f1e50cadca05 | 27 | #ifdef READ_MODE |
naoakiarimoto | 0:f1e50cadca05 | 28 | char id=0; |
naoakiarimoto | 0:f1e50cadca05 | 29 | for(int i = 0;i<4;i++){ |
naoakiarimoto | 0:f1e50cadca05 | 30 | leds[i]=0; |
naoakiarimoto | 0:f1e50cadca05 | 31 | } |
naoakiarimoto | 0:f1e50cadca05 | 32 | leds[0]=1; |
naoakiarimoto | 0:f1e50cadca05 | 33 | |
naoakiarimoto | 0:f1e50cadca05 | 34 | master.baud(115200); |
naoakiarimoto | 0:f1e50cadca05 | 35 | master.format(8,Serial::Even,1); |
naoakiarimoto | 0:f1e50cadca05 | 36 | setID((char)id); |
naoakiarimoto | 0:f1e50cadca05 | 37 | readID(); |
naoakiarimoto | 0:f1e50cadca05 | 38 | |
naoakiarimoto | 0:f1e50cadca05 | 39 | for(int j = 1;j<4;j++){ |
naoakiarimoto | 0:f1e50cadca05 | 40 | leds[j]=1; |
naoakiarimoto | 0:f1e50cadca05 | 41 | wait(0.25); |
naoakiarimoto | 0:f1e50cadca05 | 42 | } |
naoakiarimoto | 0:f1e50cadca05 | 43 | #endif |
naoakiarimoto | 0:f1e50cadca05 | 44 | #ifndef READ_MODE |
naoakiarimoto | 0:f1e50cadca05 | 45 | double deg = 0; |
naoakiarimoto | 0:f1e50cadca05 | 46 | printf("\r\nRESET\t"); |
naoakiarimoto | 0:f1e50cadca05 | 47 | |
naoakiarimoto | 0:f1e50cadca05 | 48 | for(int i = 0;i<4;i++){ |
naoakiarimoto | 0:f1e50cadca05 | 49 | leds[i]=0; |
naoakiarimoto | 0:f1e50cadca05 | 50 | } |
naoakiarimoto | 0:f1e50cadca05 | 51 | for(int j = 0;j<4;j++){ |
naoakiarimoto | 0:f1e50cadca05 | 52 | leds[j]=1; |
naoakiarimoto | 0:f1e50cadca05 | 53 | wait(0.25); |
naoakiarimoto | 0:f1e50cadca05 | 54 | } |
naoakiarimoto | 0:f1e50cadca05 | 55 | |
naoakiarimoto | 0:f1e50cadca05 | 56 | master.baud(115200); |
naoakiarimoto | 0:f1e50cadca05 | 57 | master.format(8,Serial::Even,1); |
naoakiarimoto | 0:f1e50cadca05 | 58 | |
naoakiarimoto | 0:f1e50cadca05 | 59 | printf("deg:0\r\ndeg:"); |
naoakiarimoto | 0:f1e50cadca05 | 60 | set_degree(ID,0); |
naoakiarimoto | 0:f1e50cadca05 | 61 | while(1){ |
naoakiarimoto | 0:f1e50cadca05 | 62 | deg=n.get_number(); |
naoakiarimoto | 0:f1e50cadca05 | 63 | printf("\r\ndeg:"); |
naoakiarimoto | 0:f1e50cadca05 | 64 | set_degree(ID,deg); |
naoakiarimoto | 0:f1e50cadca05 | 65 | wait(0.1); |
naoakiarimoto | 0:f1e50cadca05 | 66 | } |
naoakiarimoto | 0:f1e50cadca05 | 67 | #endif |
naoakiarimoto | 0:f1e50cadca05 | 68 | } |
naoakiarimoto | 0:f1e50cadca05 | 69 | |
naoakiarimoto | 0:f1e50cadca05 | 70 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
naoakiarimoto | 0:f1e50cadca05 | 71 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
naoakiarimoto | 0:f1e50cadca05 | 72 | |
naoakiarimoto | 0:f1e50cadca05 | 73 | void set_degree(char id, float degree){ |
naoakiarimoto | 0:f1e50cadca05 | 74 | char pos_h,pos_l; |
naoakiarimoto | 0:f1e50cadca05 | 75 | long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30 |
naoakiarimoto | 0:f1e50cadca05 | 76 | pos_h = pos / 128; //上位7bit |
naoakiarimoto | 0:f1e50cadca05 | 77 | pos_l = pos % 128; //下位7bit |
naoakiarimoto | 0:f1e50cadca05 | 78 | // set position |
naoakiarimoto | 0:f1e50cadca05 | 79 | master.putc(0x80+id); // ID |
naoakiarimoto | 0:f1e50cadca05 | 80 | master.putc(pos_h); //position(3500-11500 | 0-270deg) |
naoakiarimoto | 0:f1e50cadca05 | 81 | master.putc(pos_l); |
naoakiarimoto | 0:f1e50cadca05 | 82 | int ret1 = master.getc() - 128; |
naoakiarimoto | 0:f1e50cadca05 | 83 | int ret2 = master.getc(); |
naoakiarimoto | 0:f1e50cadca05 | 84 | int ret3 = master.getc(); |
naoakiarimoto | 0:f1e50cadca05 | 85 | #ifdef PRINT |
naoakiarimoto | 0:f1e50cadca05 | 86 | printf("\tret : id=%d, H=%x, L=%x, deg=%d\r\n", ret1, ret2, ret3, ((ret2*128+ret3)-3500)*270/8000); |
naoakiarimoto | 0:f1e50cadca05 | 87 | #endif |
naoakiarimoto | 0:f1e50cadca05 | 88 | } |
naoakiarimoto | 0:f1e50cadca05 | 89 | /* <setID readID> */ |
naoakiarimoto | 0:f1e50cadca05 | 90 | /* setID、readID使用時はサーボと1対1で接続すること */ |
naoakiarimoto | 0:f1e50cadca05 | 91 | void setID(char id){ |
naoakiarimoto | 0:f1e50cadca05 | 92 | master.putc(0xE0+id); |
naoakiarimoto | 0:f1e50cadca05 | 93 | master.putc(0x01); |
naoakiarimoto | 0:f1e50cadca05 | 94 | master.putc(0x01); |
naoakiarimoto | 0:f1e50cadca05 | 95 | master.putc(0x01); |
naoakiarimoto | 0:f1e50cadca05 | 96 | } |
naoakiarimoto | 0:f1e50cadca05 | 97 | |
naoakiarimoto | 0:f1e50cadca05 | 98 | void readID(void){ |
naoakiarimoto | 0:f1e50cadca05 | 99 | printf("reading...\t"); |
naoakiarimoto | 0:f1e50cadca05 | 100 | master.putc(0xFF); |
naoakiarimoto | 0:f1e50cadca05 | 101 | master.putc(0x00); |
naoakiarimoto | 0:f1e50cadca05 | 102 | master.putc(0x00); |
naoakiarimoto | 0:f1e50cadca05 | 103 | master.putc(0x00); |
naoakiarimoto | 0:f1e50cadca05 | 104 | char id = master.getc() - 224; |
naoakiarimoto | 0:f1e50cadca05 | 105 | printf("ret : %d\r\n", id); |
naoakiarimoto | 0:f1e50cadca05 | 106 | } |