hidaka sato / B3MServo

Library for Kondo B3M Servo Motor

Dependencies:   BufferedSerial

B3MServo.cpp@4:5e8fad3a7991, 2018-09-07 (annotated)

Committer:
sgrsn
Date:
Fri Sep 07 01:52:50 2018 +0000
Revision:
4:5e8fad3a7991
Parent:
3:4b96ab3e9e02
Child:
5:3ed0a70f697b
Child:
6:fe45a1f21522
Add control_mode Value.; do not have to use onTorque() func.

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 4:5e8fad3a7991 28 control_mode = 0xFF;
sgrsn 2:89e3b7b3ca11 29 //Serial::baud(115200);
sgrsn 0:d784ce1cfa3d 30 }
sgrsn 0:d784ce1cfa3d 31
sgrsn 0:d784ce1cfa3d 32 void B3MServo::sendPacket(char command, char id, char addr, int data, char size, char option, char count)
sgrsn 0:d784ce1cfa3d 33 {
sgrsn 0:d784ce1cfa3d 34 char packet[0x09] = {};
sgrsn 0:d784ce1cfa3d 35
sgrsn 0:d784ce1cfa3d 36 packet[0] = size+7;
sgrsn 0:d784ce1cfa3d 37 packet[1] = command;
sgrsn 0:d784ce1cfa3d 38 packet[2] = option;
sgrsn 0:d784ce1cfa3d 39 packet[3] = id;
sgrsn 0:d784ce1cfa3d 40 for(int i = 0; i < size; i++)
sgrsn 0:d784ce1cfa3d 41 packet[4+i] = char( ( data >> (i*8) ) & 0xFF );
sgrsn 0:d784ce1cfa3d 42 packet[size+4] = addr;
sgrsn 0:d784ce1cfa3d 43 packet[size+5] = count;
sgrsn 0:d784ce1cfa3d 44 packet[size+6] = 0;
sgrsn 0:d784ce1cfa3d 45 for(int i = 0; i < packet[0]-1; i++)
sgrsn 0:d784ce1cfa3d 46 packet[size+6] += packet[i];
sgrsn 0:d784ce1cfa3d 47
sgrsn 2:89e3b7b3ca11 48 _enable = 1;
sgrsn 0:d784ce1cfa3d 49 BufferedSerial::write(packet, packet[0]);
sgrsn 2:89e3b7b3ca11 50 //送信待機時間が必要、短いと送信できないが、長いと受信できない
sgrsn 2:89e3b7b3ca11 51 wait_us(750);
sgrsn 4:5e8fad3a7991 52 /*_enable = 0;
sgrsn 2:89e3b7b3ca11 53 wait_us(750);
sgrsn 2:89e3b7b3ca11 54 char re[5] = {};
sgrsn 2:89e3b7b3ca11 55 for(int i = 0; i < 5; i++)
sgrsn 2:89e3b7b3ca11 56 {
sgrsn 2:89e3b7b3ca11 57 re[i] = getc();
sgrsn 4:5e8fad3a7991 58 }*/
sgrsn 0:d784ce1cfa3d 59 }
sgrsn 0:d784ce1cfa3d 60
sgrsn 4:5e8fad3a7991 61 void B3MServo::sendPacket(char *id, char n, int *data, char size, char addr, char option)
sgrsn 4:5e8fad3a7991 62 {
sgrsn 4:5e8fad3a7991 63 char packet[20] = {};
sgrsn 4:5e8fad3a7991 64
sgrsn 4:5e8fad3a7991 65 packet[0] = 6 + (1+size) * n;
sgrsn 4:5e8fad3a7991 66 packet[1] = WRITE;
sgrsn 4:5e8fad3a7991 67 packet[2] = option;
sgrsn 4:5e8fad3a7991 68
sgrsn 4:5e8fad3a7991 69 for(int i = 0; i < n; i++)
sgrsn 4:5e8fad3a7991 70 {
sgrsn 4:5e8fad3a7991 71 packet[3+(1+size)*i] = id[i];
sgrsn 4:5e8fad3a7991 72 for(int j = 0; j < size; j++)
sgrsn 4:5e8fad3a7991 73 {
sgrsn 4:5e8fad3a7991 74 packet[3+(1+size)*i + 1+j] = (data[i] >> (j*8)) & 0xFF;
sgrsn 4:5e8fad3a7991 75 }
sgrsn 4:5e8fad3a7991 76 }
sgrsn 4:5e8fad3a7991 77 packet[3+(1+size)*n] = addr;
sgrsn 4:5e8fad3a7991 78 packet[4+(1+size)*n] = n;
sgrsn 4:5e8fad3a7991 79 packet[5+(1+size)*n] = 0x00;
sgrsn 4:5e8fad3a7991 80 for(int i = 0; i < packet[0]-1; i++)
sgrsn 4:5e8fad3a7991 81 packet[5+(1+size)*n] += packet[i];
sgrsn 4:5e8fad3a7991 82 _enable = 1;
sgrsn 4:5e8fad3a7991 83 BufferedSerial::write(packet, packet[0]);
sgrsn 4:5e8fad3a7991 84 wait_us(750);
sgrsn 4:5e8fad3a7991 85 }
sgrsn 0:d784ce1cfa3d 86 void B3MServo::onTorque(char id, char movingMode, char controlMode)
sgrsn 0:d784ce1cfa3d 87 {
sgrsn 0:d784ce1cfa3d 88 sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
sgrsn 0:d784ce1cfa3d 89 }
sgrsn 0:d784ce1cfa3d 90 void B3MServo::controlAngle(char id, float angle)
sgrsn 0:d784ce1cfa3d 91 {
sgrsn 4:5e8fad3a7991 92 if(control_mode == Position)
sgrsn 4:5e8fad3a7991 93 {
sgrsn 4:5e8fad3a7991 94 sendPacket(WRITE, id, DesiredPosition, int(angle*100));
sgrsn 4:5e8fad3a7991 95 }
sgrsn 4:5e8fad3a7991 96 else
sgrsn 4:5e8fad3a7991 97 {
sgrsn 4:5e8fad3a7991 98 control_mode = Position;
sgrsn 4:5e8fad3a7991 99 onTorque(id, Normal, Position);
sgrsn 4:5e8fad3a7991 100 wait_us(600);
sgrsn 4:5e8fad3a7991 101 controlAngle(id, angle);
sgrsn 4:5e8fad3a7991 102 }
sgrsn 4:5e8fad3a7991 103 }
sgrsn 4:5e8fad3a7991 104 void B3MServo::controlAngle(char *id, char n, float *angle)
sgrsn 4:5e8fad3a7991 105 {
sgrsn 4:5e8fad3a7991 106 int data[n];
sgrsn 4:5e8fad3a7991 107 for(int i = 0; i < n; i++) data[i] = int(angle[i]*100);
sgrsn 4:5e8fad3a7991 108 sendPacket(id, n, data, 2, DesiredPosition);
sgrsn 2:89e3b7b3ca11 109 }
sgrsn 2:89e3b7b3ca11 110 void B3MServo::controlTorque(char id, int mNm)
sgrsn 2:89e3b7b3ca11 111 {
sgrsn 4:5e8fad3a7991 112 if(control_mode == Current)
sgrsn 4:5e8fad3a7991 113 {
sgrsn 4:5e8fad3a7991 114 sendPacket(WRITE, id, DesiredTorque, mNm);
sgrsn 4:5e8fad3a7991 115 }
sgrsn 4:5e8fad3a7991 116 else
sgrsn 4:5e8fad3a7991 117 {
sgrsn 4:5e8fad3a7991 118 control_mode = Current;
sgrsn 4:5e8fad3a7991 119 onTorque(id, Normal, Current);
sgrsn 4:5e8fad3a7991 120 wait_us(600);
sgrsn 4:5e8fad3a7991 121 controlTorque(id, mNm);
sgrsn 4:5e8fad3a7991 122 }
sgrsn 2:89e3b7b3ca11 123 }
sgrsn 2:89e3b7b3ca11 124
sgrsn 2:89e3b7b3ca11 125 int B3MServo::readPacket(char id, char addr, char length)
sgrsn 2:89e3b7b3ca11 126 {
sgrsn 2:89e3b7b3ca11 127 char packet[0x07] = {};
sgrsn 2:89e3b7b3ca11 128
sgrsn 2:89e3b7b3ca11 129 packet[0] = 0x07;
sgrsn 2:89e3b7b3ca11 130 packet[1] = 0x03;
sgrsn 2:89e3b7b3ca11 131 packet[2] = 0x00;
sgrsn 2:89e3b7b3ca11 132 packet[3] = id;
sgrsn 2:89e3b7b3ca11 133 packet[4] = addr;
sgrsn 2:89e3b7b3ca11 134 packet[5] = length;
sgrsn 2:89e3b7b3ca11 135 packet[6] = 0;
sgrsn 2:89e3b7b3ca11 136 for(int i = 0; i < packet[0]-1; i++)
sgrsn 2:89e3b7b3ca11 137 packet[6] += packet[i];
sgrsn 2:89e3b7b3ca11 138
sgrsn 2:89e3b7b3ca11 139 _enable = 1;
sgrsn 2:89e3b7b3ca11 140 BufferedSerial::write(packet, 7);
sgrsn 2:89e3b7b3ca11 141 wait_us(600);
sgrsn 2:89e3b7b3ca11 142 _enable = 0;
sgrsn 2:89e3b7b3ca11 143 wait_us(1000);
sgrsn 2:89e3b7b3ca11 144 char re[10] = {};
sgrsn 2:89e3b7b3ca11 145 for(int i = 0; i < 5+length; i++)
sgrsn 2:89e3b7b3ca11 146 {
sgrsn 2:89e3b7b3ca11 147 re[i] = getc();
sgrsn 2:89e3b7b3ca11 148 }
sgrsn 2:89e3b7b3ca11 149 char checksum = 0;
sgrsn 2:89e3b7b3ca11 150 for(int i = 0; i < 4+length; i++)
sgrsn 2:89e3b7b3ca11 151 checksum += re[i];
sgrsn 2:89e3b7b3ca11 152 if(checksum != re[4+length])
sgrsn 2:89e3b7b3ca11 153 {
sgrsn 3:4b96ab3e9e02 154 //printf("data error re%d != check%d", re[4+length], checksum);
sgrsn 2:89e3b7b3ca11 155 return;
sgrsn 2:89e3b7b3ca11 156 }
sgrsn 2:89e3b7b3ca11 157
sgrsn 2:89e3b7b3ca11 158 int data = 0;
sgrsn 2:89e3b7b3ca11 159 for(int i = 0; i < length; i++)
sgrsn 2:89e3b7b3ca11 160 {
sgrsn 2:89e3b7b3ca11 161 data |= re[4+i] << (i*8);
sgrsn 2:89e3b7b3ca11 162 }
sgrsn 2:89e3b7b3ca11 163 return data;
sgrsn 2:89e3b7b3ca11 164 }
sgrsn 2:89e3b7b3ca11 165
sgrsn 2:89e3b7b3ca11 166 float B3MServo::getCurrentPosition(char id)
sgrsn 2:89e3b7b3ca11 167 {
sgrsn 2:89e3b7b3ca11 168 return float(readPacket(id, CurrentPosition, 2)) / 100;
sgrsn 2:89e3b7b3ca11 169 }
sgrsn 2:89e3b7b3ca11 170
sgrsn 2:89e3b7b3ca11 171 float B3MServo::getCurrent(char id)
sgrsn 2:89e3b7b3ca11 172 {
sgrsn 2:89e3b7b3ca11 173 return float(readPacket(id, CurrentAmpere, 2));
sgrsn 0:d784ce1cfa3d 174 }