Li Weiyi
/
Cube
Microduino的cube小车。
Microduino_Protocol_HardSer.cpp@3:e4ac7c1a14de, 2016-05-27 (annotated)
- Committer:
- lixianyu
- Date:
- Fri May 27 01:44:31 2016 +0000
- Revision:
- 3:e4ac7c1a14de
- Child:
- 4:0670023d3f36
???????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lixianyu | 3:e4ac7c1a14de | 1 | #include "Microduino_Protocol_HardSer.h" |
lixianyu | 3:e4ac7c1a14de | 2 | |
lixianyu | 3:e4ac7c1a14de | 3 | uint8_t getChecksum(uint8_t length, uint8_t cmd, uint8_t mydata[]) |
lixianyu | 3:e4ac7c1a14de | 4 | { |
lixianyu | 3:e4ac7c1a14de | 5 | //三个参数分别为: 数据长度 , 指令代码 , 实际数据数组 |
lixianyu | 3:e4ac7c1a14de | 6 | uint8_t checksum = 0; |
lixianyu | 3:e4ac7c1a14de | 7 | checksum ^= (length & 0xFF); |
lixianyu | 3:e4ac7c1a14de | 8 | checksum ^= (cmd & 0xFF); |
lixianyu | 3:e4ac7c1a14de | 9 | for (int i = 0; i < length; i++) checksum ^= (mydata[i] & 0xFF); |
lixianyu | 3:e4ac7c1a14de | 10 | return checksum; |
lixianyu | 3:e4ac7c1a14de | 11 | } |
lixianyu | 3:e4ac7c1a14de | 12 | |
lixianyu | 3:e4ac7c1a14de | 13 | |
lixianyu | 3:e4ac7c1a14de | 14 | /* Protocol::Protocol(PRO_PORT *ser , byte _channel) { |
lixianyu | 3:e4ac7c1a14de | 15 | // common_init(); // Set everything to common state, then... |
lixianyu | 3:e4ac7c1a14de | 16 | this->channel = _channel; |
lixianyu | 3:e4ac7c1a14de | 17 | this->num = 0; |
lixianyu | 3:e4ac7c1a14de | 18 | this->sta = false; |
lixianyu | 3:e4ac7c1a14de | 19 | this->error = false; |
lixianyu | 3:e4ac7c1a14de | 20 | P_Serial = ser; // ...override P_Serial with value passed. |
lixianyu | 3:e4ac7c1a14de | 21 | } */ |
lixianyu | 3:e4ac7c1a14de | 22 | |
lixianyu | 3:e4ac7c1a14de | 23 | |
lixianyu | 3:e4ac7c1a14de | 24 | void Protocol::begin(uint16_t _baud) |
lixianyu | 3:e4ac7c1a14de | 25 | { |
lixianyu | 3:e4ac7c1a14de | 26 | //P_Serial->begin(_baud); |
lixianyu | 3:e4ac7c1a14de | 27 | P_Serial->baud(_baud); |
lixianyu | 3:e4ac7c1a14de | 28 | //delay(20); |
lixianyu | 3:e4ac7c1a14de | 29 | wait_ms(20); |
lixianyu | 3:e4ac7c1a14de | 30 | } |
lixianyu | 3:e4ac7c1a14de | 31 | |
lixianyu | 3:e4ac7c1a14de | 32 | |
lixianyu | 3:e4ac7c1a14de | 33 | bool Protocol::available(bool _sta) |
lixianyu | 3:e4ac7c1a14de | 34 | { |
lixianyu | 3:e4ac7c1a14de | 35 | //if (P_Serial->available() > 0) { |
lixianyu | 3:e4ac7c1a14de | 36 | if (P_Serial->readable() > 0) { |
lixianyu | 3:e4ac7c1a14de | 37 | if (_sta) { |
lixianyu | 3:e4ac7c1a14de | 38 | this->inCache = this->inChar; |
lixianyu | 3:e4ac7c1a14de | 39 | //this->inChar = P_Serial->read(); |
lixianyu | 3:e4ac7c1a14de | 40 | this->inChar = P_Serial->getc(); |
lixianyu | 3:e4ac7c1a14de | 41 | this->buffer[num] = this->inChar; |
lixianyu | 3:e4ac7c1a14de | 42 | |
lixianyu | 3:e4ac7c1a14de | 43 | if (this->num > BUFFER_MAX - 1) { |
lixianyu | 3:e4ac7c1a14de | 44 | this->num = 0; |
lixianyu | 3:e4ac7c1a14de | 45 | return false; |
lixianyu | 3:e4ac7c1a14de | 46 | } else { |
lixianyu | 3:e4ac7c1a14de | 47 | this->num++; |
lixianyu | 3:e4ac7c1a14de | 48 | } |
lixianyu | 3:e4ac7c1a14de | 49 | } |
lixianyu | 3:e4ac7c1a14de | 50 | return true; |
lixianyu | 3:e4ac7c1a14de | 51 | } |
lixianyu | 3:e4ac7c1a14de | 52 | return false; |
lixianyu | 3:e4ac7c1a14de | 53 | } |
lixianyu | 3:e4ac7c1a14de | 54 | |
lixianyu | 3:e4ac7c1a14de | 55 | |
lixianyu | 3:e4ac7c1a14de | 56 | uint8_t Protocol::parse(uint16_t* _data, bool _mod) |
lixianyu | 3:e4ac7c1a14de | 57 | { |
lixianyu | 3:e4ac7c1a14de | 58 | if (available(!_mod)) { |
lixianyu | 3:e4ac7c1a14de | 59 | //time = millis(); |
lixianyu | 3:e4ac7c1a14de | 60 | time = _timer.read_ms(); |
lixianyu | 3:e4ac7c1a14de | 61 | do { |
lixianyu | 3:e4ac7c1a14de | 62 | if (this->sta) { |
lixianyu | 3:e4ac7c1a14de | 63 | this->sta = false; |
lixianyu | 3:e4ac7c1a14de | 64 | this->num = 0; |
lixianyu | 3:e4ac7c1a14de | 65 | if (this->inChar == this->channel) { |
lixianyu | 3:e4ac7c1a14de | 66 | this->error = false; |
lixianyu | 3:e4ac7c1a14de | 67 | if (!_mod) { |
lixianyu | 3:e4ac7c1a14de | 68 | return P_BUSY; |
lixianyu | 3:e4ac7c1a14de | 69 | } |
lixianyu | 3:e4ac7c1a14de | 70 | } else { |
lixianyu | 3:e4ac7c1a14de | 71 | this->error = true; |
lixianyu | 3:e4ac7c1a14de | 72 | return P_ERROR; |
lixianyu | 3:e4ac7c1a14de | 73 | } |
lixianyu | 3:e4ac7c1a14de | 74 | } |
lixianyu | 3:e4ac7c1a14de | 75 | |
lixianyu | 3:e4ac7c1a14de | 76 | if (this->inChar == 0xBB && this->inCache == 0xAA) { |
lixianyu | 3:e4ac7c1a14de | 77 | this->sta = true; |
lixianyu | 3:e4ac7c1a14de | 78 | if (!_mod) { |
lixianyu | 3:e4ac7c1a14de | 79 | return P_BUSY; |
lixianyu | 3:e4ac7c1a14de | 80 | } |
lixianyu | 3:e4ac7c1a14de | 81 | } |
lixianyu | 3:e4ac7c1a14de | 82 | |
lixianyu | 3:e4ac7c1a14de | 83 | if (this->num == (CHANNEL_NUM * 2 + 1) && !this->error) { |
lixianyu | 3:e4ac7c1a14de | 84 | this->inCache = this->buffer[CHANNEL_NUM * 2]; |
lixianyu | 3:e4ac7c1a14de | 85 | this->buffer[CHANNEL_NUM * 2] = NULL; |
lixianyu | 3:e4ac7c1a14de | 86 | this->inChar = getChecksum(CHANNEL_NUM * 2, 200, this->buffer); |
lixianyu | 3:e4ac7c1a14de | 87 | |
lixianyu | 3:e4ac7c1a14de | 88 | this->num = 0; |
lixianyu | 3:e4ac7c1a14de | 89 | if (!this->error && this->inCache == this->inChar) { |
lixianyu | 3:e4ac7c1a14de | 90 | for (uint8_t a = 0; a < CHANNEL_NUM; a++) { |
lixianyu | 3:e4ac7c1a14de | 91 | _data[a] = ((uint16_t)(this->buffer[a * 2])) | ((uint16_t)this->buffer[a * 2 + 1] << 8); |
lixianyu | 3:e4ac7c1a14de | 92 | } |
lixianyu | 3:e4ac7c1a14de | 93 | return P_FINE; |
lixianyu | 3:e4ac7c1a14de | 94 | } else { |
lixianyu | 3:e4ac7c1a14de | 95 | return P_ERROR; |
lixianyu | 3:e4ac7c1a14de | 96 | } |
lixianyu | 3:e4ac7c1a14de | 97 | } else if (!_mod) { |
lixianyu | 3:e4ac7c1a14de | 98 | return P_BUSY; |
lixianyu | 3:e4ac7c1a14de | 99 | } |
lixianyu | 3:e4ac7c1a14de | 100 | } while (_mod && (available(true) && _timer.read_ms() - time < 100)); |
lixianyu | 3:e4ac7c1a14de | 101 | |
lixianyu | 3:e4ac7c1a14de | 102 | if (_mod) { |
lixianyu | 3:e4ac7c1a14de | 103 | return P_TIMEOUT; |
lixianyu | 3:e4ac7c1a14de | 104 | } |
lixianyu | 3:e4ac7c1a14de | 105 | } else { |
lixianyu | 3:e4ac7c1a14de | 106 | return P_NONE; |
lixianyu | 3:e4ac7c1a14de | 107 | } |
lixianyu | 3:e4ac7c1a14de | 108 | } |