新部内対抗５班 / Mbed 2 deprecated _Serial-servo_mbed

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?

UserRevisionLine numberNew 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 //回路図は近藤科学の「シリアルサーボ制御方法（１） 回路編」
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 }