ケンタ ミヤザキ / kondoSerialServo

kondoSerialservo krs6003rhv

Dependents:   quad_spider_nhk2019_04

SerialServo.cpp@0:e7ccd59b1ab3, 2019-02-16 (annotated)

Committer:
kenken0721
Date:
Sat Feb 16 08:23:51 2019 +0000
Revision:
0:e7ccd59b1ab3
Child:
1:a07940935629
kondo serialservo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:e7ccd59b1ab3 1 #include "mbed.h"
kenken0721 0:e7ccd59b1ab3 2 #include "SerialServo.h"
kenken0721 0:e7ccd59b1ab3 3 //#include "Servo.h"
kenken0721 0:e7ccd59b1ab3 4
kenken0721 0:e7ccd59b1ab3 5 SerialServo::SerialServo(PinName TX, PinName RX) : ser(TX, RX){
kenken0721 0:e7ccd59b1ab3 6 }
kenken0721 0:e7ccd59b1ab3 7
kenken0721 0:e7ccd59b1ab3 8 //初期化関数
kenken0721 0:e7ccd59b1ab3 9 void SerialServo::init(){
kenken0721 0:e7ccd59b1ab3 10 ser.baud(115200);
kenken0721 0:e7ccd59b1ab3 11 ser.format(8, Serial::Even, 1);
kenken0721 0:e7ccd59b1ab3 12 tx_data[0] = 0;
kenken0721 0:e7ccd59b1ab3 13 tx_data[1] = 0;
kenken0721 0:e7ccd59b1ab3 14 tx_data[2] = 0;
kenken0721 0:e7ccd59b1ab3 15 }
kenken0721 0:e7ccd59b1ab3 16
kenken0721 0:e7ccd59b1ab3 17
kenken0721 0:e7ccd59b1ab3 18 //データ送信関数
kenken0721 0:e7ccd59b1ab3 19 void SerialServo::transmission(){
kenken0721 0:e7ccd59b1ab3 20 ser.putc(tx_data[0]);
kenken0721 0:e7ccd59b1ab3 21 ser.putc(tx_data[1]);
kenken0721 0:e7ccd59b1ab3 22 ser.putc(tx_data[2]);
kenken0721 0:e7ccd59b1ab3 23 wait(0.005);
kenken0721 0:e7ccd59b1ab3 24 }
kenken0721 0:e7ccd59b1ab3 25
kenken0721 0:e7ccd59b1ab3 26 //ポジション設定
kenken0721 0:e7ccd59b1ab3 27 void SerialServo::move(char id,int pos){
kenken0721 0:e7ccd59b1ab3 28 int temp1 = (pos & 0x3F80) >> 7;
kenken0721 0:e7ccd59b1ab3 29 int temp2 = pos & 0x7f;
kenken0721 0:e7ccd59b1ab3 30 tx_data[0] = ICS_POS_CMD | id;
kenken0721 0:e7ccd59b1ab3 31 tx_data[1] = (unsigned char)temp1;
kenken0721 0:e7ccd59b1ab3 32 tx_data[2] = (unsigned char)temp2;
kenken0721 0:e7ccd59b1ab3 33 transmission();//送信
kenken0721 0:e7ccd59b1ab3 34 }
kenken0721 0:e7ccd59b1ab3 35