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Ωの電流制限をいれる
+//回路図は近藤科学の「シリアルサーボ制御方法（１） 回路編」
+//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