Titouan Soulard / MX12

MX-12W Servo Library

Dependents:   DISCO_L475VG_IOT01-Sensors-BSP

MX12.cpp@0:5e5d94ac9c80, 2020-12-10 (annotated)

Committer:
tsoul
Date:
Thu Dec 10 21:51:18 2020 +0000
Revision:
0:5e5d94ac9c80
Child:
1:d0c3878f77ee
First working version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tsoul 0:5e5d94ac9c80 1 #include "MX12.h"
tsoul 0:5e5d94ac9c80 2
tsoul 0:5e5d94ac9c80 3 MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
tsoul 0:5e5d94ac9c80 4 _baud = baud;
tsoul 0:5e5d94ac9c80 5 _mx12.baud(_baud);
tsoul 0:5e5d94ac9c80 6 _mx12.format(8, SerialBase::None, 1);
tsoul 0:5e5d94ac9c80 7 }
tsoul 0:5e5d94ac9c80 8
tsoul 0:5e5d94ac9c80 9 void MX12::SetSpeed(unsigned char mot_id, float speed) {
tsoul 0:5e5d94ac9c80 10 char data[2];
tsoul 0:5e5d94ac9c80 11
tsoul 0:5e5d94ac9c80 12 // Speed absolute value
tsoul 0:5e5d94ac9c80 13 int goal = (0x3ff * abs(speed));
tsoul 0:5e5d94ac9c80 14
tsoul 0:5e5d94ac9c80 15 // Spin direction (CW is negative)
tsoul 0:5e5d94ac9c80 16 if (speed < 0) {
tsoul 0:5e5d94ac9c80 17 goal |= (0x1 << 10);
tsoul 0:5e5d94ac9c80 18 }
tsoul 0:5e5d94ac9c80 19
tsoul 0:5e5d94ac9c80 20 data[0] = goal & 0xff;
tsoul 0:5e5d94ac9c80 21 data[1] = goal >> 8;
tsoul 0:5e5d94ac9c80 22
tsoul 0:5e5d94ac9c80 23 // Send instruction
tsoul 0:5e5d94ac9c80 24 rw(1, mot_id, 0x20, 2, data);
tsoul 0:5e5d94ac9c80 25 }
tsoul 0:5e5d94ac9c80 26
tsoul 0:5e5d94ac9c80 27 // R=0/W=1
tsoul 0:5e5d94ac9c80 28 void MX12::rw(char rw, unsigned char mot_id, char adress, char len, char *data) {
tsoul 0:5e5d94ac9c80 29 // Forge packet
tsoul 0:5e5d94ac9c80 30 char cmd[16];
tsoul 0:5e5d94ac9c80 31
tsoul 0:5e5d94ac9c80 32 cmd[0] = 0xff;
tsoul 0:5e5d94ac9c80 33 cmd[1] = 0xff;
tsoul 0:5e5d94ac9c80 34 cmd[4] = 0x02 + rw;
tsoul 0:5e5d94ac9c80 35
tsoul 0:5e5d94ac9c80 36 cmd[2] = mot_id;
tsoul 0:5e5d94ac9c80 37 cmd[3] = 3 + len;
tsoul 0:5e5d94ac9c80 38 cmd[5] = adress;
tsoul 0:5e5d94ac9c80 39
tsoul 0:5e5d94ac9c80 40 // Compute checksum
tsoul 0:5e5d94ac9c80 41 char cs = rw + mot_id + len + adress + 5;
tsoul 0:5e5d94ac9c80 42
tsoul 0:5e5d94ac9c80 43 for(char i = 0; i < len; i++) {
tsoul 0:5e5d94ac9c80 44 cmd[6 + i] = data[i];
tsoul 0:5e5d94ac9c80 45 cs += data[i];
tsoul 0:5e5d94ac9c80 46 }
tsoul 0:5e5d94ac9c80 47
tsoul 0:5e5d94ac9c80 48 cmd[6 + len] = 0xFF - cs;
tsoul 0:5e5d94ac9c80 49
tsoul 0:5e5d94ac9c80 50 // Send packet
tsoul 0:5e5d94ac9c80 51 if(mot_id != 0xFE) {
tsoul 0:5e5d94ac9c80 52 for(char i = 0; i < (7 + len); i++) {
tsoul 0:5e5d94ac9c80 53 _mx12.write(&cmd[i], 1);
tsoul 0:5e5d94ac9c80 54 }
tsoul 0:5e5d94ac9c80 55 }
tsoul 0:5e5d94ac9c80 56 }