hidaka sato / JrkG2

Library for JrkG2. this bases on Arduino Library (https://github.com/pololu/jrk-g2-arduino)

JrkG2.cpp@2:e78c0ddcf337, 2018-08-26 (annotated)

Committer:
sgrsn
Date:
Sun Aug 26 08:42:11 2018 +0000
Revision:
2:e78c0ddcf337
Parent:
0:33bfb28b0ffc 
Use AsyncSerial for Serial require.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:33bfb28b0ffc 1 #include <JrkG2.h>
sgrsn 0:33bfb28b0ffc 2
sgrsn 2:e78c0ddcf337 3 #define serial_delay_us 1500 //baud:9600
sgrsn 2:e78c0ddcf337 4
sgrsn 0:33bfb28b0ffc 5 /**** JrkG2Serial ****/
sgrsn 0:33bfb28b0ffc 6
sgrsn 0:33bfb28b0ffc 7 void JrkG2Serial::commandW7(uint8_t cmd, uint8_t val)
sgrsn 0:33bfb28b0ffc 8 {
sgrsn 0:33bfb28b0ffc 9 sendCommandHeader(cmd);
sgrsn 0:33bfb28b0ffc 10 serialW7(val);
sgrsn 0:33bfb28b0ffc 11
sgrsn 0:33bfb28b0ffc 12 _lastError = 0;
sgrsn 0:33bfb28b0ffc 13 }
sgrsn 0:33bfb28b0ffc 14
sgrsn 0:33bfb28b0ffc 15 void JrkG2Serial::commandWs14(uint8_t cmd, int16_t val)
sgrsn 0:33bfb28b0ffc 16 {
sgrsn 0:33bfb28b0ffc 17 uint16_t v = val;
sgrsn 0:33bfb28b0ffc 18 sendCommandHeader(cmd);
sgrsn 0:33bfb28b0ffc 19 serialW7(v); // lower 7 bits
sgrsn 0:33bfb28b0ffc 20 serialW7(v >> 7); // upper 7 bits
sgrsn 0:33bfb28b0ffc 21
sgrsn 0:33bfb28b0ffc 22 _lastError = 0;
sgrsn 0:33bfb28b0ffc 23 }
sgrsn 0:33bfb28b0ffc 24
sgrsn 0:33bfb28b0ffc 25 uint8_t JrkG2Serial::commandR8(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 26 {
sgrsn 0:33bfb28b0ffc 27 uint8_t val;
sgrsn 0:33bfb28b0ffc 28
sgrsn 0:33bfb28b0ffc 29 sendCommandHeader(cmd);
sgrsn 2:e78c0ddcf337 30 wait_us(serial_delay_us);
sgrsn 0:33bfb28b0ffc 31 if(_stream->readable() != 1)
sgrsn 0:33bfb28b0ffc 32 {
sgrsn 2:e78c0ddcf337 33 _stream->abort_read();
sgrsn 0:33bfb28b0ffc 34 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 35 return 0;
sgrsn 0:33bfb28b0ffc 36 }
sgrsn 0:33bfb28b0ffc 37 val = _stream->getc();
sgrsn 0:33bfb28b0ffc 38 _lastError = 0;
sgrsn 0:33bfb28b0ffc 39 return val;
sgrsn 0:33bfb28b0ffc 40 }
sgrsn 0:33bfb28b0ffc 41
sgrsn 0:33bfb28b0ffc 42 uint16_t JrkG2Serial::commandR16(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 43 {
sgrsn 0:33bfb28b0ffc 44 uint8_t buffer[2];
sgrsn 0:33bfb28b0ffc 45
sgrsn 0:33bfb28b0ffc 46 sendCommandHeader(cmd);
sgrsn 0:33bfb28b0ffc 47
sgrsn 2:e78c0ddcf337 48 wait_us(serial_delay_us);
sgrsn 2:e78c0ddcf337 49 if(_stream->readable() != 2)
sgrsn 0:33bfb28b0ffc 50 {
sgrsn 2:e78c0ddcf337 51 _stream->abort_read();
sgrsn 0:33bfb28b0ffc 52 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 53 return 0;
sgrsn 0:33bfb28b0ffc 54 }
sgrsn 0:33bfb28b0ffc 55 for(int i = 0; i < 2; i++)
sgrsn 0:33bfb28b0ffc 56 buffer[i] = _stream->getc();
sgrsn 0:33bfb28b0ffc 57
sgrsn 0:33bfb28b0ffc 58 _lastError = 0;
sgrsn 0:33bfb28b0ffc 59 return ((uint16_t)buffer[0] << 0) | ((uint16_t)buffer[1] << 8);
sgrsn 0:33bfb28b0ffc 60 }
sgrsn 0:33bfb28b0ffc 61
sgrsn 0:33bfb28b0ffc 62 void JrkG2Serial::segmentRead(uint8_t cmd, uint8_t offset,
sgrsn 0:33bfb28b0ffc 63 uint8_t length, uint8_t * buffer)
sgrsn 0:33bfb28b0ffc 64 {
sgrsn 0:33bfb28b0ffc 65 // The Jrk does not allow reads longer than 15 bytes.
sgrsn 0:33bfb28b0ffc 66 if (length > 15) { length = 15; }
sgrsn 0:33bfb28b0ffc 67
sgrsn 0:33bfb28b0ffc 68 sendCommandHeader(cmd);
sgrsn 0:33bfb28b0ffc 69 serialW7(offset);
sgrsn 2:e78c0ddcf337 70 wait_us(serial_delay_us);
sgrsn 0:33bfb28b0ffc 71 serialW7(length);
sgrsn 2:e78c0ddcf337 72 wait_us(serial_delay_us);
sgrsn 2:e78c0ddcf337 73 if(_stream->readable() != length)
sgrsn 0:33bfb28b0ffc 74 {
sgrsn 2:e78c0ddcf337 75 _stream->abort_read();
sgrsn 0:33bfb28b0ffc 76 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 77 // Set the buffer bytes to 0 so the program will not use an uninitialized
sgrsn 0:33bfb28b0ffc 78 // variable.
sgrsn 0:33bfb28b0ffc 79 memset(buffer, 0, length);
sgrsn 0:33bfb28b0ffc 80 return;
sgrsn 0:33bfb28b0ffc 81 }
sgrsn 0:33bfb28b0ffc 82 for(int i = 0; i < length; i++)
sgrsn 0:33bfb28b0ffc 83 buffer[i] = _stream->getc();
sgrsn 0:33bfb28b0ffc 84
sgrsn 0:33bfb28b0ffc 85 _lastError = 0;
sgrsn 0:33bfb28b0ffc 86 }
sgrsn 0:33bfb28b0ffc 87
sgrsn 0:33bfb28b0ffc 88 void JrkG2Serial::segmentWrite(uint8_t cmd, uint8_t offset,
sgrsn 0:33bfb28b0ffc 89 uint8_t length, uint8_t * buffer)
sgrsn 0:33bfb28b0ffc 90 {
sgrsn 0:33bfb28b0ffc 91 // The Jrk does not accept writes longer than 7 bytes over serial.
sgrsn 0:33bfb28b0ffc 92 if (length > 7) { length = 7; }
sgrsn 0:33bfb28b0ffc 93
sgrsn 0:33bfb28b0ffc 94 sendCommandHeader(cmd);
sgrsn 0:33bfb28b0ffc 95 serialW7(offset);
sgrsn 0:33bfb28b0ffc 96 serialW7(length);
sgrsn 0:33bfb28b0ffc 97
sgrsn 0:33bfb28b0ffc 98 // bit i = most-significant bit of buffer[i]
sgrsn 0:33bfb28b0ffc 99 uint8_t msbs = 0;
sgrsn 0:33bfb28b0ffc 100 for (uint8_t i = 0; i < length; i++)
sgrsn 0:33bfb28b0ffc 101 {
sgrsn 0:33bfb28b0ffc 102 serialW7(buffer[i]);
sgrsn 0:33bfb28b0ffc 103 msbs |= (buffer[i] >> 7 & 1) << i;
sgrsn 0:33bfb28b0ffc 104 }
sgrsn 0:33bfb28b0ffc 105 serialW7(msbs);
sgrsn 0:33bfb28b0ffc 106
sgrsn 0:33bfb28b0ffc 107 _lastError = 0;
sgrsn 0:33bfb28b0ffc 108 }
sgrsn 0:33bfb28b0ffc 109
sgrsn 0:33bfb28b0ffc 110 void JrkG2Serial::sendCommandHeader(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 111 {
sgrsn 0:33bfb28b0ffc 112 if (_deviceNumber == 255)
sgrsn 0:33bfb28b0ffc 113 {
sgrsn 0:33bfb28b0ffc 114 // Compact protocol
sgrsn 0:33bfb28b0ffc 115 _stream->putc((uint8_t)cmd);
sgrsn 0:33bfb28b0ffc 116 }
sgrsn 0:33bfb28b0ffc 117 else
sgrsn 0:33bfb28b0ffc 118 {
sgrsn 0:33bfb28b0ffc 119 // Pololu protocol
sgrsn 0:33bfb28b0ffc 120 _stream->putc(0xAA);
sgrsn 0:33bfb28b0ffc 121 serialW7(_deviceNumber);
sgrsn 0:33bfb28b0ffc 122 serialW7((uint8_t)cmd);
sgrsn 0:33bfb28b0ffc 123 }
sgrsn 0:33bfb28b0ffc 124 _lastError = 0;
sgrsn 0:33bfb28b0ffc 125 }
sgrsn 0:33bfb28b0ffc 126
sgrsn 0:33bfb28b0ffc 127 /**** JrkG2I2C ****/
sgrsn 0:33bfb28b0ffc 128
sgrsn 0:33bfb28b0ffc 129 void JrkG2I2C::commandQuick(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 130 {
sgrsn 0:33bfb28b0ffc 131 char data[2] = {cmd, 0};
sgrsn 0:33bfb28b0ffc 132 _lastError = _i2c->write(_address, data, 1);
sgrsn 0:33bfb28b0ffc 133 }
sgrsn 0:33bfb28b0ffc 134
sgrsn 0:33bfb28b0ffc 135 void JrkG2I2C::commandW7(uint8_t cmd, uint8_t val)
sgrsn 0:33bfb28b0ffc 136 {
sgrsn 0:33bfb28b0ffc 137 char data[2] = {cmd, val & 0x7F};
sgrsn 0:33bfb28b0ffc 138 _lastError = _i2c->write(_address, data, 2);
sgrsn 0:33bfb28b0ffc 139 }
sgrsn 0:33bfb28b0ffc 140
sgrsn 0:33bfb28b0ffc 141 void JrkG2I2C::commandWs14(uint8_t cmd, int16_t val)
sgrsn 0:33bfb28b0ffc 142 {
sgrsn 0:33bfb28b0ffc 143 uint16_t v = val;
sgrsn 0:33bfb28b0ffc 144 char data[3] = {cmd, v & 0xFF, v >> 8 & 0xFF};
sgrsn 0:33bfb28b0ffc 145 _lastError = _i2c->write(_address, data, 3);
sgrsn 0:33bfb28b0ffc 146 }
sgrsn 0:33bfb28b0ffc 147
sgrsn 0:33bfb28b0ffc 148 uint8_t JrkG2I2C::commandR8(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 149 {
sgrsn 0:33bfb28b0ffc 150 char w_data[2] = {cmd, 0};
sgrsn 0:33bfb28b0ffc 151 _lastError = _i2c->write(_address, w_data, 1);
sgrsn 0:33bfb28b0ffc 152 if (_lastError != 0) { return 0; }
sgrsn 0:33bfb28b0ffc 153
sgrsn 0:33bfb28b0ffc 154 char r_data;
sgrsn 0:33bfb28b0ffc 155 uint8_t val = _i2c->read(_address, &r_data, 1);
sgrsn 0:33bfb28b0ffc 156 if (val != 0)
sgrsn 0:33bfb28b0ffc 157 {
sgrsn 0:33bfb28b0ffc 158 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 159 return 0;
sgrsn 0:33bfb28b0ffc 160 }
sgrsn 0:33bfb28b0ffc 161 _lastError = 0;
sgrsn 0:33bfb28b0ffc 162 return r_data;
sgrsn 0:33bfb28b0ffc 163 }
sgrsn 0:33bfb28b0ffc 164
sgrsn 0:33bfb28b0ffc 165 uint16_t JrkG2I2C::commandR16(uint8_t cmd)
sgrsn 0:33bfb28b0ffc 166 {
sgrsn 0:33bfb28b0ffc 167 char w_data[2] = {cmd, 0};
sgrsn 0:33bfb28b0ffc 168 _lastError = _i2c->write(_address, w_data, 1, true);
sgrsn 0:33bfb28b0ffc 169 if (_lastError != 0) { return 0; }
sgrsn 0:33bfb28b0ffc 170
sgrsn 0:33bfb28b0ffc 171 char r_data[2] = {};
sgrsn 0:33bfb28b0ffc 172 uint8_t val = _i2c->read(_address, r_data, 2);
sgrsn 0:33bfb28b0ffc 173 if (val != 0)
sgrsn 0:33bfb28b0ffc 174 {
sgrsn 0:33bfb28b0ffc 175 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 176 return 0;
sgrsn 0:33bfb28b0ffc 177 }
sgrsn 0:33bfb28b0ffc 178 _lastError = 0;
sgrsn 0:33bfb28b0ffc 179 uint8_t valL = r_data[0];
sgrsn 0:33bfb28b0ffc 180 uint8_t valH = r_data[1];
sgrsn 0:33bfb28b0ffc 181 return (uint16_t)valL | ((uint16_t)valH << 8);
sgrsn 0:33bfb28b0ffc 182 }
sgrsn 0:33bfb28b0ffc 183
sgrsn 0:33bfb28b0ffc 184 void JrkG2I2C::segmentRead(uint8_t cmd, uint8_t offset,
sgrsn 0:33bfb28b0ffc 185 uint8_t length, uint8_t * buffer)
sgrsn 0:33bfb28b0ffc 186 {
sgrsn 0:33bfb28b0ffc 187 // The Jrk does not allow reads longer than 15 bytes.
sgrsn 0:33bfb28b0ffc 188 if (length > 15) { length = 15; }
sgrsn 0:33bfb28b0ffc 189
sgrsn 0:33bfb28b0ffc 190 char data[2] = {cmd, offset};
sgrsn 0:33bfb28b0ffc 191 _lastError = _i2c->write(_address, data, 2, true);
sgrsn 0:33bfb28b0ffc 192 if (_lastError != 0)
sgrsn 0:33bfb28b0ffc 193 {
sgrsn 0:33bfb28b0ffc 194 memset(buffer, 0, length);
sgrsn 0:33bfb28b0ffc 195 return;
sgrsn 0:33bfb28b0ffc 196 }
sgrsn 0:33bfb28b0ffc 197
sgrsn 0:33bfb28b0ffc 198 uint8_t val = _i2c->read(_address, (char*)buffer, length);
sgrsn 0:33bfb28b0ffc 199 if (val != 0)
sgrsn 0:33bfb28b0ffc 200 {
sgrsn 0:33bfb28b0ffc 201 _lastError = JrkG2CommReadError;
sgrsn 0:33bfb28b0ffc 202 memset(buffer, 0, length);
sgrsn 0:33bfb28b0ffc 203 return;
sgrsn 0:33bfb28b0ffc 204 }
sgrsn 0:33bfb28b0ffc 205 _lastError = 0;
sgrsn 0:33bfb28b0ffc 206 }
sgrsn 0:33bfb28b0ffc 207
sgrsn 0:33bfb28b0ffc 208 void JrkG2I2C::segmentWrite(uint8_t cmd, uint8_t offset,
sgrsn 0:33bfb28b0ffc 209 uint8_t length, uint8_t * buffer)
sgrsn 0:33bfb28b0ffc 210 {
sgrsn 0:33bfb28b0ffc 211 // The Jrk does not accept writes longer than 13 bytes over I2C.
sgrsn 0:33bfb28b0ffc 212 if (length > 13) { length = 13; }
sgrsn 0:33bfb28b0ffc 213
sgrsn 0:33bfb28b0ffc 214 char data[3] = {cmd, offset, length};
sgrsn 0:33bfb28b0ffc 215 _lastError = _i2c->write(_address, data, 3);
sgrsn 0:33bfb28b0ffc 216 _i2c->write(_address, (char*)buffer, length);
sgrsn 0:33bfb28b0ffc 217 }