![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Serial-servo駆動プログラム
Dependencies: def_number mbed
Diff: main.cpp
- Revision:
- 0:f1e50cadca05
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 18 06:20:07 2016 +0000 @@ -0,0 +1,106 @@ +//近藤科学製サーボ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); +} \ No newline at end of file