SES / ICSbus

サーボのライブラリ. サーボのID, ポジションを指定してシリアル通信でサーボを動かす.

ICSbus.cpp@0:7c7b0148ef4d, 2016-06-16 (annotated)

Committer:
SES02
Date:
Thu Jun 16 11:29:28 2016 +0000
Revision:
0:7c7b0148ef4d
ver 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SES02 0:7c7b0148ef4d 1 #include "mbed.h"
SES02 0:7c7b0148ef4d 2 #include "ICSbus.h"
SES02 0:7c7b0148ef4d 3 ICSbus::ICSbus(PinName pinTX, PinName pinRX) :
SES02 0:7c7b0148ef4d 4 serial(pinTX, pinRX)
SES02 0:7c7b0148ef4d 5 {}
SES02 0:7c7b0148ef4d 6
SES02 0:7c7b0148ef4d 7 void ICSbus::set_pos(int id, float angle) {
SES02 0:7c7b0148ef4d 8 float pos = 0;
SES02 0:7c7b0148ef4d 9 int data = 0; //3500～11500
SES02 0:7c7b0148ef4d 10 angle += 135;
SES02 0:7c7b0148ef4d 11 pos = (8000 / 270) * angle;
SES02 0:7c7b0148ef4d 12 data = (int)(pos > 0.5 ? pos + 0.5 : pos - 0.5);
SES02 0:7c7b0148ef4d 13 data += 3500;
SES02 0:7c7b0148ef4d 14
SES02 0:7c7b0148ef4d 15 int CMD = 0b10000000 + id;
SES02 0:7c7b0148ef4d 16 char pos_H = (uint16_t)((data & 0b11111110000000) >> 7);
SES02 0:7c7b0148ef4d 17 char pos_L = (uint16_t)(data & 0b00000001111111);
SES02 0:7c7b0148ef4d 18
SES02 0:7c7b0148ef4d 19 serial.putc(CMD);
SES02 0:7c7b0148ef4d 20 serial.putc(pos_H);
SES02 0:7c7b0148ef4d 21 serial.putc(pos_L);
SES02 0:7c7b0148ef4d 22 //int rx_CMD = serial.getc(); counter++;
SES02 0:7c7b0148ef4d 23 //char rx_pos_H = serial.getc(); counter++;
SES02 0:7c7b0148ef4d 24 //char rx_pos_L = serial.getc(); counter++;
SES02 0:7c7b0148ef4d 25
SES02 0:7c7b0148ef4d 26 //while (counter <= 3);
SES02 0:7c7b0148ef4d 27 //counter = 0;
SES02 0:7c7b0148ef4d 28 }
SES02 0:7c7b0148ef4d 29
SES02 0:7c7b0148ef4d 30 void ICSbus::set_serial(){
SES02 0:7c7b0148ef4d 31 serial.baud(115200);
SES02 0:7c7b0148ef4d 32 serial.format(8, Serial::Even, 1);
SES02 0:7c7b0148ef4d 33 //pc.baud(115200);
SES02 0:7c7b0148ef4d 34 serial.format(8, Serial::Even, 1);
SES02 0:7c7b0148ef4d 35 }