SES / ICSbus

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

ICSbus.cpp

Committer:
SES02
Date:
2016-06-16
Revision:
0:7c7b0148ef4d

File content as of revision 0:7c7b0148ef4d:

#include "mbed.h"
#include "ICSbus.h"
ICSbus::ICSbus(PinName pinTX, PinName pinRX) : 
serial(pinTX, pinRX)
{}

void ICSbus::set_pos(int id, float angle) {
    float pos = 0;
    int data = 0; //3500～11500
    angle += 135;
    pos = (8000 / 270) * angle;
    data = (int)(pos > 0.5 ? pos + 0.5 : pos - 0.5);
    data += 3500;
    
    int CMD = 0b10000000 + id;
    char pos_H = (uint16_t)((data & 0b11111110000000) >> 7);
    char pos_L = (uint16_t)(data & 0b00000001111111);
    
    serial.putc(CMD);
    serial.putc(pos_H);
    serial.putc(pos_L);
    //int rx_CMD = serial.getc(); counter++;
    //char rx_pos_H = serial.getc(); counter++;
    //char rx_pos_L = serial.getc(); counter++;
    
    //while (counter <= 3);
    //counter = 0;
}
    
void ICSbus::set_serial(){
    serial.baud(115200);
    serial.format(8, Serial::Even, 1);
    //pc.baud(115200);
    serial.format(8, Serial::Even, 1);
}