MX12.cpp

00001 #include "MX12.h"
00002 
00003 MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
00004     // Serial configuration
00005     _mx12.baud(baud);
00006     _mx12.format(8, SerialBase::None, 1);
00007     _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
00008 
00009     // Internal defaults
00010     _sstate = SerialState::Idling;
00011     _pstate = ParsingState::Header;
00012 }
00013 
00014 void MX12::SetSpeed(unsigned char mot_id, float speed) {
00015     char data[2];
00016 
00017     // Speed absolute value
00018     int goal = (0x3ff * abs(speed));
00019  
00020     // Spin direction (CW is negative)
00021     if (speed < 0) {
00022         goal |= (0x1 << 10);
00023     }
00024 
00025     data[0] = goal & 0xff;
00026     data[1] = goal >> 8;
00027     
00028     // Send instruction
00029     _sstate = SerialState::Writing;
00030     rw(mot_id, 0x20, 2, data);
00031 }
00032 
00033 char MX12::IsAvailable(void) {
00034     return (_sstate == SerialState::Idling);
00035 }
00036 
00037 void MX12::rw(unsigned char mot_id, char adress, char len, char *data) {
00038     _answer = 0;
00039     memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
00040 
00041     // Forge packet
00042     char cmd[16];
00043     
00044     cmd[0] = 0xff;
00045     cmd[1] = 0xff;
00046     cmd[4] = 0x02 + (data != NULL);
00047     
00048     cmd[2] = mot_id;
00049     
00050     if(data == NULL) cmd[3] = 4;
00051     else cmd[3] = 3 + len;
00052     
00053     cmd[5] = adress;
00054     
00055     // Compute checksum
00056     if(data == NULL) {
00057         cmd[6] = len;
00058         cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
00059         
00060         // Force length to one to force send
00061         len = 1;
00062     } else {
00063         char cs = mot_id + len + adress + 6;
00064         
00065         for(char i = 0; i < len; i++) {
00066             cmd[6 + i] = data[i];
00067             cs += data[i];
00068         }
00069         
00070         cmd[6 + len] = 0xFF - cs;
00071     }
00072     
00073     // Send packet
00074     if(mot_id != 0xFE) {
00075         for(char i = 0; i < (7 + len); i++) {
00076             _mx12.write(&cmd[i], 1);
00077         }
00078     }
00079 }
00080 
00081 void MX12::PrintSerial() {
00082     for(int i = 0; i < _frame_pointer; i++) {
00083         printf("%x ", _stored_frame[i]);
00084     }
00085 
00086     printf("\n");
00087 }
00088 
00089 MX12::Status MX12::GetStatus() {
00090     // Return the corresponding status code
00091     switch(_current_frame.data[0]) {
00092         case 0:
00093             return Ok;
00094             break;
00095         case 1 << 0:
00096             return InputVoltageError;
00097             break;
00098         case 1 << 1:
00099             return AngleLimitError;
00100             break;
00101         case 1 << 2:
00102             return OverheatingError;
00103             break;
00104         case 1 << 3:
00105             return RangeError;
00106             break;
00107         case 1 << 4:
00108             return ChecksumError;
00109             break;
00110         case 1 << 5:
00111             return OverloadError;
00112             break;
00113         case 1 << 6:
00114             return InstructionError;
00115             break;
00116         default:
00117             return Unknown;
00118     }
00119 }
00120 
00121 void MX12::_ReadCallback() {
00122     char c;
00123     
00124     // Try to read serial
00125     if(_mx12.read(&c, 1)) {
00126         _stored_frame[_frame_pointer++] = c;
00127         _scontext.checksum += c;
00128 
00129         // State-machine parsing
00130         switch(_pstate) {
00131             case Header:
00132                 if(++(_scontext.headingCount) >= 2) {
00133                     _scontext.headingCount = 0;
00134                     _pstate = Id;
00135                 }
00136 
00137                 _scontext.checksum -= c;
00138                 break;
00139             
00140             case Id:
00141                 _current_frame.motorId = c;
00142                 _pstate = Length;
00143                 break;
00144 
00145             case Length:
00146                 _current_frame.length = c - 1;
00147                 _pstate = Data;
00148                 break;
00149 
00150             case Data:
00151                 _current_frame.data[_scontext.dataCount] = c;
00152                 
00153                 if(++(_scontext.dataCount) >= _current_frame.length) {
00154                     _scontext.dataCount = 0;
00155                     _pstate = Checksum;
00156                 }
00157                 break;
00158             
00159             case Checksum:
00160                 _current_frame.valid = (_scontext.checksum == 0xFF);
00161                 _scontext.checksum = 0;
00162                 _pstate = Header;
00163                 if(_answer) _sstate = Idling;
00164                 _answer = 1;
00165                 break;
00166         }
00167     }
00168 }