JrkG2.cpp

00001 #include <JrkG2.h>
00002 
00003 #define serial_delay_us 1500    //baud:9600
00004 
00005 /**** JrkG2Serial ****/
00006 
00007 void JrkG2Serial::commandW7(uint8_t cmd, uint8_t val)
00008 {
00009     sendCommandHeader(cmd);
00010     serialW7(val);
00011     
00012     _lastError = 0;
00013 }
00014 
00015 void JrkG2Serial::commandWs14(uint8_t cmd, int16_t val)
00016 {
00017     uint16_t v = val;
00018     sendCommandHeader(cmd);
00019     serialW7(v);  // lower 7 bits
00020     serialW7(v >> 7);  // upper 7 bits
00021     
00022     _lastError = 0;
00023 }
00024 
00025 uint8_t JrkG2Serial::commandR8(uint8_t cmd)
00026 {
00027     uint8_t val;
00028     
00029     sendCommandHeader(cmd);
00030     wait_us(serial_delay_us);
00031     if(_stream->readable() != 1)
00032     {
00033         _stream->abort_read();
00034         _lastError = JrkG2CommReadError;
00035         return 0;
00036     }
00037     val = _stream->getc();
00038     _lastError = 0;
00039     return val;
00040 }
00041 
00042 uint16_t JrkG2Serial::commandR16(uint8_t cmd)
00043 {
00044     uint8_t buffer[2];
00045     
00046     sendCommandHeader(cmd);
00047     
00048     wait_us(serial_delay_us);
00049     if(_stream->readable() != 2)
00050     {
00051         _stream->abort_read();
00052         _lastError = JrkG2CommReadError;
00053         return 0;
00054     }
00055     for(int i = 0; i < 2; i++)
00056         buffer[i] = _stream->getc();
00057     
00058     _lastError = 0;
00059     return ((uint16_t)buffer[0] << 0) | ((uint16_t)buffer[1] << 8);
00060 }
00061 
00062 void JrkG2Serial::segmentRead(uint8_t cmd, uint8_t offset,
00063     uint8_t length, uint8_t * buffer)
00064 {
00065     // The Jrk does not allow reads longer than 15 bytes.
00066     if (length > 15) { length = 15; }
00067     
00068     sendCommandHeader(cmd);
00069     serialW7(offset);
00070     wait_us(serial_delay_us);
00071     serialW7(length);
00072     wait_us(serial_delay_us);
00073     if(_stream->readable() != length)
00074     {
00075         _stream->abort_read();
00076         _lastError = JrkG2CommReadError;
00077         // Set the buffer bytes to 0 so the program will not use an uninitialized
00078         // variable.
00079         memset(buffer, 0, length);
00080         return;
00081     }
00082     for(int i = 0; i < length; i++)
00083         buffer[i] = _stream->getc();
00084     
00085     _lastError = 0;
00086 }
00087 
00088 void JrkG2Serial::segmentWrite(uint8_t cmd, uint8_t offset,
00089     uint8_t length, uint8_t * buffer)
00090 {
00091     // The Jrk does not accept writes longer than 7 bytes over serial.
00092     if (length > 7) { length = 7; }
00093     
00094     sendCommandHeader(cmd);
00095     serialW7(offset);
00096     serialW7(length);
00097     
00098     // bit i = most-significant bit of buffer[i]
00099     uint8_t msbs = 0;
00100     for (uint8_t i = 0; i < length; i++)
00101     {
00102         serialW7(buffer[i]);
00103         msbs |= (buffer[i] >> 7 & 1) << i;
00104     }
00105     serialW7(msbs);
00106     
00107     _lastError = 0;
00108 }
00109 
00110 void JrkG2Serial::sendCommandHeader(uint8_t cmd)
00111 {
00112     if (_deviceNumber == 255)
00113     {
00114         // Compact protocol
00115         _stream->putc((uint8_t)cmd);
00116     }
00117     else
00118     {
00119         // Pololu protocol
00120         _stream->putc(0xAA);
00121         serialW7(_deviceNumber);
00122         serialW7((uint8_t)cmd);
00123     }
00124     _lastError = 0;
00125 }
00126 
00127 /**** JrkG2I2C ****/
00128 
00129 void JrkG2I2C::commandQuick(uint8_t cmd)
00130 {
00131     char data[2] = {cmd, 0};
00132     _lastError = _i2c->write(_address, data, 1);
00133 }
00134 
00135 void JrkG2I2C::commandW7(uint8_t cmd, uint8_t val)
00136 {
00137     char data[2] = {cmd, val & 0x7F};
00138     _lastError = _i2c->write(_address, data, 2);
00139 }
00140 
00141 void JrkG2I2C::commandWs14(uint8_t cmd, int16_t val)
00142 {
00143     uint16_t v = val;
00144     char data[3] = {cmd, v & 0xFF, v >> 8 & 0xFF};
00145     _lastError = _i2c->write(_address, data, 3);
00146 }
00147 
00148 uint8_t JrkG2I2C::commandR8(uint8_t cmd)
00149 {
00150     char w_data[2] = {cmd, 0};
00151     _lastError = _i2c->write(_address, w_data, 1);
00152     if (_lastError != 0) { return 0; }
00153     
00154     char r_data;
00155     uint8_t val = _i2c->read(_address, &r_data, 1);
00156     if (val != 0)
00157     {
00158         _lastError = JrkG2CommReadError;
00159         return 0;
00160     } 
00161     _lastError = 0;
00162     return r_data;
00163 }
00164 
00165 uint16_t JrkG2I2C::commandR16(uint8_t cmd)
00166 {
00167     char w_data[2] = {cmd, 0};
00168     _lastError = _i2c->write(_address, w_data, 1, true);
00169     if (_lastError != 0) { return 0; }
00170     
00171     char r_data[2] = {};
00172     uint8_t val = _i2c->read(_address, r_data, 2);
00173     if (val != 0)
00174     {
00175         _lastError = JrkG2CommReadError;
00176         return 0;
00177     } 
00178     _lastError = 0;
00179     uint8_t valL = r_data[0];
00180     uint8_t valH = r_data[1];
00181     return (uint16_t)valL | ((uint16_t)valH << 8);
00182 }
00183 
00184 void JrkG2I2C::segmentRead(uint8_t cmd, uint8_t offset,
00185         uint8_t length, uint8_t * buffer)
00186 {
00187     // The Jrk does not allow reads longer than 15 bytes.
00188     if (length > 15) { length = 15; }
00189     
00190     char data[2] = {cmd, offset};
00191     _lastError = _i2c->write(_address, data, 2, true);
00192     if (_lastError != 0)
00193     {
00194         memset(buffer, 0, length);
00195         return; 
00196     }
00197     
00198     uint8_t val = _i2c->read(_address, (char*)buffer, length);
00199     if (val != 0)
00200     {
00201         _lastError = JrkG2CommReadError;
00202         memset(buffer, 0, length);
00203         return;
00204     }
00205     _lastError = 0;
00206 }
00207 
00208 void JrkG2I2C::segmentWrite(uint8_t cmd, uint8_t offset,
00209         uint8_t length, uint8_t * buffer)
00210 {
00211     // The Jrk does not accept writes longer than 13 bytes over I2C.
00212     if (length > 13) { length = 13; }
00213     
00214     char data[3] = {cmd, offset, length};
00215     _lastError = _i2c->write(_address, data, 3);
00216     _i2c->write(_address, (char*)buffer, length);
00217 }