Serial-servo駆動プログラム
Dependencies: def_number mbed
main.cpp
- Committer:
- naoakiarimoto
- Date:
- 2016-11-18
- Revision:
- 0:f1e50cadca05
File content as of revision 0:f1e50cadca05:
//近藤科学製サーボKHR-6003HV用プログラム //ICS3.0対応 //半2重通信(HalfDuplex)使用 //※mbedライブラリのリビジョンを43に戻してコンパイルしてください //RX,TXを2.2kΩでプルアップ、RXに2.2kΩの電流制限をいれる //回路図は近藤科学の「シリアルサーボ制御方法(1) 回路編」 //http://kondo-robot.com/faq/serial-servo-method-tech 参照のこと #include "mbed.h" #include "number.h" #define READ_MODE //#define PRINT #define ID 2 Num n(USBTX,USBRX,3); //Serial pc(USBTX,USBRX); SerialHalfDuplex master(p13,p14); //SerialHalfDuplex master(p9,p10); DigitalOut leds[4]={LED1,LED2,LED3,LED4}; void set_degree(char id, float degree); void setID(char id); void readID(void); int main() { #ifdef READ_MODE char id=0; for(int i = 0;i<4;i++){ leds[i]=0; } leds[0]=1; master.baud(115200); master.format(8,Serial::Even,1); setID((char)id); readID(); for(int j = 1;j<4;j++){ leds[j]=1; wait(0.25); } #endif #ifndef READ_MODE double deg = 0; printf("\r\nRESET\t"); for(int i = 0;i<4;i++){ leds[i]=0; } for(int j = 0;j<4;j++){ leds[j]=1; wait(0.25); } master.baud(115200); master.format(8,Serial::Even,1); printf("deg:0\r\ndeg:"); set_degree(ID,0); while(1){ deg=n.get_number(); printf("\r\ndeg:"); set_degree(ID,deg); wait(0.1); } #endif } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void set_degree(char id, float degree){ char pos_h,pos_l; long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30 pos_h = pos / 128; //上位7bit pos_l = pos % 128; //下位7bit // set position master.putc(0x80+id); // ID master.putc(pos_h); //position(3500-11500 | 0-270deg) master.putc(pos_l); int ret1 = master.getc() - 128; int ret2 = master.getc(); int ret3 = master.getc(); #ifdef PRINT printf("\tret : id=%d, H=%x, L=%x, deg=%d\r\n", ret1, ret2, ret3, ((ret2*128+ret3)-3500)*270/8000); #endif } /* <setID readID> */ /* setID、readID使用時はサーボと1対1で接続すること */ void setID(char id){ master.putc(0xE0+id); master.putc(0x01); master.putc(0x01); master.putc(0x01); } void readID(void){ printf("reading...\t"); master.putc(0xFF); master.putc(0x00); master.putc(0x00); master.putc(0x00); char id = master.getc() - 224; printf("ret : %d\r\n", id); }