hidaka sato / JrkG2

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

JrkG2.cpp@0:33bfb28b0ffc, 2018-08-25 (annotated)

Committer:
sgrsn
Date:
Sat Aug 25 13:00:25 2018 +0000
Revision:
0:33bfb28b0ffc
Child:
2:e78c0ddcf337
first commit

Who changed what in which revision?

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