Shundo Kishi / RS300

Class for Futaba Servo motor RS3xx series

Dependents:   Hobby_Humanoid_controlor

RS300.cpp@2:1ab1adf0915c, 2013-09-25 (annotated)

Committer:
syundo0730
Date:
Wed Sep 25 12:28:31 2013 +0000
Revision:
2:1ab1adf0915c
Parent:
1:64e11d4c49ae 
duplicate includion prohibited

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:6b230fd13b40 1 #include "RS300.h"
syundo0730 0:6b230fd13b40 2
syundo0730 0:6b230fd13b40 3 RS300::RS300(PinName tx, PinName rx) : serial(tx, rx) {
syundo0730 0:6b230fd13b40 4 serial.baud(115200);
syundo0730 0:6b230fd13b40 5 }
syundo0730 0:6b230fd13b40 6
syundo0730 0:6b230fd13b40 7 void RS300::send_packet(uint8_t adr, std::vector<uint8_t> &data, uint8_t cnt, uint8_t id, uint8_t flag)
syundo0730 0:6b230fd13b40 8 {
syundo0730 0:6b230fd13b40 9 std::vector<uint8_t> buf;
syundo0730 0:6b230fd13b40 10 buf.push_back(0xFA);//Header :H
syundo0730 0:6b230fd13b40 11 buf.push_back(0xAF);//Header :L
syundo0730 0:6b230fd13b40 12 buf.push_back(id); //id
syundo0730 0:6b230fd13b40 13 buf.push_back(flag);//flag
syundo0730 0:6b230fd13b40 14 buf.push_back(adr); //adress
syundo0730 0:6b230fd13b40 15 buf.push_back(data.size() / cnt);//data size
syundo0730 0:6b230fd13b40 16
syundo0730 0:6b230fd13b40 17 buf.push_back(cnt);//amount of servo
syundo0730 0:6b230fd13b40 18 buf.insert(buf.end(), data.begin(), data.end());//data
syundo0730 0:6b230fd13b40 19
syundo0730 0:6b230fd13b40 20 uint8_t sum = buf[2];//check sum from id to data
syundo0730 0:6b230fd13b40 21 for (int i = 3, size = buf.size(); i < size; ++i) {
syundo0730 0:6b230fd13b40 22 sum = sum ^ buf[i];
syundo0730 0:6b230fd13b40 23 }
syundo0730 0:6b230fd13b40 24 buf.push_back(sum);
syundo0730 0:6b230fd13b40 25
syundo0730 0:6b230fd13b40 26 for (std::vector<uint8_t>::iterator i = buf.begin(); i != buf.end(); ++i) {
syundo0730 0:6b230fd13b40 27 serial.putc(*i);
syundo0730 0:6b230fd13b40 28 }
syundo0730 0:6b230fd13b40 29 }
syundo0730 0:6b230fd13b40 30
syundo0730 0:6b230fd13b40 31 void RS300::on_all_servo()
syundo0730 0:6b230fd13b40 32 {
syundo0730 0:6b230fd13b40 33 std::vector<uint8_t> dat;
syundo0730 0:6b230fd13b40 34 dat.push_back(0x01);
syundo0730 2:1ab1adf0915c 35 for (uint8_t i = 1; i < 10; ++i) send_packet(0x24, dat, 1, i);//adress, on, servo amount, id(all)
syundo0730 0:6b230fd13b40 36 }
syundo0730 0:6b230fd13b40 37
syundo0730 0:6b230fd13b40 38 void RS300::off_all_servo()
syundo0730 0:6b230fd13b40 39 {
syundo0730 0:6b230fd13b40 40 std::vector<uint8_t> dat;
syundo0730 0:6b230fd13b40 41 dat.push_back(0x00);
syundo0730 0:6b230fd13b40 42 send_packet(0x24, dat, 1, 0xFF);//adress, off, servo amount, id(all)
syundo0730 0:6b230fd13b40 43 }
syundo0730 0:6b230fd13b40 44
syundo0730 0:6b230fd13b40 45 void RS300::send_servo_pos(uint8_t id_s, std::vector<uint16_t> &pos)
syundo0730 0:6b230fd13b40 46 {
syundo0730 0:6b230fd13b40 47 std::vector<uint8_t> buf;
syundo0730 0:6b230fd13b40 48 {
syundo0730 0:6b230fd13b40 49 uint8_t id = id_s;
syundo0730 0:6b230fd13b40 50 std::vector<uint16_t>::iterator i = pos.begin();
syundo0730 0:6b230fd13b40 51 for ( ; i != pos.end(); ++i, ++id) {
syundo0730 0:6b230fd13b40 52 buf.push_back(id);
syundo0730 0:6b230fd13b40 53 buf.push_back((uint8_t)((*i) & 0x00FF));
syundo0730 0:6b230fd13b40 54 buf.push_back((uint8_t)((*i) >> 8));
syundo0730 0:6b230fd13b40 55 }
syundo0730 0:6b230fd13b40 56 }
syundo0730 0:6b230fd13b40 57 send_packet(0x1E, buf, pos.size());
syundo0730 0:6b230fd13b40 58 }