hidaka sato / B3MServo

Library for Kondo B3M Servo Motor

Dependencies:   BufferedSerial

B3MServo.cpp@3:4b96ab3e9e02, 2018-09-04 (annotated)

Committer:
sgrsn
Date:
Tue Sep 04 15:06:58 2018 +0000
Revision:
3:4b96ab3e9e02
Parent:
2:89e3b7b3ca11
Child:
4:5e8fad3a7991
Remove comment.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:d784ce1cfa3d 1 #include "B3MServo.h"
sgrsn 0:d784ce1cfa3d 2
sgrsn 0:d784ce1cfa3d 3 //https://kondo-robot.com/faq/b3m_softwaremanual1200
sgrsn 0:d784ce1cfa3d 4
sgrsn 0:d784ce1cfa3d 5 /*sample program*********************************************
sgrsn 0:d784ce1cfa3d 6
sgrsn 0:d784ce1cfa3d 7 #include "mbed.h"
sgrsn 0:d784ce1cfa3d 8 #include "B3MServo.h"
sgrsn 0:d784ce1cfa3d 9
sgrsn 0:d784ce1cfa3d 10 int main()
sgrsn 0:d784ce1cfa3d 11 {
sgrsn 0:d784ce1cfa3d 12 B3MServo myServo(p13, p14, p12);
sgrsn 0:d784ce1cfa3d 13 myServo.onTorque(0);
sgrsn 0:d784ce1cfa3d 14 while(1)
sgrsn 0:d784ce1cfa3d 15 {
sgrsn 0:d784ce1cfa3d 16 myServo.controlAngle(0, 90);
sgrsn 0:d784ce1cfa3d 17 wait_ms(2500);
sgrsn 0:d784ce1cfa3d 18 myServo.controlAngle(0, -90);
sgrsn 0:d784ce1cfa3d 19 wait_ms(2500);
sgrsn 0:d784ce1cfa3d 20 }
sgrsn 0:d784ce1cfa3d 21 }
sgrsn 0:d784ce1cfa3d 22
sgrsn 0:d784ce1cfa3d 23 ************************************************************/
sgrsn 0:d784ce1cfa3d 24
sgrsn 0:d784ce1cfa3d 25 B3MServo::B3MServo(PinName tx, PinName rx, PinName enable) : BufferedSerial(tx, rx), _enable(enable)
sgrsn 0:d784ce1cfa3d 26 {
sgrsn 0:d784ce1cfa3d 27 BufferedSerial::baud(115200);
sgrsn 2:89e3b7b3ca11 28 //Serial::baud(115200);
sgrsn 0:d784ce1cfa3d 29 }
sgrsn 0:d784ce1cfa3d 30
sgrsn 0:d784ce1cfa3d 31 void B3MServo::sendPacket(char command, char id, char addr, int data, char size, char option, char count)
sgrsn 0:d784ce1cfa3d 32 {
sgrsn 0:d784ce1cfa3d 33 char packet[0x09] = {};
sgrsn 0:d784ce1cfa3d 34
sgrsn 0:d784ce1cfa3d 35 packet[0] = size+7;
sgrsn 0:d784ce1cfa3d 36 packet[1] = command;
sgrsn 0:d784ce1cfa3d 37 packet[2] = option;
sgrsn 0:d784ce1cfa3d 38 packet[3] = id;
sgrsn 0:d784ce1cfa3d 39 for(int i = 0; i < size; i++)
sgrsn 0:d784ce1cfa3d 40 packet[4+i] = char( ( data >> (i*8) ) & 0xFF );
sgrsn 0:d784ce1cfa3d 41 packet[size+4] = addr;
sgrsn 0:d784ce1cfa3d 42 packet[size+5] = count;
sgrsn 0:d784ce1cfa3d 43 packet[size+6] = 0;
sgrsn 0:d784ce1cfa3d 44 for(int i = 0; i < packet[0]-1; i++)
sgrsn 0:d784ce1cfa3d 45 packet[size+6] += packet[i];
sgrsn 0:d784ce1cfa3d 46
sgrsn 2:89e3b7b3ca11 47 _enable = 1;
sgrsn 0:d784ce1cfa3d 48 BufferedSerial::write(packet, packet[0]);
sgrsn 2:89e3b7b3ca11 49 //送信待機時間が必要、短いと送信できないが、長いと受信できない
sgrsn 2:89e3b7b3ca11 50 wait_us(750);
sgrsn 0:d784ce1cfa3d 51 _enable = 0;
sgrsn 2:89e3b7b3ca11 52 wait_us(750);
sgrsn 2:89e3b7b3ca11 53 char re[5] = {};
sgrsn 2:89e3b7b3ca11 54 for(int i = 0; i < 5; i++)
sgrsn 2:89e3b7b3ca11 55 {
sgrsn 2:89e3b7b3ca11 56 re[i] = getc();
sgrsn 2:89e3b7b3ca11 57 }
sgrsn 0:d784ce1cfa3d 58 }
sgrsn 0:d784ce1cfa3d 59
sgrsn 0:d784ce1cfa3d 60 void B3MServo::onTorque(char id, char movingMode, char controlMode)
sgrsn 0:d784ce1cfa3d 61 {
sgrsn 0:d784ce1cfa3d 62 sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
sgrsn 0:d784ce1cfa3d 63 }
sgrsn 0:d784ce1cfa3d 64 void B3MServo::controlAngle(char id, float angle)
sgrsn 0:d784ce1cfa3d 65 {
sgrsn 0:d784ce1cfa3d 66 sendPacket(WRITE, id, DesiredPosition, int(angle*100));
sgrsn 2:89e3b7b3ca11 67 }
sgrsn 2:89e3b7b3ca11 68 void B3MServo::controlTorque(char id, int mNm)
sgrsn 2:89e3b7b3ca11 69 {
sgrsn 2:89e3b7b3ca11 70 onTorque(id, Normal, Current);
sgrsn 2:89e3b7b3ca11 71 sendPacket(WRITE, id, DesiredTorque, mNm);
sgrsn 2:89e3b7b3ca11 72 }
sgrsn 2:89e3b7b3ca11 73
sgrsn 2:89e3b7b3ca11 74 int B3MServo::readPacket(char id, char addr, char length)
sgrsn 2:89e3b7b3ca11 75 {
sgrsn 2:89e3b7b3ca11 76 char packet[0x07] = {};
sgrsn 2:89e3b7b3ca11 77
sgrsn 2:89e3b7b3ca11 78 packet[0] = 0x07;
sgrsn 2:89e3b7b3ca11 79 packet[1] = 0x03;
sgrsn 2:89e3b7b3ca11 80 packet[2] = 0x00;
sgrsn 2:89e3b7b3ca11 81 packet[3] = id;
sgrsn 2:89e3b7b3ca11 82 packet[4] = addr;
sgrsn 2:89e3b7b3ca11 83 packet[5] = length;
sgrsn 2:89e3b7b3ca11 84 packet[6] = 0;
sgrsn 2:89e3b7b3ca11 85 for(int i = 0; i < packet[0]-1; i++)
sgrsn 2:89e3b7b3ca11 86 packet[6] += packet[i];
sgrsn 2:89e3b7b3ca11 87
sgrsn 2:89e3b7b3ca11 88 _enable = 1;
sgrsn 2:89e3b7b3ca11 89 BufferedSerial::write(packet, 7);
sgrsn 2:89e3b7b3ca11 90 wait_us(600);
sgrsn 2:89e3b7b3ca11 91 _enable = 0;
sgrsn 2:89e3b7b3ca11 92 wait_us(1000);
sgrsn 2:89e3b7b3ca11 93 char re[10] = {};
sgrsn 2:89e3b7b3ca11 94 for(int i = 0; i < 5+length; i++)
sgrsn 2:89e3b7b3ca11 95 {
sgrsn 2:89e3b7b3ca11 96 re[i] = getc();
sgrsn 2:89e3b7b3ca11 97 }
sgrsn 2:89e3b7b3ca11 98 char checksum = 0;
sgrsn 2:89e3b7b3ca11 99 for(int i = 0; i < 4+length; i++)
sgrsn 2:89e3b7b3ca11 100 checksum += re[i];
sgrsn 2:89e3b7b3ca11 101 if(checksum != re[4+length])
sgrsn 2:89e3b7b3ca11 102 {
sgrsn 3:4b96ab3e9e02 103 //printf("data error re%d != check%d", re[4+length], checksum);
sgrsn 2:89e3b7b3ca11 104 return;
sgrsn 2:89e3b7b3ca11 105 }
sgrsn 2:89e3b7b3ca11 106
sgrsn 2:89e3b7b3ca11 107 int data = 0;
sgrsn 2:89e3b7b3ca11 108 for(int i = 0; i < length; i++)
sgrsn 2:89e3b7b3ca11 109 {
sgrsn 2:89e3b7b3ca11 110 data |= re[4+i] << (i*8);
sgrsn 2:89e3b7b3ca11 111 }
sgrsn 2:89e3b7b3ca11 112 return data;
sgrsn 2:89e3b7b3ca11 113 }
sgrsn 2:89e3b7b3ca11 114
sgrsn 2:89e3b7b3ca11 115 float B3MServo::getCurrentPosition(char id)
sgrsn 2:89e3b7b3ca11 116 {
sgrsn 2:89e3b7b3ca11 117 return float(readPacket(id, CurrentPosition, 2)) / 100;
sgrsn 2:89e3b7b3ca11 118 }
sgrsn 2:89e3b7b3ca11 119
sgrsn 2:89e3b7b3ca11 120 float B3MServo::getCurrent(char id)
sgrsn 2:89e3b7b3ca11 121 {
sgrsn 2:89e3b7b3ca11 122 return float(readPacket(id, CurrentAmpere, 2));
sgrsn 0:d784ce1cfa3d 123 }