ケンタ ミヤザキ / kondoSerialServo

kondoSerialservo krs6003rhv

Dependents:   quad_spider_nhk2019_04

SerialServo.cpp

Committer:
kenken0721
Date:
2019-02-16
Revision:
0:e7ccd59b1ab3
Child:
1:a07940935629

File content as of revision 0:e7ccd59b1ab3:

#include "mbed.h"
#include "SerialServo.h"
//#include "Servo.h"

SerialServo::SerialServo(PinName TX, PinName RX) : ser(TX, RX){
}

//初期化関数
void SerialServo::init(){
    ser.baud(115200);
    ser.format(8, Serial::Even, 1);
    tx_data[0] = 0;
    tx_data[1] = 0;
    tx_data[2] = 0;
}


//データ送信関数
void SerialServo::transmission(){
    ser.putc(tx_data[0]);
    ser.putc(tx_data[1]);
    ser.putc(tx_data[2]);
    wait(0.005);
}

//ポジション設定
void SerialServo::move(char id,int pos){
    int temp1 = (pos & 0x3F80) >> 7;
    int temp2 = pos & 0x7f;
    tx_data[0] = ICS_POS_CMD | id;
    tx_data[1] = (unsigned char)temp1;
    tx_data[2] = (unsigned char)temp2;
    transmission();//送信
}