新部内対抗５班
Serial-servo駆動プログラム
Dependencies: def_number mbed
Revision 0:f1e50cadca05, committed 2016-11-18
- Comitter:
- naoakiarimoto
- Date:
- Fri Nov 18 06:20:07 2016 +0000
- Commit message:
- serial_servo
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/def_number.lib Fri Nov 18 06:20:07 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/naoakiarimoto/code/def_number/#9b2cd42228b6
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 18 06:20:07 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ed12d17f06 \ No newline at end of file