File content as of revision 5:4bdd101ce4ec:

#include "MX12.h"

MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
	// Serial configuration
	_mx12.baud(baud);
	_mx12.format(8, SerialBase::None, 1);
	_mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);

	// Internal defaults
	_sstate = SerialState::Idling;
	_pstate = ParsingState::Header;
}

void MX12::SetSpeed(unsigned char mot_id, float speed) {
	char data[2];

	// Speed absolute value
	int goal = (0x3ff * abs(speed));
 
	// Spin direction (CW is negative)
	if (speed < 0) {
		goal |= (0x1 << 10);
	}

	data[0] = goal & 0xff;
	data[1] = goal >> 8;
	
	// Send instruction
	_sstate = SerialState::Writing;
	rw(mot_id, 0x20, 2, data);
}

char MX12::IsAvailable(void) {
	return (_sstate == SerialState::Idling);
}

void MX12::rw(unsigned char mot_id, char adress, char len, char *data) {
	_answer = 0;
	memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);

	// Forge packet
	char cmd[16];
	
	cmd[0] = 0xff;
	cmd[1] = 0xff;
	cmd[4] = 0x02 + (data != NULL);
	
	cmd[2] = mot_id;
	
	if(data == NULL) cmd[3] = 4;
	else cmd[3] = 3 + len;
	
	cmd[5] = adress;
	
	// Compute checksum
	if(data == NULL) {
		cmd[6] = len;
		cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
		
		// Force length to one to force send
		len = 1;
	} else {
		char cs = mot_id + len + adress + 6;
		
		for(char i = 0; i < len; i++) {
			cmd[6 + i] = data[i];
			cs += data[i];
		}
		
		cmd[6 + len] = 0xFF - cs;
	}
	
	// Send packet
	if(mot_id != 0xFE) {
		for(char i = 0; i < (7 + len); i++) {
			_mx12.write(&cmd[i], 1);
		}
	}
}

void MX12::PrintSerial() {
	for(int i = 0; i < _frame_pointer; i++) {
		printf("%x ", _stored_frame[i]);
	}

	printf("\n");
}

MX12::Status MX12::GetStatus() {
	// Return the corresponding status code
	switch(_current_frame.data[0]) {
		case 0:
			return Ok;
			break;
		case 1 << 0:
			return InputVoltageError;
			break;
		case 1 << 1:
			return AngleLimitError;
			break;
		case 1 << 2:
			return OverheatingError;
			break;
		case 1 << 3:
			return RangeError;
			break;
		case 1 << 4:
			return ChecksumError;
			break;
		case 1 << 5:
			return OverloadError;
			break;
		case 1 << 6:
			return InstructionError;
			break;
		default:
			return Unknown;
	}
}

void MX12::_ReadCallback() {
	char c;
	
	// Try to read serial
	if(_mx12.read(&c, 1)) {
		_stored_frame[_frame_pointer++] = c;
		_scontext.checksum += c;

		// State-machine parsing
		switch(_pstate) {
			case Header:
				if(++(_scontext.headingCount) >= 2) {
					_scontext.headingCount = 0;
					_pstate = Id;
				}

				_scontext.checksum -= c;
				break;
			
			case Id:
				_current_frame.motorId = c;
				_pstate = Length;
				break;

			case Length:
				_current_frame.length = c - 1;
				_pstate = Data;
				break;

			case Data:
				_current_frame.data[_scontext.dataCount] = c;
				
				if(++(_scontext.dataCount) >= _current_frame.length) {
					_scontext.dataCount = 0;
					_pstate = Checksum;
				}
				break;
			
			case Checksum:
				_current_frame.valid = (_scontext.checksum == 0xFF);
				_scontext.checksum = 0;
				_pstate = Header;
				if(_answer) _sstate = Idling;
				_answer = 1;
				break;
		}
	}
}